open_source / modbus

Commit 5735b406dd98273d4503c336ebc19559730e09b0

Authored by Peter M. Groen
1 parent 783ce3c5

Revert "Setting up working version" 

This reverts commit 783ce3c583f4d6094ed7e9a68c41534fc2bceffd.
Inline Side-by-side
Showing 29 changed files with 6101 additions and 648 deletions
3rdparty/libmodbus/config.h
  View file @5735b40
  1 +/* config.h. Generated from config.h.in by configure. */
  2 +/* config.h.in. Generated from configure.ac by autoheader. */
  3 +
1 4 /* Define to 1 if you have the <arpa/inet.h> header file. */
2 5 #define HAVE_ARPA_INET_H 1
3 6  
4   -/* Define to 1 if you have the declaration of `TIOCSRS485', and to 0 if you don't. */
  7 +/* Define to 1 if you have the declaration of `TIOCSRS485', and to 0 if you
  8 + don't. */
5 9 #define HAVE_DECL_TIOCSRS485 0
6 10  
7   -/* Define to 1 if you have the declaration of `TIOCM_RTS', and to 0 if you don't. */
  11 +/* Define to 1 if you have the declaration of `TIOCM_RTS', and to 0 if you
  12 + don't. */
8 13 #define HAVE_DECL_TIOCM_RTS 0
9 14  
10   -/* Define to 1 if you have the declaration of `__CYGWIN__', and to 0 if you don't. */
  15 +/* Define to 1 if you have the declaration of `__CYGWIN__', and to 0 if you
  16 + don't. */
11 17 #define HAVE_DECL___CYGWIN__ 0
12 18  
13 19 /* Define to 1 if you have the <dlfcn.h> header file. */
... ... @@ -135,13 +141,13 @@
135 141 #define PACKAGE_STRING "libmodbus 3.1.0-1"
136 142  
137 143 /* Define to the one symbol short name of this package. */
138   -#define PACKAGE_TARNAME "modbus"
  144 +#define PACKAGE_TARNAME "libmodbus"
139 145  
140 146 /* Define to the home page for this package. */
141 147 #define PACKAGE_URL ""
142 148  
143 149 /* Define to the version of this package. */
144   -#define PACKAGE_VERSION "1.0.0"
  150 +#define PACKAGE_VERSION "3.1.0-1"
145 151  
146 152 /* Define to 1 if you have the ANSI C header files. */
147 153 #define STDC_HEADERS 1
... ... @@ -150,7 +156,7 @@
150 156 #define TIME_WITH_SYS_TIME 1
151 157  
152 158 /* Version number of package */
153   -#define VERSION "1.0.0"
  159 +#define VERSION "3.1.0-1"
154 160  
155 161 /* Define to empty if `const' does not conform to ANSI C. */
156 162 /* #undef const */
... ...
3rdparty/libmodbus/modbus-data.c
  View file @5735b40
  1 +/*
  2 + * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
1 6  
2 7 #include <stdlib.h>
3 8  
4   -#include "stdint.h"
  9 +#ifndef _MSC_VER
  10 +# include <stdint.h>
  11 +#else
  12 +# include "stdint.h"
  13 +#endif
5 14  
6 15 #include <string.h>
7 16 #include <assert.h>
8   -#include <arpa/inet.h>
  17 +
  18 +#if defined(_WIN32)
  19 +# include <winsock2.h>
  20 +#else
  21 +# include <arpa/inet.h>
  22 +#endif
  23 +
9 24 #include <config.h>
10 25  
11 26 #include "modbus.h"
... ... @@ -14,6 +29,13 @@
14 29 # include <byteswap.h>
15 30 #endif
16 31  
  32 +#if defined(__APPLE__)
  33 +# include <libkern/OSByteOrder.h>
  34 +# define bswap_16 OSSwapInt16
  35 +# define bswap_32 OSSwapInt32
  36 +# define bswap_64 OSSwapInt64
  37 +#endif
  38 +
17 39 #if defined(__GNUC__)
18 40 # define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
19 41 # if GCC_VERSION >= 430
... ... @@ -23,8 +45,13 @@
23 45 # endif
24 46 #endif
25 47  
  48 +#if defined(_MSC_VER) && (_MSC_VER >= 1400)
  49 +# define bswap_32 _byteswap_ulong
  50 +# define bswap_16 _byteswap_ushort
  51 +#endif
  52 +
26 53 #if !defined(__CYGWIN__) && !defined(bswap_16)
27   -#pragma message "Fallback on C functions for bswap_16"
  54 +# warning "Fallback on C functions for bswap_16"
28 55 static inline uint16_t bswap_16(uint16_t x)
29 56 {
30 57 return (x >> 8) | (x << 8);
... ... @@ -32,7 +59,7 @@ static inline uint16_t bswap_16(uint16_t x)
32 59 #endif
33 60  
34 61 #if !defined(bswap_32)
35   -#pragma message "Fallback on C functions for bswap_32"
  62 +# warning "Fallback on C functions for bswap_32"
36 63 static inline uint32_t bswap_32(uint32_t x)
37 64 {
38 65 return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
... ... @@ -41,25 +68,24 @@ static inline uint32_t bswap_32(uint32_t x)
41 68  
42 69 /* Sets many bits from a single byte value (all 8 bits of the byte value are
43 70 set) */
44   -void modbus_set_bits_from_byte( uint8_t *dest, int idx, const uint8_t value )
  71 +void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
45 72 {
46 73 int i;
47 74  
48   - for (i=0; i < 8; i++)
49   - {
  75 + for (i=0; i < 8; i++) {
50 76 dest[idx+i] = (value & (1 << i)) ? 1 : 0;
51 77 }
52 78 }
53 79  
54 80 /* Sets many bits from a table of bytes (only the bits between idx and
55 81 idx + nb_bits are set) */
56   -void modbus_set_bits_from_bytes( uint8_t *dest, int idx, unsigned int nb_bits, const uint8_t *tab_byte )
  82 +void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
  83 + const uint8_t *tab_byte)
57 84 {
58 85 unsigned int i;
59 86 int shift = 0;
60 87  
61   - for ( i = idx; i < idx + nb_bits; i++ )
62   - {
  88 + for (i = idx; i < idx + nb_bits; i++) {
63 89 dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
64 90 /* gcc doesn't like: shift = (++shift) % 8; */
65 91 shift++;
... ... @@ -69,20 +95,19 @@ void modbus_set_bits_from_bytes( uint8_t *dest, int idx, unsigned int nb_bits, c
69 95  
70 96 /* Gets the byte value from many bits.
71 97 To obtain a full byte, set nb_bits to 8. */
72   -uint8_t modbus_get_byte_from_bits( const uint8_t *src, int idx, unsigned int nb_bits )
  98 +uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx,
  99 + unsigned int nb_bits)
73 100 {
74 101 unsigned int i;
75 102 uint8_t value = 0;
76 103  
77   - if (nb_bits > 8)
78   - {
  104 + if (nb_bits > 8) {
79 105 /* Assert is ignored if NDEBUG is set */
80 106 assert(nb_bits < 8);
81 107 nb_bits = 8;
82 108 }
83 109  
84   - for (i=0; i < nb_bits; i++)
85   - {
  110 + for (i=0; i < nb_bits; i++) {
86 111 value |= (src[idx+i] << i);
87 112 }
88 113  
... ...
3rdparty/libmodbus/modbus-private.h
  View file @5735b40
  1 +/*
  2 + * Copyright © 2010-2012 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
1 6  
2   -#pragma once
  7 +#ifndef MODBUS_PRIVATE_H
  8 +#define MODBUS_PRIVATE_H
3 9  
  10 +#ifndef _MSC_VER
4 11 # include <stdint.h>
5 12 # include <sys/time.h>
  13 +#else
  14 +# include "stdint.h"
  15 +# include <time.h>
  16 +typedef int ssize_t;
  17 +#endif
6 18 #include <sys/types.h>
7 19 #include <config.h>
8 20  
... ... @@ -18,15 +30,16 @@ MODBUS_BEGIN_DECLS
18 30 * - HEADER_LENGTH_RTU (1) + function (1) + address (2) + number (2) + CRC (2)
19 31 */
20 32 #define _MIN_REQ_LENGTH 12
  33 +
21 34 #define _REPORT_SLAVE_ID 180
  35 +
22 36 #define _MODBUS_EXCEPTION_RSP_LENGTH 5
23 37  
24 38 /* Timeouts in microsecond (0.5 s) */
25 39 #define _RESPONSE_TIMEOUT 500000
26 40 #define _BYTE_TIMEOUT 500000
27 41  
28   -typedef enum
29   -{
  42 +typedef enum {
30 43 _MODBUS_BACKEND_TYPE_RTU=0,
31 44 _MODBUS_BACKEND_TYPE_TCP
32 45 } modbus_backend_type_t;
... ... @@ -36,8 +49,7 @@ typedef enum
36 49 * | Client | ---------------------->| Server |
37 50 * ---------- Confirmation Response ----------
38 51 */
39   -typedef enum
40   -{
  52 +typedef enum {
41 53 /* Request message on the server side */
42 54 MSG_INDICATION,
43 55 /* Request message on the client side */
... ... @@ -46,29 +58,30 @@ typedef enum
46 58  
47 59 /* This structure reduces the number of params in functions and so
48 60 * optimizes the speed of execution (~ 37%). */
49   -typedef struct _sft
50   -{
  61 +typedef struct _sft {
51 62 int slave;
52 63 int function;
53 64 int t_id;
54 65 } sft_t;
55 66  
56   -typedef struct _modbus_backend
57   -{
  67 +typedef struct _modbus_backend {
58 68 unsigned int backend_type;
59 69 unsigned int header_length;
60 70 unsigned int checksum_length;
61 71 unsigned int max_adu_length;
62 72 int (*set_slave) (modbus_t *ctx, int slave);
63   - int (*build_request_basis) (modbus_t *ctx, int function, int addr, int nb, uint8_t *req);
  73 + int (*build_request_basis) (modbus_t *ctx, int function, int addr,
  74 + int nb, uint8_t *req);
64 75 int (*build_response_basis) (sft_t *sft, uint8_t *rsp);
65 76 int (*prepare_response_tid) (const uint8_t *req, int *req_length);
66 77 int (*send_msg_pre) (uint8_t *req, int req_length);
67 78 ssize_t (*send) (modbus_t *ctx, const uint8_t *req, int req_length);
68 79 int (*receive) (modbus_t *ctx, uint8_t *req);
69 80 ssize_t (*recv) (modbus_t *ctx, uint8_t *rsp, int rsp_length);
70   - int (*check_integrity) (modbus_t *ctx, uint8_t *msg, const int msg_length);
71   - int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length);
  81 + int (*check_integrity) (modbus_t *ctx, uint8_t *msg,
  82 + const int msg_length);
  83 + int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req,
  84 + const uint8_t *rsp, int rsp_length);
72 85 int (*connect) (modbus_t *ctx);
73 86 void (*close) (modbus_t *ctx);
74 87 int (*flush) (modbus_t *ctx);
... ... @@ -76,8 +89,7 @@ typedef struct _modbus_backend
76 89 void (*free) (modbus_t *ctx);
77 90 } modbus_backend_t;
78 91  
79   -struct _modbus
80   -{
  92 +struct _modbus {
81 93 /* Slave address */
82 94 int slave;
83 95 /* Socket or file descriptor */
... ... @@ -99,3 +111,5 @@ size_t strlcpy(char *dest, const char *src, size_t dest_size);
99 111 #endif
100 112  
101 113 MODBUS_END_DECLS
  114 +
  115 +#endif /* MODBUS_PRIVATE_H */
... ...
3rdparty/libmodbus/modbus-rtu-private.h
  View file @5735b40
  1 +/*
  2 + * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
1 6  
2   -#pragma once
  7 +#ifndef MODBUS_RTU_PRIVATE_H
  8 +#define MODBUS_RTU_PRIVATE_H
3 9  
  10 +#ifndef _MSC_VER
4 11 #include <stdint.h>
  12 +#else
  13 +#include "stdint.h"
  14 +#endif
  15 +
  16 +#if defined(_WIN32)
  17 +#include <windows.h>
  18 +#else
5 19 #include <termios.h>
  20 +#endif
6 21  
7 22 #define _MODBUS_RTU_HEADER_LENGTH 1
8 23 #define _MODBUS_RTU_PRESET_REQ_LENGTH 6
... ... @@ -10,8 +25,24 @@
10 25  
11 26 #define _MODBUS_RTU_CHECKSUM_LENGTH 2
12 27  
13   -typedef struct _modbus_rtu
14   -{
  28 +#if defined(_WIN32)
  29 +#if !defined(ENOTSUP)
  30 +#define ENOTSUP WSAEOPNOTSUPP
  31 +#endif
  32 +
  33 +/* WIN32: struct containing serial handle and a receive buffer */
  34 +#define PY_BUF_SIZE 512
  35 +struct win32_ser {
  36 + /* File handle */
  37 + HANDLE fd;
  38 + /* Receive buffer */
  39 + uint8_t buf[PY_BUF_SIZE];
  40 + /* Received chars */
  41 + DWORD n_bytes;
  42 +};
  43 +#endif /* _WIN32 */
  44 +
  45 +typedef struct _modbus_rtu {
15 46 /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
16 47 char *device;
17 48 /* Bauds: 9600, 19200, 57600, 115200, etc */
... ... @@ -22,13 +53,25 @@ typedef struct _modbus_rtu
22 53 uint8_t stop_bit;
23 54 /* Parity: 'N', 'O', 'E' */
24 55 char parity;
  56 +#if defined(_WIN32)
  57 + struct win32_ser w_ser;
  58 + DCB old_dcb;
  59 +#else
25 60 /* Save old termios settings */
26 61 struct termios old_tios;
27   -
  62 +#endif
28 63 #if HAVE_DECL_TIOCSRS485
29 64 int serial_mode;
30 65 #endif
31   -
  66 +//***Not part of libmodbus - added for QModMaster***//
  67 +#if defined(_WIN32) || HAVE_DECL_TIOCM_RTS
  68 + int rts;
  69 + int rts_delay;
  70 + int onebyte_time;
  71 + void (*set_rts) (modbus_t *ctx, int on);
  72 +#endif
32 73 /* To handle many slaves on the same link */
33 74 int confirmation_to_ignore;
34 75 } modbus_rtu_t;
  76 +
  77 +#endif /* MODBUS_RTU_PRIVATE_H */
... ...
3rdparty/libmodbus/modbus-rtu.c
  View file @5735b40
  1 +/*
  2 + * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
1 6  
2 7 #include <stdio.h>
3 8 #include <stdlib.h>
4 9 #include <errno.h>
5 10 #include <fcntl.h>
6 11 #include <string.h>
  12 +#ifndef _MSC_VER
7 13 #include <unistd.h>
  14 +#endif
8 15 #include <assert.h>
9 16  
10 17 #include "modbus-private.h"
... ... @@ -82,15 +89,12 @@ static const uint8_t table_crc_lo[] = {
82 89  
83 90 /* Define the slave ID of the remote device to talk in master mode or set the
84 91 * internal slave ID in slave mode */
85   -static int _modbus_set_slave( modbus_t *ctx, int slave )
  92 +static int _modbus_set_slave(modbus_t *ctx, int slave)
86 93 {
87 94 /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
88   - if (slave >= 0 && slave <= 247)
89   - {
  95 + if (slave >= 0 && slave <= 247) {
90 96 ctx->slave = slave;
91   - }
92   - else
93   - {
  97 + } else {
94 98 errno = EINVAL;
95 99 return -1;
96 100 }
... ... @@ -99,7 +103,9 @@ static int _modbus_set_slave( modbus_t *ctx, int slave )
99 103 }
100 104  
101 105 /* Builds a RTU request header */
102   -static int _modbus_rtu_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
  106 +static int _modbus_rtu_build_request_basis(modbus_t *ctx, int function,
  107 + int addr, int nb,
  108 + uint8_t *req)
103 109 {
104 110 assert(ctx->slave != -1);
105 111 req[0] = ctx->slave;
... ... @@ -113,7 +119,7 @@ static int _modbus_rtu_build_request_basis( modbus_t *ctx, int function, int add
113 119 }
114 120  
115 121 /* Builds a RTU response header */
116   -static int _modbus_rtu_build_response_basis( sft_t *sft, uint8_t *rsp )
  122 +static int _modbus_rtu_build_response_basis(sft_t *sft, uint8_t *rsp)
117 123 {
118 124 /* In this case, the slave is certainly valid because a check is already
119 125 * done in _modbus_rtu_listen */
... ... @@ -123,11 +129,11 @@ static int _modbus_rtu_build_response_basis( sft_t *sft, uint8_t *rsp )
123 129 return _MODBUS_RTU_PRESET_RSP_LENGTH;
124 130 }
125 131  
126   -static uint16_t crc16( uint8_t *buffer, uint16_t buffer_length )
  132 +static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length)
127 133 {
128   - uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
129   - uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
130   - unsigned int i; /* will index into CRC lookup */
  134 + uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
  135 + uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
  136 + unsigned int i; /* will index into CRC lookup */
131 137  
132 138 /* pass through message buffer */
133 139 while (buffer_length--) {
... ... @@ -139,47 +145,138 @@ static uint16_t crc16( uint8_t *buffer, uint16_t buffer_length )
139 145 return (crc_hi << 8 | crc_lo);
140 146 }
141 147  
142   -static int _modbus_rtu_prepare_response_tid( const uint8_t *req, int *req_length )
  148 +static int _modbus_rtu_prepare_response_tid(const uint8_t *req, int *req_length)
143 149 {
144 150 (*req_length) -= _MODBUS_RTU_CHECKSUM_LENGTH;
145 151 /* No TID */
146 152 return 0;
147 153 }
148 154  
149   -static int _modbus_rtu_send_msg_pre( uint8_t *req, int req_length )
  155 +static int _modbus_rtu_send_msg_pre(uint8_t *req, int req_length)
150 156 {
151   - uint16_t crc = crc16( req, req_length );
  157 + uint16_t crc = crc16(req, req_length);
152 158 req[req_length++] = crc >> 8;
153 159 req[req_length++] = crc & 0x00FF;
154 160  
155 161 return req_length;
156 162 }
157 163  
  164 +#if defined(_WIN32)
  165 +
  166 +/* This simple implementation is sort of a substitute of the select() call,
  167 + * working this way: the win32_ser_select() call tries to read some data from
  168 + * the serial port, setting the timeout as the select() call would. Data read is
  169 + * stored into the receive buffer, that is then consumed by the win32_ser_read()
  170 + * call. So win32_ser_select() does both the event waiting and the reading,
  171 + * while win32_ser_read() only consumes the receive buffer.
  172 + */
  173 +
  174 +static void win32_ser_init(struct win32_ser *ws)
  175 +{
  176 + /* Clear everything */
  177 + memset(ws, 0x00, sizeof(struct win32_ser));
  178 +
  179 + /* Set file handle to invalid */
  180 + ws->fd = INVALID_HANDLE_VALUE;
  181 +}
  182 +
  183 +/* FIXME Try to remove length_to_read -> max_len argument, only used by win32 */
  184 +static int win32_ser_select(struct win32_ser *ws, int max_len,
  185 + const struct timeval *tv)
  186 +{
  187 + COMMTIMEOUTS comm_to;
  188 + unsigned int msec = 0;
  189 +
  190 + /* Check if some data still in the buffer to be consumed */
  191 + if (ws->n_bytes > 0) {
  192 + return 1;
  193 + }
  194 +
  195 + /* Setup timeouts like select() would do.
  196 + FIXME Please someone on Windows can look at this?
  197 + Does it possible to use WaitCommEvent?
  198 + When tv is NULL, MAXDWORD isn't infinite!
  199 + */
  200 + if (tv == NULL) {
  201 + msec = MAXDWORD;
  202 + } else {
  203 + msec = tv->tv_sec * 1000 + tv->tv_usec / 1000;
  204 + if (msec < 1)
  205 + msec = 1;
  206 + }
  207 +
  208 + comm_to.ReadIntervalTimeout = msec;
  209 + comm_to.ReadTotalTimeoutMultiplier = 0;
  210 + comm_to.ReadTotalTimeoutConstant = msec;
  211 + comm_to.WriteTotalTimeoutMultiplier = 0;
  212 + comm_to.WriteTotalTimeoutConstant = 1000;
  213 + SetCommTimeouts(ws->fd, &comm_to);
  214 +
  215 + /* Read some bytes */
  216 + if ((max_len > PY_BUF_SIZE) || (max_len < 0)) {
  217 + max_len = PY_BUF_SIZE;
  218 + }
  219 +
  220 + if (ReadFile(ws->fd, &ws->buf, max_len, &ws->n_bytes, NULL)) {
  221 + /* Check if some bytes available */
  222 + if (ws->n_bytes > 0) {
  223 + /* Some bytes read */
  224 + return 1;
  225 + } else {
  226 + /* Just timed out */
  227 + return 0;
  228 + }
  229 + } else {
  230 + /* Some kind of error */
  231 + return -1;
  232 + }
  233 +}
  234 +
  235 +static int win32_ser_read(struct win32_ser *ws, uint8_t *p_msg,
  236 + unsigned int max_len)
  237 +{
  238 + unsigned int n = ws->n_bytes;
  239 +
  240 + if (max_len < n) {
  241 + n = max_len;
  242 + }
  243 +
  244 + if (n > 0) {
  245 + memcpy(p_msg, ws->buf, n);
  246 + }
  247 +
  248 + ws->n_bytes -= n;
  249 +
  250 + return n;
  251 +}
  252 +#endif
  253 +
158 254 #if HAVE_DECL_TIOCM_RTS
159   -static void _modbus_rtu_ioctl_rts( modbus_t *ctx, int on )
  255 +static void _modbus_rtu_ioctl_rts(modbus_t *ctx, int on)
160 256 {
161 257 int fd = ctx->s;
162 258 int flags;
163 259  
164 260 ioctl(fd, TIOCMGET, &flags);
165   - if (on)
166   - {
  261 + if (on) {
167 262 flags |= TIOCM_RTS;
168   - }
169   - else
170   - {
  263 + } else {
171 264 flags &= ~TIOCM_RTS;
172 265 }
173 266 ioctl(fd, TIOCMSET, &flags);
174 267 }
175 268 #endif
176 269  
177   -static ssize_t _modbus_rtu_send( modbus_t *ctx, const uint8_t *req, int req_length )
  270 +static ssize_t _modbus_rtu_send(modbus_t *ctx, const uint8_t *req, int req_length)
178 271 {
  272 +#if defined(_WIN32)
  273 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  274 + DWORD n_bytes = 0;
  275 + return (WriteFile(ctx_rtu->w_ser.fd, req, req_length, &n_bytes, NULL)) ? (ssize_t)n_bytes : -1;
  276 +#else
179 277 #if HAVE_DECL_TIOCM_RTS
180 278 modbus_rtu_t *ctx_rtu = ctx->backend_data;
181   - if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE)
182   - {
  279 + if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE) {
183 280 ssize_t size;
184 281  
185 282 if (ctx->debug) {
... ... @@ -195,37 +292,31 @@ static ssize_t _modbus_rtu_send( modbus_t *ctx, const uint8_t *req, int req_leng
195 292 ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
196 293  
197 294 return size;
198   - }
199   - else
200   - {
  295 + } else {
201 296 #endif
202 297 return write(ctx->s, req, req_length);
203 298 #if HAVE_DECL_TIOCM_RTS
204 299 }
205 300 #endif
  301 +#endif
206 302 }
207 303  
208   -static int _modbus_rtu_receive( modbus_t *ctx, uint8_t *req )
  304 +static int _modbus_rtu_receive(modbus_t *ctx, uint8_t *req)
209 305 {
210 306 int rc;
211 307 modbus_rtu_t *ctx_rtu = ctx->backend_data;
212 308  
213   - if( ctx_rtu->confirmation_to_ignore )
214   - {
  309 + if (ctx_rtu->confirmation_to_ignore) {
215 310 _modbus_receive_msg(ctx, req, MSG_CONFIRMATION);
216 311 /* Ignore errors and reset the flag */
217 312 ctx_rtu->confirmation_to_ignore = FALSE;
218 313 rc = 0;
219   - if( ctx->debug )
220   - {
  314 + if (ctx->debug) {
221 315 printf("Confirmation to ignore\n");
222 316 }
223   - }
224   - else
225   - {
226   - rc = _modbus_receive_msg( ctx, req, MSG_INDICATION );
227   - if( rc == 0 )
228   - {
  317 + } else {
  318 + rc = _modbus_receive_msg(ctx, req, MSG_INDICATION);
  319 + if (rc == 0) {
229 320 /* The next expected message is a confirmation to ignore */
230 321 ctx_rtu->confirmation_to_ignore = TRUE;
231 322 }
... ... @@ -233,33 +324,40 @@ static int _modbus_rtu_receive( modbus_t *ctx, uint8_t *req )
233 324 return rc;
234 325 }
235 326  
236   -static ssize_t _modbus_rtu_recv( modbus_t *ctx, uint8_t *rsp, int rsp_length )
  327 +static ssize_t _modbus_rtu_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
237 328 {
  329 +#if defined(_WIN32)
  330 + return win32_ser_read(&((modbus_rtu_t *)ctx->backend_data)->w_ser, rsp, rsp_length);
  331 +#else
238 332 return read(ctx->s, rsp, rsp_length);
  333 +#endif
239 334 }
240 335  
241 336 static int _modbus_rtu_flush(modbus_t *);
242 337  
243   -static int _modbus_rtu_pre_check_confirmation(modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length)
  338 +static int _modbus_rtu_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
  339 + const uint8_t *rsp, int rsp_length)
244 340 {
245   - /* Check responding slave is the slave we requested (except for broacast request) */
246   - if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS)
247   - {
248   - if (ctx->debug)
249   - {
250   - fprintf(stderr, "The responding slave %d isn't the requested slave %d\n", rsp[0], req[0]);
  341 + /* Check responding slave is the slave we requested (except for broacast
  342 + * request) */
  343 + if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS) {
  344 + if (ctx->debug) {
  345 + fprintf(stderr,
  346 + "The responding slave %d isn't the requested slave %d\n",
  347 + rsp[0], req[0]);
251 348 }
252 349 errno = EMBBADSLAVE;
253 350 return -1;
254   - }
255   -
256   - return 0;
  351 + } else {
  352 + return 0;
  353 + }
257 354 }
258 355  
259 356 /* The check_crc16 function shall return 0 is the message is ignored and the
260 357 message length if the CRC is valid. Otherwise it shall return -1 and set
261 358 errno to EMBADCRC. */
262   -static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
  359 +static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
  360 + const int msg_length)
263 361 {
264 362 uint16_t crc_calculated;
265 363 uint16_t crc_received;
... ... @@ -267,10 +365,8 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int ms
267 365  
268 366 /* Filter on the Modbus unit identifier (slave) in RTU mode to avoid useless
269 367 * CRC computing. */
270   - if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS)
271   - {
272   - if (ctx->debug)
273   - {
  368 + if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) {
  369 + if (ctx->debug) {
274 370 printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave);
275 371 }
276 372 /* Following call to check_confirmation handles this error */
... ... @@ -281,20 +377,15 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int ms
281 377 crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
282 378  
283 379 /* Check CRC of msg */
284   - if (crc_calculated == crc_received)
285   - {
  380 + if (crc_calculated == crc_received) {
286 381 return msg_length;
287   - }
288   - else
289   - {
290   - if (ctx->debug)
291   - {
  382 + } else {
  383 + if (ctx->debug) {
292 384 fprintf(stderr, "ERROR CRC received 0x%0X != CRC calculated 0x%0X\n",
293 385 crc_received, crc_calculated);
294 386 }
295 387  
296   - if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL)
297   - {
  388 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
298 389 _modbus_rtu_flush(ctx);
299 390 }
300 391 errno = EMBBADCRC;
... ... @@ -305,18 +396,189 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int ms
305 396 /* Sets up a serial port for RTU communications */
306 397 static int _modbus_rtu_connect(modbus_t *ctx)
307 398 {
  399 +#if defined(_WIN32)
  400 + DCB dcb;
  401 +#else
308 402 struct termios tios;
309 403 speed_t speed;
310 404 int flags;
311   -
  405 +#endif
312 406 modbus_rtu_t *ctx_rtu = ctx->backend_data;
313 407  
314   - if (ctx->debug)
315   - {
  408 + if (ctx->debug) {
316 409 printf("Opening %s at %d bauds (%c, %d, %d)\n",
317   - ctx_rtu->device, ctx_rtu->baud, ctx_rtu->parity, ctx_rtu->data_bit, ctx_rtu->stop_bit);
  410 + ctx_rtu->device, ctx_rtu->baud, ctx_rtu->parity,
  411 + ctx_rtu->data_bit, ctx_rtu->stop_bit);
  412 + }
  413 +
  414 +#if defined(_WIN32)
  415 + /* Some references here:
  416 + * http://msdn.microsoft.com/en-us/library/aa450602.aspx
  417 + */
  418 + win32_ser_init(&ctx_rtu->w_ser);
  419 +
  420 + /* ctx_rtu->device should contain a string like "COMxx:" xx being a decimal
  421 + * number */
  422 + ctx_rtu->w_ser.fd = CreateFileA(ctx_rtu->device,
  423 + GENERIC_READ | GENERIC_WRITE,
  424 + 0,
  425 + NULL,
  426 + OPEN_EXISTING,
  427 + 0,
  428 + NULL);
  429 +
  430 + /* Error checking */
  431 + if (ctx_rtu->w_ser.fd == INVALID_HANDLE_VALUE) {
  432 + if (ctx->debug) {
  433 + fprintf(stderr, "ERROR Can't open the device %s (LastError %d)\n",
  434 + ctx_rtu->device, (int)GetLastError());
  435 + }
  436 + return -1;
  437 + }
  438 +
  439 + /* Save params */
  440 + ctx_rtu->old_dcb.DCBlength = sizeof(DCB);
  441 + if (!GetCommState(ctx_rtu->w_ser.fd, &ctx_rtu->old_dcb)) {
  442 + if (ctx->debug) {
  443 + fprintf(stderr, "ERROR Error getting configuration (LastError %d)\n",
  444 + (int)GetLastError());
  445 + }
  446 + CloseHandle(ctx_rtu->w_ser.fd);
  447 + ctx_rtu->w_ser.fd = INVALID_HANDLE_VALUE;
  448 + return -1;
318 449 }
319 450  
  451 + /* Build new configuration (starting from current settings) */
  452 + dcb = ctx_rtu->old_dcb;
  453 +
  454 + /* Speed setting */
  455 + switch (ctx_rtu->baud) {
  456 + case 110:
  457 + dcb.BaudRate = CBR_110;
  458 + break;
  459 + case 300:
  460 + dcb.BaudRate = CBR_300;
  461 + break;
  462 + case 600:
  463 + dcb.BaudRate = CBR_600;
  464 + break;
  465 + case 1200:
  466 + dcb.BaudRate = CBR_1200;
  467 + break;
  468 + case 2400:
  469 + dcb.BaudRate = CBR_2400;
  470 + break;
  471 + case 4800:
  472 + dcb.BaudRate = CBR_4800;
  473 + break;
  474 + case 9600:
  475 + dcb.BaudRate = CBR_9600;
  476 + break;
  477 + case 14400:
  478 + dcb.BaudRate = CBR_14400;
  479 + break;
  480 + case 19200:
  481 + dcb.BaudRate = CBR_19200;
  482 + break;
  483 + case 38400:
  484 + dcb.BaudRate = CBR_38400;
  485 + break;
  486 + case 57600:
  487 + dcb.BaudRate = CBR_57600;
  488 + break;
  489 + case 115200:
  490 + dcb.BaudRate = CBR_115200;
  491 + break;
  492 + case 230400:
  493 + /* CBR_230400 - not defined */
  494 + dcb.BaudRate = 230400;
  495 + break;
  496 + case 250000:
  497 + dcb.BaudRate = 250000;
  498 + break;
  499 + case 460800:
  500 + dcb.BaudRate = 460800;
  501 + break;
  502 + case 500000:
  503 + dcb.BaudRate = 500000;
  504 + break;
  505 + case 921600:
  506 + dcb.BaudRate = 921600;
  507 + break;
  508 + case 1000000:
  509 + dcb.BaudRate = 1000000;
  510 + break;
  511 + default:
  512 + dcb.BaudRate = CBR_9600;
  513 + if (ctx->debug) {
  514 + fprintf(stderr, "WARNING Unknown baud rate %d for %s (B9600 used)\n",
  515 + ctx_rtu->baud, ctx_rtu->device);
  516 + }
  517 + }
  518 +
  519 + /* Data bits */
  520 + switch (ctx_rtu->data_bit) {
  521 + case 5:
  522 + dcb.ByteSize = 5;
  523 + break;
  524 + case 6:
  525 + dcb.ByteSize = 6;
  526 + break;
  527 + case 7:
  528 + dcb.ByteSize = 7;
  529 + break;
  530 + case 8:
  531 + default:
  532 + dcb.ByteSize = 8;
  533 + break;
  534 + }
  535 +
  536 + /* Stop bits */
  537 + if (ctx_rtu->stop_bit == 1)
  538 + dcb.StopBits = ONESTOPBIT;
  539 + else /* 2 */
  540 + dcb.StopBits = TWOSTOPBITS;
  541 +
  542 + /* Parity */
  543 + if (ctx_rtu->parity == 'N') {
  544 + dcb.Parity = NOPARITY;
  545 + dcb.fParity = FALSE;
  546 + } else if (ctx_rtu->parity == 'E') {
  547 + dcb.Parity = EVENPARITY;
  548 + dcb.fParity = TRUE;
  549 + } else {
  550 + /* odd */
  551 + dcb.Parity = ODDPARITY;
  552 + dcb.fParity = TRUE;
  553 + }
  554 +
  555 + /* Hardware handshaking left as default settings retrieved */
  556 +
  557 + /* No software handshaking */
  558 + dcb.fTXContinueOnXoff = TRUE;
  559 + dcb.fOutX = FALSE;
  560 + dcb.fInX = FALSE;
  561 +
  562 + /* Binary mode (it's the only supported on Windows anyway) */
  563 + dcb.fBinary = TRUE;
  564 +
  565 + /* Don't want errors to be blocking */
  566 + dcb.fAbortOnError = FALSE;
  567 +
  568 + //***Not part of libmodbus - added for QModMaster***//
  569 + dcb.fRtsControl = ctx_rtu->rts;
  570 +
  571 + /* Setup port */
  572 + if (!SetCommState(ctx_rtu->w_ser.fd, &dcb)) {
  573 + if (ctx->debug) {
  574 + fprintf(stderr, "ERROR Error setting new configuration (LastError %d)\n",
  575 + (int)GetLastError());
  576 + }
  577 + CloseHandle(ctx_rtu->w_ser.fd);
  578 + ctx_rtu->w_ser.fd = INVALID_HANDLE_VALUE;
  579 + return -1;
  580 + }
  581 +#else
320 582 /* The O_NOCTTY flag tells UNIX that this program doesn't want
321 583 to be the "controlling terminal" for that port. If you
322 584 don't specify this then any input (such as keyboard abort
... ... @@ -346,8 +608,7 @@ static int _modbus_rtu_connect(modbus_t *ctx)
346 608 /* C_ISPEED Input baud (new interface)
347 609 C_OSPEED Output baud (new interface)
348 610 */
349   - switch (ctx_rtu->baud)
350   - {
  611 + switch (ctx_rtu->baud) {
351 612 case 110:
352 613 speed = B110;
353 614 break;
... ... @@ -628,13 +889,15 @@ static int _modbus_rtu_connect(modbus_t *ctx)
628 889 */
629 890 /* Unused because we use open with the NDELAY option */
630 891 tios.c_cc[VMIN] = 0;
631   - tios.c_cc[VTIME] = 10; // Set per default to 1 second
  892 + tios.c_cc[VTIME] = 0;
632 893  
633 894 if (tcsetattr(ctx->s, TCSANOW, &tios) < 0) {
634 895 close(ctx->s);
635 896 ctx->s = -1;
636 897 return -1;
637 898 }
  899 +#endif
  900 +
638 901 return 0;
639 902 }
640 903  
... ... @@ -849,35 +1112,62 @@ static void _modbus_rtu_close(modbus_t *ctx)
849 1112 /* Restore line settings and close file descriptor in RTU mode */
850 1113 modbus_rtu_t *ctx_rtu = ctx->backend_data;
851 1114  
  1115 +#if defined(_WIN32)
  1116 + /* Revert settings */
  1117 + if (!SetCommState(ctx_rtu->w_ser.fd, &ctx_rtu->old_dcb) && ctx->debug) {
  1118 + fprintf(stderr, "ERROR Couldn't revert to configuration (LastError %d)\n",
  1119 + (int)GetLastError());
  1120 + }
  1121 +
  1122 + if (!CloseHandle(ctx_rtu->w_ser.fd) && ctx->debug) {
  1123 + fprintf(stderr, "ERROR Error while closing handle (LastError %d)\n",
  1124 + (int)GetLastError());
  1125 + }
  1126 +#else
852 1127 if (ctx->s != -1) {
853 1128 tcsetattr(ctx->s, TCSANOW, &ctx_rtu->old_tios);
854 1129 close(ctx->s);
855 1130 ctx->s = -1;
856 1131 }
  1132 +#endif
857 1133 }
858 1134  
859 1135 static int _modbus_rtu_flush(modbus_t *ctx)
860 1136 {
  1137 +#if defined(_WIN32)
  1138 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  1139 + ctx_rtu->w_ser.n_bytes = 0;
  1140 + return (PurgeComm(ctx_rtu->w_ser.fd, PURGE_RXCLEAR) == FALSE);
  1141 +#else
861 1142 return tcflush(ctx->s, TCIOFLUSH);
  1143 +#endif
862 1144 }
863 1145  
864   -static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
  1146 +static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset,
  1147 + struct timeval *tv, int length_to_read)
865 1148 {
866 1149 int s_rc;
867   - while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1)
868   - {
869   - if (errno == EINTR)
870   - {
871   - if (ctx->debug)
872   - {
  1150 +#if defined(_WIN32)
  1151 + s_rc = win32_ser_select(&((modbus_rtu_t *)ctx->backend_data)->w_ser,
  1152 + length_to_read, tv);
  1153 + if (s_rc == 0) {
  1154 + errno = ETIMEDOUT;
  1155 + return -1;
  1156 + }
  1157 +
  1158 + if (s_rc < 0) {
  1159 + return -1;
  1160 + }
  1161 +#else
  1162 + while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1) {
  1163 + if (errno == EINTR) {
  1164 + if (ctx->debug) {
873 1165 fprintf(stderr, "A non blocked signal was caught\n");
874 1166 }
875 1167 /* Necessary after an error */
876 1168 FD_ZERO(rset);
877 1169 FD_SET(ctx->s, rset);
878   - }
879   - else
880   - {
  1170 + } else {
881 1171 return -1;
882 1172 }
883 1173 }
... ... @@ -887,6 +1177,7 @@ static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, i
887 1177 errno = ETIMEDOUT;
888 1178 return -1;
889 1179 }
  1180 +#endif
890 1181  
891 1182 return s_rc;
892 1183 }
... ... @@ -919,9 +1210,10 @@ const modbus_backend_t _modbus_rtu_backend = {
919 1210 _modbus_rtu_free
920 1211 };
921 1212  
  1213 +//***Not part of libmodbus - rts param added for QModMaster***//
922 1214 modbus_t* modbus_new_rtu(const char *device,
923 1215 int baud, char parity, int data_bit,
924   - int stop_bit)
  1216 + int stop_bit, int rts)
925 1217 {
926 1218 modbus_t *ctx;
927 1219 modbus_rtu_t *ctx_rtu;
... ... @@ -981,6 +1273,11 @@ modbus_t* modbus_new_rtu(const char *device,
981 1273 ctx_rtu->rts_delay = ctx_rtu->onebyte_time;
982 1274 #endif
983 1275  
  1276 +//***Not part of libmodbus - added for QModMaster***//
  1277 +#if defined(_WIN32)
  1278 + ctx_rtu->rts = rts;
  1279 +#endif
  1280 +
984 1281 ctx_rtu->confirmation_to_ignore = FALSE;
985 1282  
986 1283 return ctx;
... ...
3rdparty/libmodbus/modbus-rtu.h
  View file @5735b40
  1 +/*
  2 + * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
1 6  
2   -#pragma once
  7 +#ifndef MODBUS_RTU_H
  8 +#define MODBUS_RTU_H
3 9  
4 10 #include "modbus.h"
5 11  
... ... @@ -10,25 +16,28 @@ MODBUS_BEGIN_DECLS
10 16 */
11 17 #define MODBUS_RTU_MAX_ADU_LENGTH 256
12 18  
13   -MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity, int data_bit, int stop_bit);
  19 +//***Not part of libmodbus - rts param added for QModMaster***//
  20 +MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity,
  21 + int data_bit, int stop_bit, int rts);
14 22  
15 23 #define MODBUS_RTU_RS232 0
16 24 #define MODBUS_RTU_RS485 1
17 25  
18   -MODBUS_API int modbus_rtu_set_serial_mode( modbus_t *ctx, int mode );
19   -MODBUS_API int modbus_rtu_get_serial_mode( modbus_t *ctx );
  26 +MODBUS_API int modbus_rtu_set_serial_mode(modbus_t *ctx, int mode);
  27 +MODBUS_API int modbus_rtu_get_serial_mode(modbus_t *ctx);
20 28  
21 29 #define MODBUS_RTU_RTS_NONE 0
22 30 #define MODBUS_RTU_RTS_UP 1
23 31 #define MODBUS_RTU_RTS_DOWN 2
24 32  
25   -MODBUS_API int modbus_rtu_set_rts( modbus_t *ctx, int mode );
26   -MODBUS_API int modbus_rtu_get_rts( modbus_t *ctx );
  33 +MODBUS_API int modbus_rtu_set_rts(modbus_t *ctx, int mode);
  34 +MODBUS_API int modbus_rtu_get_rts(modbus_t *ctx);
27 35  
28   -MODBUS_API int modbus_rtu_set_custom_rts( modbus_t *ctx, void ( *set_rts ) ( modbus_t *ctx, int on ) );
  36 +MODBUS_API int modbus_rtu_set_custom_rts(modbus_t *ctx, void (*set_rts) (modbus_t *ctx, int on));
29 37  
30   -MODBUS_API int modbus_rtu_set_rts_delay( modbus_t *ctx, int us );
31   -MODBUS_API int modbus_rtu_get_rts_delay( modbus_t *ctx );
  38 +MODBUS_API int modbus_rtu_set_rts_delay(modbus_t *ctx, int us);
  39 +MODBUS_API int modbus_rtu_get_rts_delay(modbus_t *ctx);
32 40  
33 41 MODBUS_END_DECLS
34 42  
  43 +#endif /* MODBUS_RTU_H */
... ...
3rdparty/libmodbus/modbus-tcp-private.h
  View file @5735b40
1   -#pragma once
  1 +/*
  2 + * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
  6 +
  7 +#ifndef MODBUS_TCP_PRIVATE_H
  8 +#define MODBUS_TCP_PRIVATE_H
2 9  
3 10 #define _MODBUS_TCP_HEADER_LENGTH 7
4 11 #define _MODBUS_TCP_PRESET_REQ_LENGTH 12
... ... @@ -8,8 +15,7 @@
8 15  
9 16 /* In both structures, the transaction ID must be placed on first position
10 17 to have a quick access not dependant of the TCP backend */
11   -typedef struct _modbus_tcp
12   -{
  18 +typedef struct _modbus_tcp {
13 19 /* Extract from MODBUS Messaging on TCP/IP Implementation Guide V1.0b
14 20 (page 23/46):
15 21 The transaction identifier is used to associate the future response
... ... @@ -24,8 +30,7 @@ typedef struct _modbus_tcp
24 30 #define _MODBUS_TCP_PI_NODE_LENGTH 1025
25 31 #define _MODBUS_TCP_PI_SERVICE_LENGTH 32
26 32  
27   -typedef struct _modbus_tcp_pi
28   -{
  33 +typedef struct _modbus_tcp_pi {
29 34 /* Transaction ID */
30 35 uint16_t t_id;
31 36 /* TCP port */
... ... @@ -36,4 +41,4 @@ typedef struct _modbus_tcp_pi
36 41 char service[_MODBUS_TCP_PI_SERVICE_LENGTH];
37 42 } modbus_tcp_pi_t;
38 43  
39   -
  44 +#endif /* MODBUS_TCP_PRIVATE_H */
... ...
3rdparty/libmodbus/modbus-tcp.c
  View file @5735b40
1   -
2 1 #include <stdio.h>
3 2 #include <stdlib.h>
4 3 #include <string.h>
... ... @@ -21,30 +20,46 @@
21 20 # include <arpa/inet.h>
22 21 # include <netdb.h>
23 22  
  23 +
24 24 #if !defined(MSG_NOSIGNAL)
25 25 #define MSG_NOSIGNAL 0
26 26 #endif
27 27  
  28 +#if defined(_AIX) && !defined(MSG_DONTWAIT)
  29 +#define MSG_DONTWAIT MSG_NONBLOCK
  30 +#endif
  31 +
28 32 #include "modbus-private.h"
29 33  
30 34 #include "modbus-tcp.h"
31 35 #include "modbus-tcp-private.h"
32 36  
  37 +#ifdef OS_WIN32
  38 +static int _modbus_tcp_init_win32(void)
  39 +{
  40 + /* Initialise Windows Socket API */
  41 + WSADATA wsaData;
  42 +
  43 + if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
  44 + fprintf(stderr, "WSAStartup() returned error code %d\n",
  45 + (unsigned int)GetLastError());
  46 + errno = EIO;
  47 + return -1;
  48 + }
  49 + return 0;
  50 +}
  51 +#endif
  52 +
33 53 static int _modbus_set_slave(modbus_t *ctx, int slave)
34 54 {
35 55 /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
36   - if ( slave >= 0 && slave <= 247)
37   - {
  56 + if (slave >= 0 && slave <= 247) {
38 57 ctx->slave = slave;
39   - }
40   - else if ( slave == MODBUS_TCP_SLAVE )
41   - {
  58 + } else if (slave == MODBUS_TCP_SLAVE) {
42 59 /* The special value MODBUS_TCP_SLAVE (0xFF) can be used in TCP mode to
43 60 * restore the default value. */
44 61 ctx->slave = slave;
45   - }
46   - else
47   - {
  62 + } else {
48 63 errno = EINVAL;
49 64 return -1;
50 65 }
... ... @@ -53,7 +68,9 @@ static int _modbus_set_slave(modbus_t *ctx, int slave)
53 68 }
54 69  
55 70 /* Builds a TCP request header */
56   -static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
  71 +static int _modbus_tcp_build_request_basis(modbus_t *ctx, int function,
  72 + int addr, int nb,
  73 + uint8_t *req)
57 74 {
58 75 modbus_tcp_t *ctx_tcp = ctx->backend_data;
59 76  
... ... @@ -62,7 +79,6 @@ static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int add
62 79 ctx_tcp->t_id++;
63 80 else
64 81 ctx_tcp->t_id = 0;
65   -
66 82 req[0] = ctx_tcp->t_id >> 8;
67 83 req[1] = ctx_tcp->t_id & 0x00ff;
68 84  
... ... @@ -84,7 +100,7 @@ static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int add
84 100 }
85 101  
86 102 /* Builds a TCP response header */
87   -static int _modbus_tcp_build_response_basis( sft_t *sft, uint8_t *rsp )
  103 +static int _modbus_tcp_build_response_basis(sft_t *sft, uint8_t *rsp)
88 104 {
89 105 /* Extract from MODBUS Messaging on TCP/IP Implementation
90 106 Guide V1.0b (page 23/46):
... ... @@ -107,12 +123,12 @@ static int _modbus_tcp_build_response_basis( sft_t *sft, uint8_t *rsp )
107 123 }
108 124  
109 125  
110   -static int _modbus_tcp_prepare_response_tid( const uint8_t *req, int *req_length )
  126 +static int _modbus_tcp_prepare_response_tid(const uint8_t *req, int *req_length)
111 127 {
112 128 return (req[0] << 8) + req[1];
113 129 }
114 130  
115   -static int _modbus_tcp_send_msg_pre( uint8_t *req, int req_length )
  131 +static int _modbus_tcp_send_msg_pre(uint8_t *req, int req_length)
116 132 {
117 133 /* Substract the header length to the message length */
118 134 int mbap_length = req_length - 6;
... ... @@ -123,7 +139,7 @@ static int _modbus_tcp_send_msg_pre( uint8_t *req, int req_length )
123 139 return req_length;
124 140 }
125 141  
126   -static ssize_t _modbus_tcp_send( modbus_t *ctx, const uint8_t *req, int req_length )
  142 +static ssize_t _modbus_tcp_send(modbus_t *ctx, const uint8_t *req, int req_length)
127 143 {
128 144 /* MSG_NOSIGNAL
129 145 Requests not to send SIGPIPE on errors on stream oriented
... ... @@ -132,27 +148,25 @@ static ssize_t _modbus_tcp_send( modbus_t *ctx, const uint8_t *req, int req_leng
132 148 return send(ctx->s, (const char *)req, req_length, MSG_NOSIGNAL);
133 149 }
134 150  
135   -static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req)
136   -{
  151 +static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req) {
137 152 return _modbus_receive_msg(ctx, req, MSG_INDICATION);
138 153 }
139 154  
140   -static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
141   -{
142   - return recv( ctx->s, (char *)rsp, rsp_length, 0 );
  155 +static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length) {
  156 + return recv(ctx->s, (char *)rsp, rsp_length, 0);
143 157 }
144 158  
145   -static int _modbus_tcp_check_integrity( modbus_t *ctx, uint8_t *msg, const int msg_length )
  159 +static int _modbus_tcp_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
146 160 {
147 161 return msg_length;
148 162 }
149 163  
150   -static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length )
  164 +static int _modbus_tcp_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
  165 + const uint8_t *rsp, int rsp_length)
151 166 {
152 167 /* Check transaction ID */
153 168 if (req[0] != rsp[0] || req[1] != rsp[1]) {
154   - if (ctx->debug)
155   - {
  169 + if (ctx->debug) {
156 170 fprintf(stderr, "Invalid transaction ID received 0x%X (not 0x%X)\n",
157 171 (rsp[0] << 8) + rsp[1], (req[0] << 8) + req[1]);
158 172 }
... ... @@ -161,10 +175,8 @@ static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req
161 175 }
162 176  
163 177 /* Check protocol ID */
164   - if (rsp[2] != 0x0 && rsp[3] != 0x0)
165   - {
166   - if (ctx->debug)
167   - {
  178 + if (rsp[2] != 0x0 && rsp[3] != 0x0) {
  179 + if (ctx->debug) {
168 180 fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n",
169 181 (rsp[2] << 8) + rsp[3]);
170 182 }
... ... @@ -175,7 +187,7 @@ static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req
175 187 return 0;
176 188 }
177 189  
178   -static int _modbus_tcp_set_ipv4_options( int s )
  190 +static int _modbus_tcp_set_ipv4_options(int s)
179 191 {
180 192 int rc;
181 193 int option;
... ... @@ -183,9 +195,9 @@ static int _modbus_tcp_set_ipv4_options( int s )
183 195 /* Set the TCP no delay flag */
184 196 /* SOL_TCP = IPPROTO_TCP */
185 197 option = 1;
186   - rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (const void *)&option, sizeof(int) );
187   - if (rc == -1)
188   - {
  198 + rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY,
  199 + (const void *)&option, sizeof(int));
  200 + if (rc == -1) {
189 201 return -1;
190 202 }
191 203  
... ... @@ -193,10 +205,19 @@ static int _modbus_tcp_set_ipv4_options( int s )
193 205 * make sockets non-blocking */
194 206 /* Do not care about the return value, this is optional */
195 207 #if !defined(SOCK_NONBLOCK) && defined(FIONBIO)
  208 +#ifdef OS_WIN32
  209 + {
  210 + /* Setting FIONBIO expects an unsigned long according to MSDN */
  211 + u_long loption = 1;
  212 + ioctlsocket(s, FIONBIO, &loption);
  213 + }
  214 +#else
196 215 option = 1;
197 216 ioctl(s, FIONBIO, &option);
198 217 #endif
  218 +#endif
199 219  
  220 +#ifndef OS_WIN32
200 221 /**
201 222 * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's
202 223 * necessary to workaround that problem.
... ... @@ -208,16 +229,26 @@ static int _modbus_tcp_set_ipv4_options( int s )
208 229 if (rc == -1) {
209 230 return -1;
210 231 }
  232 +#endif
211 233  
212 234 return 0;
213 235 }
214 236  
215   -static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen, const struct timeval *ro_tv )
  237 +static int _connect(int sockfd, const struct sockaddr *addr, socklen_t addrlen,
  238 + const struct timeval *ro_tv)
216 239 {
217 240 int rc = connect(sockfd, addr, addrlen);
218 241  
219   - if (rc == -1 && errno == EINPROGRESS)
220   - {
  242 +#ifdef OS_WIN32
  243 + int wsaError = 0;
  244 + if (rc == -1) {
  245 + wsaError = WSAGetLastError();
  246 + }
  247 +
  248 + if (wsaError == WSAEWOULDBLOCK || wsaError == WSAEINPROGRESS) {
  249 +#else
  250 + if (rc == -1 && errno == EINPROGRESS) {
  251 +#endif
221 252 fd_set wset;
222 253 int optval;
223 254 socklen_t optlen = sizeof(optval);
... ... @@ -227,20 +258,16 @@ static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen,
227 258 FD_ZERO(&wset);
228 259 FD_SET(sockfd, &wset);
229 260 rc = select(sockfd + 1, NULL, &wset, NULL, &tv);
230   - if (rc <= 0)
231   - {
  261 + if (rc <= 0) {
232 262 /* Timeout or fail */
233 263 return -1;
234 264 }
235 265  
236 266 /* The connection is established if SO_ERROR and optval are set to 0 */
237 267 rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *)&optval, &optlen);
238   - if ( rc == 0 && optval == 0 )
239   - {
  268 + if (rc == 0 && optval == 0) {
240 269 return 0;
241   - }
242   - else
243   - {
  270 + } else {
244 271 errno = ECONNREFUSED;
245 272 return -1;
246 273 }
... ... @@ -249,7 +276,7 @@ static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen,
249 276 }
250 277  
251 278 /* Establishes a modbus TCP connection with a Modbus server. */
252   -static int _modbus_tcp_connect( modbus_t *ctx )
  279 +static int _modbus_tcp_connect(modbus_t *ctx)
253 280 {
254 281 int rc;
255 282 /* Specialized version of sockaddr for Internet socket address (same size) */
... ... @@ -257,6 +284,12 @@ static int _modbus_tcp_connect( modbus_t *ctx )
257 284 modbus_tcp_t *ctx_tcp = ctx->backend_data;
258 285 int flags = SOCK_STREAM;
259 286  
  287 +#ifdef OS_WIN32
  288 + if (_modbus_tcp_init_win32() == -1) {
  289 + return -1;
  290 + }
  291 +#endif
  292 +
260 293 #ifdef SOCK_CLOEXEC
261 294 flags |= SOCK_CLOEXEC;
262 295 #endif
... ... @@ -266,21 +299,18 @@ static int _modbus_tcp_connect( modbus_t *ctx )
266 299 #endif
267 300  
268 301 ctx->s = socket(PF_INET, flags, 0);
269   - if (ctx->s == -1)
270   - {
  302 + if (ctx->s == -1) {
271 303 return -1;
272 304 }
273 305  
274 306 rc = _modbus_tcp_set_ipv4_options(ctx->s);
275   - if (rc == -1)
276   - {
  307 + if (rc == -1) {
277 308 close(ctx->s);
278 309 ctx->s = -1;
279 310 return -1;
280 311 }
281 312  
282   - if (ctx->debug)
283   - {
  313 + if (ctx->debug) {
284 314 printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port);
285 315 }
286 316  
... ... @@ -288,9 +318,8 @@ static int _modbus_tcp_connect( modbus_t *ctx )
288 318 addr.sin_port = htons(ctx_tcp->port);
289 319 addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
290 320 rc = _connect(ctx->s, (struct sockaddr *)&addr, sizeof(addr), &ctx->response_timeout);
291   - if( rc == -1 )
292   - {
293   - close( ctx->s );
  321 + if (rc == -1) {
  322 + close(ctx->s);
294 323 ctx->s = -1;
295 324 return -1;
296 325 }
... ... @@ -299,7 +328,7 @@ static int _modbus_tcp_connect( modbus_t *ctx )
299 328 }
300 329  
301 330 /* Establishes a modbus TCP PI connection with a Modbus server. */
302   -static int _modbus_tcp_pi_connect( modbus_t *ctx )
  331 +static int _modbus_tcp_pi_connect(modbus_t *ctx)
303 332 {
304 333 int rc;
305 334 struct addrinfo *ai_list;
... ... @@ -307,6 +336,12 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
307 336 struct addrinfo ai_hints;
308 337 modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
309 338  
  339 +#ifdef OS_WIN32
  340 + if (_modbus_tcp_init_win32() == -1) {
  341 + return -1;
  342 + }
  343 +#endif
  344 +
310 345 memset(&ai_hints, 0, sizeof(ai_hints));
311 346 #ifdef AI_ADDRCONFIG
312 347 ai_hints.ai_flags |= AI_ADDRCONFIG;
... ... @@ -318,19 +353,17 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
318 353 ai_hints.ai_next = NULL;
319 354  
320 355 ai_list = NULL;
321   - rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service, &ai_hints, &ai_list);
322   - if (rc != 0)
323   - {
324   - if (ctx->debug)
325   - {
  356 + rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service,
  357 + &ai_hints, &ai_list);
  358 + if (rc != 0) {
  359 + if (ctx->debug) {
326 360 fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
327 361 }
328 362 errno = ECONNREFUSED;
329 363 return -1;
330 364 }
331 365  
332   - for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next)
333   - {
  366 + for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
334 367 int flags = ai_ptr->ai_socktype;
335 368 int s;
336 369  
... ... @@ -349,14 +382,12 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
349 382 if (ai_ptr->ai_family == AF_INET)
350 383 _modbus_tcp_set_ipv4_options(s);
351 384  
352   - if (ctx->debug)
353   - {
  385 + if (ctx->debug) {
354 386 printf("Connecting to [%s]:%s\n", ctx_tcp_pi->node, ctx_tcp_pi->service);
355 387 }
356 388  
357 389 rc = _connect(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen, &ctx->response_timeout);
358   - if (rc == -1)
359   - {
  390 + if (rc == -1) {
360 391 close(s);
361 392 continue;
362 393 }
... ... @@ -367,8 +398,7 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
367 398  
368 399 freeaddrinfo(ai_list);
369 400  
370   - if (ctx->s < 0)
371   - {
  401 + if (ctx->s < 0) {
372 402 return -1;
373 403 }
374 404  
... ... @@ -376,88 +406,102 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
376 406 }
377 407  
378 408 /* Closes the network connection and socket in TCP mode */
379   -static void _modbus_tcp_close( modbus_t *ctx )
  409 +static void _modbus_tcp_close(modbus_t *ctx)
380 410 {
381   - if (ctx->s != -1)
382   - {
  411 + if (ctx->s != -1) {
383 412 shutdown(ctx->s, SHUT_RDWR);
384 413 close(ctx->s);
385 414 ctx->s = -1;
386 415 }
387 416 }
388 417  
389   -static int _modbus_tcp_flush( modbus_t *ctx )
  418 +static int _modbus_tcp_flush(modbus_t *ctx)
390 419 {
391 420 int rc;
392 421 int rc_sum = 0;
393 422  
394   - do
395   - {
  423 + do {
396 424 /* Extract the garbage from the socket */
397 425 char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
398   -
  426 +#ifndef OS_WIN32
399 427 rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
400   - if ( rc > 0 )
401   - {
  428 +#else
  429 + /* On Win32, it's a bit more complicated to not wait */
  430 + fd_set rset;
  431 + struct timeval tv;
  432 +
  433 + tv.tv_sec = 0;
  434 + tv.tv_usec = 0;
  435 + FD_ZERO(&rset);
  436 + FD_SET(ctx->s, &rset);
  437 + rc = select(ctx->s+1, &rset, NULL, NULL, &tv);
  438 + if (rc == -1) {
  439 + return -1;
  440 + }
  441 +
  442 + if (rc == 1) {
  443 + /* There is data to flush */
  444 + rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0);
  445 + }
  446 +#endif
  447 + if (rc > 0) {
402 448 rc_sum += rc;
403 449 }
404   - } while ( rc == MODBUS_TCP_MAX_ADU_LENGTH );
  450 + } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
405 451  
406 452 return rc_sum;
407 453 }
408 454  
409 455 /* Listens for any request from one or many modbus masters in TCP */
410   -int modbus_tcp_listen( modbus_t *ctx, int nb_connection )
  456 +int modbus_tcp_listen(modbus_t *ctx, int nb_connection)
411 457 {
412 458 int new_s;
413 459 int enable;
414 460 struct sockaddr_in addr;
415 461 modbus_tcp_t *ctx_tcp;
416 462  
417   - if (ctx == NULL)
418   - {
  463 + if (ctx == NULL) {
419 464 errno = EINVAL;
420 465 return -1;
421 466 }
422 467  
423 468 ctx_tcp = ctx->backend_data;
424 469  
  470 +#ifdef OS_WIN32
  471 + if (_modbus_tcp_init_win32() == -1) {
  472 + return -1;
  473 + }
  474 +#endif
  475 +
425 476 new_s = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
426   - if ( new_s == -1 )
427   - {
  477 + if (new_s == -1) {
428 478 return -1;
429 479 }
430 480  
431 481 enable = 1;
432   - if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR, (char *)&enable, sizeof(enable)) == -1)
433   - {
  482 + if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR,
  483 + (char *)&enable, sizeof(enable)) == -1) {
434 484 close(new_s);
435 485 return -1;
436 486 }
437 487  
438 488 memset(&addr, 0, sizeof(addr));
439 489 addr.sin_family = AF_INET;
440   -
441 490 /* If the modbus port is < to 1024, we need the setuid root. */
442 491 addr.sin_port = htons(ctx_tcp->port);
443   - if (ctx_tcp->ip[0] == '0')
444   - {
  492 + if (ctx_tcp->ip[0] == '0') {
445 493 /* Listen any addresses */
446 494 addr.sin_addr.s_addr = htonl(INADDR_ANY);
447   - }
448   - else
449   - {
  495 + } else {
450 496 /* Listen only specified IP address */
451 497 addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
452 498 }
453   - if (bind( new_s, (struct sockaddr *)&addr, sizeof(addr)) == -1)
454   - {
  499 + if (bind(new_s, (struct sockaddr *)&addr, sizeof(addr)) == -1) {
455 500 close(new_s);
456 501 return -1;
457 502 }
458 503  
459   - if (listen(new_s, nb_connection) == -1)
460   - {
  504 + if (listen(new_s, nb_connection) == -1) {
461 505 close(new_s);
462 506 return -1;
463 507 }
... ... @@ -465,7 +509,7 @@ int modbus_tcp_listen( modbus_t *ctx, int nb_connection )
465 509 return new_s;
466 510 }
467 511  
468   -int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
  512 +int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection)
469 513 {
470 514 int rc;
471 515 struct addrinfo *ai_list;
... ... @@ -476,33 +520,32 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
476 520 int new_s;
477 521 modbus_tcp_pi_t *ctx_tcp_pi;
478 522  
479   - if (ctx == NULL)
480   - {
  523 + if (ctx == NULL) {
481 524 errno = EINVAL;
482 525 return -1;
483 526 }
484 527  
485 528 ctx_tcp_pi = ctx->backend_data;
486 529  
487   - if ( ctx_tcp_pi->node[0] == 0)
488   - {
  530 +#ifdef OS_WIN32
  531 + if (_modbus_tcp_init_win32() == -1) {
  532 + return -1;
  533 + }
  534 +#endif
  535 +
  536 + if (ctx_tcp_pi->node[0] == 0) {
489 537 node = NULL; /* == any */
490   - }
491   - else
492   - {
  538 + } else {
493 539 node = ctx_tcp_pi->node;
494 540 }
495 541  
496   - if ( ctx_tcp_pi->service[0] == 0 )
497   - {
  542 + if (ctx_tcp_pi->service[0] == 0) {
498 543 service = "502";
499   - }
500   - else
501   - {
  544 + } else {
502 545 service = ctx_tcp_pi->service;
503 546 }
504 547  
505   - memset( &ai_hints, 0, sizeof (ai_hints) );
  548 + memset(&ai_hints, 0, sizeof (ai_hints));
506 549 /* If node is not NULL, than the AI_PASSIVE flag is ignored. */
507 550 ai_hints.ai_flags |= AI_PASSIVE;
508 551 #ifdef AI_ADDRCONFIG
... ... @@ -516,10 +559,8 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
516 559  
517 560 ai_list = NULL;
518 561 rc = getaddrinfo(node, service, &ai_hints, &ai_list);
519   - if (rc != 0)
520   - {
521   - if (ctx->debug)
522   - {
  562 + if (rc != 0) {
  563 + if (ctx->debug) {
523 564 fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
524 565 }
525 566 errno = ECONNREFUSED;
... ... @@ -527,28 +568,23 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
527 568 }
528 569  
529 570 new_s = -1;
530   - for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next)
531   - {
  571 + for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
532 572 int s;
533 573  
534   - s = socket( ai_ptr->ai_family, ai_ptr->ai_socktype, ai_ptr->ai_protocol );
535   - if (s < 0)
536   - {
537   - if (ctx->debug)
538   - {
  574 + s = socket(ai_ptr->ai_family, ai_ptr->ai_socktype,
  575 + ai_ptr->ai_protocol);
  576 + if (s < 0) {
  577 + if (ctx->debug) {
539 578 perror("socket");
540 579 }
541 580 continue;
542   - }
543   - else
544   - {
  581 + } else {
545 582 int enable = 1;
546   - rc = setsockopt( s, SOL_SOCKET, SO_REUSEADDR, (void *)&enable, sizeof (enable) );
547   - if (rc != 0)
548   - {
  583 + rc = setsockopt(s, SOL_SOCKET, SO_REUSEADDR,
  584 + (void *)&enable, sizeof (enable));
  585 + if (rc != 0) {
549 586 close(s);
550   - if (ctx->debug)
551   - {
  587 + if (ctx->debug) {
552 588 perror("setsockopt");
553 589 }
554 590 continue;
... ... @@ -556,22 +592,18 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
556 592 }
557 593  
558 594 rc = bind(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen);
559   - if (rc != 0)
560   - {
  595 + if (rc != 0) {
561 596 close(s);
562   - if (ctx->debug)
563   - {
  597 + if (ctx->debug) {
564 598 perror("bind");
565 599 }
566 600 continue;
567 601 }
568 602  
569 603 rc = listen(s, nb_connection);
570   - if (rc != 0)
571   - {
  604 + if (rc != 0) {
572 605 close(s);
573   - if (ctx->debug)
574   - {
  606 + if (ctx->debug) {
575 607 perror("listen");
576 608 }
577 609 continue;
... ... @@ -582,8 +614,7 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
582 614 }
583 615 freeaddrinfo(ai_list);
584 616  
585   - if (new_s < 0)
586   - {
  617 + if (new_s < 0) {
587 618 return -1;
588 619 }
589 620  
... ... @@ -595,8 +626,7 @@ int modbus_tcp_accept(modbus_t *ctx, int *s)
595 626 struct sockaddr_in addr;
596 627 socklen_t addrlen;
597 628  
598   - if (ctx == NULL)
599   - {
  629 + if (ctx == NULL) {
600 630 errno = EINVAL;
601 631 return -1;
602 632 }
... ... @@ -609,15 +639,13 @@ int modbus_tcp_accept(modbus_t *ctx, int *s)
609 639 ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
610 640 #endif
611 641  
612   - if (ctx->s == -1)
613   - {
  642 + if (ctx->s == -1) {
614 643 close(*s);
615 644 *s = -1;
616 645 return -1;
617 646 }
618 647  
619   - if (ctx->debug)
620   - {
  648 + if (ctx->debug) {
621 649 printf("The client connection from %s is accepted\n",
622 650 inet_ntoa(addr.sin_addr));
623 651 }
... ... @@ -625,13 +653,12 @@ int modbus_tcp_accept(modbus_t *ctx, int *s)
625 653 return ctx->s;
626 654 }
627 655  
628   -int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
  656 +int modbus_tcp_pi_accept(modbus_t *ctx, int *s)
629 657 {
630 658 struct sockaddr_storage addr;
631 659 socklen_t addrlen;
632 660  
633   - if (ctx == NULL)
634   - {
  661 + if (ctx == NULL) {
635 662 errno = EINVAL;
636 663 return -1;
637 664 }
... ... @@ -643,14 +670,12 @@ int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
643 670 #else
644 671 ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
645 672 #endif
646   - if (ctx->s == -1)
647   - {
  673 + if (ctx->s == -1) {
648 674 close(*s);
649 675 *s = -1;
650 676 }
651 677  
652   - if (ctx->debug)
653   - {
  678 + if (ctx->debug) {
654 679 printf("The client connection is accepted.\n");
655 680 }
656 681  
... ... @@ -660,26 +685,20 @@ int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
660 685 static int _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
661 686 {
662 687 int s_rc;
663   - while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1)
664   - {
665   - if (errno == EINTR)
666   - {
667   - if (ctx->debug)
668   - {
  688 + while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1) {
  689 + if (errno == EINTR) {
  690 + if (ctx->debug) {
669 691 fprintf(stderr, "A non blocked signal was caught\n");
670 692 }
671 693 /* Necessary after an error */
672 694 FD_ZERO(rset);
673 695 FD_SET(ctx->s, rset);
674   - }
675   - else
676   - {
  696 + } else {
677 697 return -1;
678 698 }
679 699 }
680 700  
681   - if ( s_rc == 0 )
682   - {
  701 + if (s_rc == 0) {
683 702 errno = ETIMEDOUT;
684 703 return -1;
685 704 }
... ... @@ -687,14 +706,12 @@ static int _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, i
687 706 return s_rc;
688 707 }
689 708  
690   -static void _modbus_tcp_free( modbus_t *ctx )
691   -{
  709 +static void _modbus_tcp_free(modbus_t *ctx) {
692 710 free(ctx->backend_data);
693 711 free(ctx);
694 712 }
695 713  
696   -const modbus_backend_t _modbus_tcp_backend =
697   -{
  714 +const modbus_backend_t _modbus_tcp_backend = {
698 715 _MODBUS_BACKEND_TYPE_TCP,
699 716 _MODBUS_TCP_HEADER_LENGTH,
700 717 _MODBUS_TCP_CHECKSUM_LENGTH,
... ... @@ -717,8 +734,7 @@ const modbus_backend_t _modbus_tcp_backend =
717 734 };
718 735  
719 736  
720   -const modbus_backend_t _modbus_tcp_pi_backend =
721   -{
  737 +const modbus_backend_t _modbus_tcp_pi_backend = {
722 738 _MODBUS_BACKEND_TYPE_TCP,
723 739 _MODBUS_TCP_HEADER_LENGTH,
724 740 _MODBUS_TCP_CHECKSUM_LENGTH,
... ... @@ -740,14 +756,27 @@ const modbus_backend_t _modbus_tcp_pi_backend =
740 756 _modbus_tcp_free
741 757 };
742 758  
743   -modbus_t* modbus_new_tcp( const char *ip, int port )
  759 +modbus_t* modbus_new_tcp(const char *ip, int port)
744 760 {
745 761 modbus_t *ctx;
746 762 modbus_tcp_t *ctx_tcp;
747 763 size_t dest_size;
748 764 size_t ret_size;
749 765  
750   - ctx = (modbus_t *)malloc(sizeof(modbus_t) );
  766 +#if defined(OS_BSD)
  767 + /* MSG_NOSIGNAL is unsupported on *BSD so we install an ignore
  768 + handler for SIGPIPE. */
  769 + struct sigaction sa;
  770 +
  771 + sa.sa_handler = SIG_IGN;
  772 + if (sigaction(SIGPIPE, &sa, NULL) < 0) {
  773 + /* The debug flag can't be set here... */
  774 + fprintf(stderr, "Coud not install SIGPIPE handler.\n");
  775 + return NULL;
  776 + }
  777 +#endif
  778 +
  779 + ctx = (modbus_t *)malloc(sizeof(modbus_t));
751 780 _modbus_init_common(ctx);
752 781  
753 782 /* Could be changed after to reach a remote serial Modbus device */
... ... @@ -755,31 +784,26 @@ modbus_t* modbus_new_tcp( const char *ip, int port )
755 784  
756 785 ctx->backend = &_modbus_tcp_backend;
757 786  
758   - ctx->backend_data = ( modbus_tcp_t *)malloc(sizeof(modbus_tcp_t) );
  787 + ctx->backend_data = (modbus_tcp_t *)malloc(sizeof(modbus_tcp_t));
759 788 ctx_tcp = (modbus_tcp_t *)ctx->backend_data;
760 789  
761   - if (ip != NULL)
762   - {
  790 + if (ip != NULL) {
763 791 dest_size = sizeof(char) * 16;
764 792 ret_size = strlcpy(ctx_tcp->ip, ip, dest_size);
765   - if (ret_size == 0)
766   - {
  793 + if (ret_size == 0) {
767 794 fprintf(stderr, "The IP string is empty\n");
768 795 modbus_free(ctx);
769 796 errno = EINVAL;
770 797 return NULL;
771 798 }
772 799  
773   - if (ret_size >= dest_size)
774   - {
  800 + if (ret_size >= dest_size) {
775 801 fprintf(stderr, "The IP string has been truncated\n");
776 802 modbus_free(ctx);
777 803 errno = EINVAL;
778 804 return NULL;
779 805 }
780   - }
781   - else
782   - {
  806 + } else {
783 807 ctx_tcp->ip[0] = '0';
784 808 }
785 809 ctx_tcp->port = port;
... ... @@ -789,12 +813,12 @@ modbus_t* modbus_new_tcp( const char *ip, int port )
789 813 }
790 814  
791 815  
792   -modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
  816 +modbus_t* modbus_new_tcp_pi(const char *node, const char *service)
793 817 {
794   - modbus_t *ctx;
  818 + modbus_t *ctx;
795 819 modbus_tcp_pi_t *ctx_tcp_pi;
796   - size_t dest_size;
797   - size_t ret_size;
  820 + size_t dest_size;
  821 + size_t ret_size;
798 822  
799 823 ctx = (modbus_t *)malloc(sizeof(modbus_t));
800 824 _modbus_init_common(ctx);
... ... @@ -807,25 +831,20 @@ modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
807 831 ctx->backend_data = (modbus_tcp_pi_t *)malloc(sizeof(modbus_tcp_pi_t));
808 832 ctx_tcp_pi = (modbus_tcp_pi_t *)ctx->backend_data;
809 833  
810   - if (node == NULL)
811   - {
  834 + if (node == NULL) {
812 835 /* The node argument can be empty to indicate any hosts */
813 836 ctx_tcp_pi->node[0] = 0;
814   - }
815   - else
816   - {
  837 + } else {
817 838 dest_size = sizeof(char) * _MODBUS_TCP_PI_NODE_LENGTH;
818 839 ret_size = strlcpy(ctx_tcp_pi->node, node, dest_size);
819   - if (ret_size == 0)
820   - {
  840 + if (ret_size == 0) {
821 841 fprintf(stderr, "The node string is empty\n");
822 842 modbus_free(ctx);
823 843 errno = EINVAL;
824 844 return NULL;
825 845 }
826 846  
827   - if (ret_size >= dest_size)
828   - {
  847 + if (ret_size >= dest_size) {
829 848 fprintf(stderr, "The node string has been truncated\n");
830 849 modbus_free(ctx);
831 850 errno = EINVAL;
... ... @@ -833,27 +852,22 @@ modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
833 852 }
834 853 }
835 854  
836   - if (service != NULL)
837   - {
  855 + if (service != NULL) {
838 856 dest_size = sizeof(char) * _MODBUS_TCP_PI_SERVICE_LENGTH;
839 857 ret_size = strlcpy(ctx_tcp_pi->service, service, dest_size);
840   - }
841   - else
842   - {
  858 + } else {
843 859 /* Empty service is not allowed, error catched below. */
844 860 ret_size = 0;
845 861 }
846 862  
847   - if (ret_size == 0)
848   - {
  863 + if (ret_size == 0) {
849 864 fprintf(stderr, "The service string is empty\n");
850 865 modbus_free(ctx);
851 866 errno = EINVAL;
852 867 return NULL;
853 868 }
854 869  
855   - if (ret_size >= dest_size)
856   - {
  870 + if (ret_size >= dest_size) {
857 871 fprintf(stderr, "The service string has been truncated\n");
858 872 modbus_free(ctx);
859 873 errno = EINVAL;
... ...
3rdparty/libmodbus/modbus-tcp.h
  View file @5735b40
1   -#pragma once
  1 +/*
  2 + * Copyright © 2001-2010 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
  6 +
  7 +#ifndef MODBUS_TCP_H
  8 +#define MODBUS_TCP_H
2 9  
3 10 #include "modbus.h"
4 11  
5 12 MODBUS_BEGIN_DECLS
6 13  
  14 +#if defined(_WIN32) && !defined(__CYGWIN__)
  15 +/* Win32 with MinGW, supplement to <errno.h> */
  16 +#include <winsock2.h>
  17 +#if !defined(ECONNRESET)
  18 +#define ECONNRESET WSAECONNRESET
  19 +#endif
  20 +#if !defined(ECONNREFUSED)
  21 +#define ECONNREFUSED WSAECONNREFUSED
  22 +#endif
  23 +#if !defined(ETIMEDOUT)
  24 +#define ETIMEDOUT WSAETIMEDOUT
  25 +#endif
  26 +#if !defined(ENOPROTOOPT)
  27 +#define ENOPROTOOPT WSAENOPROTOOPT
  28 +#endif
  29 +#if !defined(EINPROGRESS)
  30 +#define EINPROGRESS WSAEINPROGRESS
  31 +#endif
  32 +#endif
  33 +
7 34 #define MODBUS_TCP_DEFAULT_PORT 502
8 35 #define MODBUS_TCP_SLAVE 0xFF
9 36  
... ... @@ -22,3 +49,4 @@ MODBUS_API int modbus_tcp_pi_accept(modbus_t *ctx, int *s);
22 49  
23 50 MODBUS_END_DECLS
24 51  
  52 +#endif /* MODBUS_TCP_H */
... ...
3rdparty/libmodbus/modbus-version.h
  View file @5735b40
1   -#pragma once
  1 +/*
  2 + * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * This library is free software; you can redistribute it and/or
  5 + * modify it under the terms of the GNU Lesser General Public
  6 + * License as published by the Free Software Foundation; either
  7 + * version 2.1 of the License, or (at your option) any later version.
  8 + *
  9 + * This library is distributed in the hope that it will be useful,
  10 + * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
  12 + * Lesser General Public License for more details.
  13 + *
  14 + * You should have received a copy of the GNU Lesser General Public
  15 + * License along with this library; if not, write to the Free Software
  16 + * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
  17 + */
  18 +
  19 +#ifndef MODBUS_VERSION_H
  20 +#define MODBUS_VERSION_H
2 21  
3 22 /* The major version, (1, if %LIBMODBUS_VERSION is 1.2.3) */
4 23 #define LIBMODBUS_VERSION_MAJOR (3)
... ... @@ -31,4 +50,4 @@
31 50 LIBMODBUS_VERSION_MINOR == (minor) && \
32 51 LIBMODBUS_VERSION_MICRO >= (micro)))
33 52  
34   -
  53 +#endif /* MODBUS_VERSION_H */
... ...
3rdparty/libmodbus/modbus.c
  View file @5735b40
1   -#include <config.h>
2   -#include <errno.h>
3   -#include <limits.h>
  1 +/*
  2 + * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + *
  6 + * This library implements the Modbus protocol.
  7 + * http://libmodbus.org/
  8 + */
  9 +
4 10 #include <stdio.h>
5 11 #include <string.h>
6 12 #include <stdlib.h>
7 13 #include <stdarg.h>
  14 +#include <errno.h>
  15 +#include <limits.h>
8 16 #include <time.h>
  17 +#ifndef _MSC_VER
9 18 #include <unistd.h>
  19 +#endif
  20 +
  21 +#include <config.h>
10 22  
11 23 #include "modbus.h"
12 24 #include "modbus-private.h"
... ... @@ -14,67 +26,65 @@
14 26 /* Internal use */
15 27 #define MSG_LENGTH_UNDEFINED -1
16 28  
  29 +/* Exported version */
  30 +const unsigned int libmodbus_version_major = LIBMODBUS_VERSION_MAJOR;
  31 +const unsigned int libmodbus_version_minor = LIBMODBUS_VERSION_MINOR;
  32 +const unsigned int libmodbus_version_micro = LIBMODBUS_VERSION_MICRO;
  33 +
17 34 /* Max between RTU and TCP max adu length (so TCP) */
18 35 #define MAX_MESSAGE_LENGTH 260
19 36  
20 37 /* 3 steps are used to parse the query */
21   -typedef enum
22   -{
  38 +typedef enum {
23 39 _STEP_FUNCTION,
24 40 _STEP_META,
25 41 _STEP_DATA
26 42 } _step_t;
27 43  
28   -const char *modbus_strerror( int errnum )
29   -{
30   - switch (errnum)
31   - {
32   - case EMBXILFUN:
33   - return "Illegal function";
34   - case EMBXILADD:
35   - return "Illegal data address";
36   - case EMBXILVAL:
37   - return "Illegal data value";
38   - case EMBXSFAIL:
39   - return "Slave device or server failure";
40   - case EMBXACK:
41   - return "Acknowledge";
42   - case EMBXSBUSY:
43   - return "Slave device or server is busy";
44   - case EMBXNACK:
45   - return "Negative acknowledge";
46   - case EMBXMEMPAR:
47   - return "Memory parity error";
48   - case EMBXGPATH:
49   - return "Gateway path unavailable";
50   - case EMBXGTAR:
51   - return "Target device failed to respond";
52   - case EMBBADCRC:
53   - return "Invalid CRC";
54   - case EMBBADDATA:
55   - return "Invalid data";
56   - case EMBBADEXC:
57   - return "Invalid exception code";
58   - case EMBMDATA:
59   - return "Too many data";
60   - case EMBBADSLAVE:
61   - return "Response not from requested slave";
62   - default:
63   - return strerror(errnum);
  44 +const char *modbus_strerror(int errnum) {
  45 + switch (errnum) {
  46 + case EMBXILFUN:
  47 + return "Illegal function";
  48 + case EMBXILADD:
  49 + return "Illegal data address";
  50 + case EMBXILVAL:
  51 + return "Illegal data value";
  52 + case EMBXSFAIL:
  53 + return "Slave device or server failure";
  54 + case EMBXACK:
  55 + return "Acknowledge";
  56 + case EMBXSBUSY:
  57 + return "Slave device or server is busy";
  58 + case EMBXNACK:
  59 + return "Negative acknowledge";
  60 + case EMBXMEMPAR:
  61 + return "Memory parity error";
  62 + case EMBXGPATH:
  63 + return "Gateway path unavailable";
  64 + case EMBXGTAR:
  65 + return "Target device failed to respond";
  66 + case EMBBADCRC:
  67 + return "Invalid CRC";
  68 + case EMBBADDATA:
  69 + return "Invalid data";
  70 + case EMBBADEXC:
  71 + return "Invalid exception code";
  72 + case EMBMDATA:
  73 + return "Too many data";
  74 + case EMBBADSLAVE:
  75 + return "Response not from requested slave";
  76 + default:
  77 + return strerror(errnum);
64 78 }
65 79 }
66 80  
67 81 void _error_print(modbus_t *ctx, const char *context)
68 82 {
69   - if (ctx->debug)
70   - {
  83 + if (ctx->debug) {
71 84 fprintf(stderr, "ERROR %s", modbus_strerror(errno));
72   - if (context != NULL)
73   - {
  85 + if (context != NULL) {
74 86 fprintf(stderr, ": %s\n", context);
75   - }
76   - else
77   - {
  87 + } else {
78 88 fprintf(stderr, "\n");
79 89 }
80 90 }
... ... @@ -83,29 +93,32 @@ void _error_print(modbus_t *ctx, const char *context)
83 93 static void _sleep_response_timeout(modbus_t *ctx)
84 94 {
85 95 /* Response timeout is always positive */
  96 +#ifdef _WIN32
  97 + /* usleep doesn't exist on Windows */
  98 + Sleep((ctx->response_timeout.tv_sec * 1000) +
  99 + (ctx->response_timeout.tv_usec / 1000));
  100 +#else
86 101 /* usleep source code */
87 102 struct timespec request, remaining;
88 103 request.tv_sec = ctx->response_timeout.tv_sec;
89 104 request.tv_nsec = ((long int)ctx->response_timeout.tv_usec) * 1000;
90   - while ( nanosleep( &request, &remaining ) == -1 && errno == EINTR )
91   - {
  105 + while (nanosleep(&request, &remaining) == -1 && errno == EINTR) {
92 106 request = remaining;
93 107 }
  108 +#endif
94 109 }
95 110  
96 111 int modbus_flush(modbus_t *ctx)
97 112 {
98 113 int rc;
99 114  
100   - if (ctx == NULL)
101   - {
  115 + if (ctx == NULL) {
102 116 errno = EINVAL;
103 117 return -1;
104 118 }
105 119  
106   - rc = ctx->backend->flush( ctx );
107   - if (rc != -1 && ctx->debug)
108   - {
  120 + rc = ctx->backend->flush(ctx);
  121 + if (rc != -1 && ctx->debug) {
109 122 /* Not all backends are able to return the number of bytes flushed */
110 123 printf("Bytes flushed (%d)\n", rc);
111 124 }
... ... @@ -118,44 +131,32 @@ static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t
118 131 int length;
119 132 const int offset = ctx->backend->header_length;
120 133  
121   - switch (req[offset])
122   - {
123   - case MODBUS_FC_READ_COILS:
124   - case MODBUS_FC_READ_DISCRETE_INPUTS:
125   - {
126   - /* Header + nb values (code from write_bits) */
127   - int nb = (req[offset + 3] << 8) | req[offset + 4];
128   - length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
129   - break;
130   - }
131   - case MODBUS_FC_WRITE_AND_READ_REGISTERS:
132   - case MODBUS_FC_READ_HOLDING_REGISTERS:
133   - case MODBUS_FC_READ_INPUT_REGISTERS:
134   - {
135   - /* Header + 2 * nb values */
136   - length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
137   - break;
138   - }
139   - case MODBUS_FC_READ_EXCEPTION_STATUS:
140   - {
141   - length = 3;
142   - break;
143   - }
144   - case MODBUS_FC_REPORT_SLAVE_ID:
145   - {
146   - /* The response is device specific (the header provides the
147   - length) */
148   - return MSG_LENGTH_UNDEFINED;
149   - }
150   - case MODBUS_FC_MASK_WRITE_REGISTER:
151   - {
152   - length = 7;
153   - break;
154   - }
155   - default:
156   - {
157   - length = 5;
158   - }
  134 + switch (req[offset]) {
  135 + case MODBUS_FC_READ_COILS:
  136 + case MODBUS_FC_READ_DISCRETE_INPUTS: {
  137 + /* Header + nb values (code from write_bits) */
  138 + int nb = (req[offset + 3] << 8) | req[offset + 4];
  139 + length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
  140 + }
  141 + break;
  142 + case MODBUS_FC_WRITE_AND_READ_REGISTERS:
  143 + case MODBUS_FC_READ_HOLDING_REGISTERS:
  144 + case MODBUS_FC_READ_INPUT_REGISTERS:
  145 + /* Header + 2 * nb values */
  146 + length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
  147 + break;
  148 + case MODBUS_FC_READ_EXCEPTION_STATUS:
  149 + length = 3;
  150 + break;
  151 + case MODBUS_FC_REPORT_SLAVE_ID:
  152 + /* The response is device specific (the header provides the
  153 + length) */
  154 + return MSG_LENGTH_UNDEFINED;
  155 + case MODBUS_FC_MASK_WRITE_REGISTER:
  156 + length = 7;
  157 + break;
  158 + default:
  159 + length = 5;
159 160 }
160 161  
161 162 return offset + length + ctx->backend->checksum_length;
... ... @@ -169,42 +170,34 @@ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
169 170  
170 171 msg_length = ctx->backend->send_msg_pre(msg, msg_length);
171 172  
172   - if (ctx->debug)
173   - {
  173 + if (ctx->debug) {
174 174 for (i = 0; i < msg_length; i++)
175   - {
176 175 printf("[%.2X]", msg[i]);
177   - }
178 176 printf("\n");
179 177 }
180   -
  178 +
181 179 /* In recovery mode, the write command will be issued until to be
182 180 successful! Disabled by default. */
183   - do
184   - {
  181 + do {
185 182 rc = ctx->backend->send(ctx, msg, msg_length);
186   - if (rc == -1)
187   - {
  183 + if (rc == -1) {
188 184 _error_print(ctx, NULL);
189   - if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK)
190   - {
  185 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
191 186 int saved_errno = errno;
192 187  
193   - if ( (errno == EBADF || errno == ECONNRESET || errno == EPIPE))
194   - {
  188 + if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
195 189 modbus_close(ctx);
196 190 _sleep_response_timeout(ctx);
197 191 modbus_connect(ctx);
198   - }
199   - else
200   - {
  192 + } else {
201 193 _sleep_response_timeout(ctx);
202 194 modbus_flush(ctx);
203 195 }
204 196 errno = saved_errno;
205 197 }
206 198 }
207   - } while ( (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && rc == -1);
  199 + } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
  200 + rc == -1);
208 201  
209 202 if (rc > 0 && rc != msg_length) {
210 203 errno = EMBBADDATA;
... ... @@ -214,20 +207,18 @@ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
214 207 return rc;
215 208 }
216 209  
217   -int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length )
  210 +int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length)
218 211 {
219 212 sft_t sft;
220 213 uint8_t req[MAX_MESSAGE_LENGTH];
221 214 int req_length;
222 215  
223   - if (ctx == NULL)
224   - {
  216 + if (ctx == NULL) {
225 217 errno = EINVAL;
226 218 return -1;
227 219 }
228 220  
229   - if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1))
230   - {
  221 + if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1)) {
231 222 /* The raw request must contain function and slave at least and
232 223 must not be longer than the maximum pdu length plus the slave
233 224 address. */
... ... @@ -242,8 +233,7 @@ int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length
242 233 /* This response function only set the header so it's convenient here */
243 234 req_length = ctx->backend->build_response_basis(&sft, req);
244 235  
245   - if( raw_req_length > 2 )
246   - {
  236 + if (raw_req_length > 2) {
247 237 /* Copy data after function code */
248 238 memcpy(req + req_length, raw_req + 2, raw_req_length - 2);
249 239 req_length += raw_req_length - 2;
... ... @@ -259,50 +249,39 @@ int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length
259 249 */
260 250  
261 251 /* Computes the length to read after the function received */
262   -static uint8_t compute_meta_length_after_function( int function, msg_type_t msg_type )
  252 +static uint8_t compute_meta_length_after_function(int function,
  253 + msg_type_t msg_type)
263 254 {
264 255 int length;
265 256  
266   - if (msg_type == MSG_INDICATION)
267   - {
268   - if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER)
269   - {
  257 + if (msg_type == MSG_INDICATION) {
  258 + if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER) {
270 259 length = 4;
271   - }
272   - else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS || function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS)
273   - {
  260 + } else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS ||
  261 + function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS) {
274 262 length = 5;
275   - }
276   - else if (function == MODBUS_FC_MASK_WRITE_REGISTER)
277   - {
  263 + } else if (function == MODBUS_FC_MASK_WRITE_REGISTER) {
278 264 length = 6;
279   - }
280   - else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS)
281   - {
  265 + } else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
282 266 length = 9;
283   - }
284   - else
285   - {
  267 + } else {
286 268 /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */
287 269 length = 0;
288 270 }
289   - }
290   - else
291   - {
  271 + } else {
292 272 /* MSG_CONFIRMATION */
293   - switch (function)
294   - {
295   - case MODBUS_FC_WRITE_SINGLE_COIL:
296   - case MODBUS_FC_WRITE_SINGLE_REGISTER:
297   - case MODBUS_FC_WRITE_MULTIPLE_COILS:
298   - case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
299   - length = 4;
300   - break;
301   - case MODBUS_FC_MASK_WRITE_REGISTER:
302   - length = 6;
303   - break;
304   - default:
305   - length = 1;
  273 + switch (function) {
  274 + case MODBUS_FC_WRITE_SINGLE_COIL:
  275 + case MODBUS_FC_WRITE_SINGLE_REGISTER:
  276 + case MODBUS_FC_WRITE_MULTIPLE_COILS:
  277 + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
  278 + length = 4;
  279 + break;
  280 + case MODBUS_FC_MASK_WRITE_REGISTER:
  281 + length = 6;
  282 + break;
  283 + default:
  284 + length = 1;
306 285 }
307 286 }
308 287  
... ... @@ -310,37 +289,31 @@ static uint8_t compute_meta_length_after_function( int function, msg_type_t msg_
310 289 }
311 290  
312 291 /* Computes the length to read after the meta information (address, count, etc) */
313   -static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
  292 +static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg,
  293 + msg_type_t msg_type)
314 294 {
315 295 int function = msg[ctx->backend->header_length];
316 296 int length;
317 297  
318   - if (msg_type == MSG_INDICATION)
319   - {
320   - switch (function)
321   - {
322   - case MODBUS_FC_WRITE_MULTIPLE_COILS:
323   - case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
324   - length = msg[ctx->backend->header_length + 5];
325   - break;
326   - case MODBUS_FC_WRITE_AND_READ_REGISTERS:
327   - length = msg[ctx->backend->header_length + 9];
328   - break;
329   - default:
330   - length = 0;
  298 + if (msg_type == MSG_INDICATION) {
  299 + switch (function) {
  300 + case MODBUS_FC_WRITE_MULTIPLE_COILS:
  301 + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
  302 + length = msg[ctx->backend->header_length + 5];
  303 + break;
  304 + case MODBUS_FC_WRITE_AND_READ_REGISTERS:
  305 + length = msg[ctx->backend->header_length + 9];
  306 + break;
  307 + default:
  308 + length = 0;
331 309 }
332   - }
333   - else
334   - {
  310 + } else {
335 311 /* MSG_CONFIRMATION */
336 312 if (function <= MODBUS_FC_READ_INPUT_REGISTERS ||
337 313 function == MODBUS_FC_REPORT_SLAVE_ID ||
338   - function == MODBUS_FC_WRITE_AND_READ_REGISTERS)
339   - {
  314 + function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
340 315 length = msg[ctx->backend->header_length + 1];
341   - }
342   - else
343   - {
  316 + } else {
344 317 length = 0;
345 318 }
346 319 }
... ... @@ -364,7 +337,7 @@ static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_
364 337 - read() or recv() error codes
365 338 */
366 339  
367   -int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
  340 +int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
368 341 {
369 342 int rc;
370 343 fd_set rset;
... ... @@ -374,14 +347,10 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
374 347 int msg_length = 0;
375 348 _step_t step;
376 349  
377   - if (ctx->debug)
378   - {
379   - if (msg_type == MSG_INDICATION)
380   - {
  350 + if (ctx->debug) {
  351 + if (msg_type == MSG_INDICATION) {
381 352 printf("Waiting for a indication...\n");
382   - }
383   - else
384   - {
  353 + } else {
385 354 printf("Waiting for a confirmation...\n");
386 355 }
387 356 }
... ... @@ -396,36 +365,27 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
396 365 step = _STEP_FUNCTION;
397 366 length_to_read = ctx->backend->header_length + 1;
398 367  
399   - if (msg_type == MSG_INDICATION)
400   - {
  368 + if (msg_type == MSG_INDICATION) {
401 369 /* Wait for a message, we don't know when the message will be
402 370 * received */
403 371 p_tv = NULL;
404   - }
405   - else
406   - {
  372 + } else {
407 373 tv.tv_sec = ctx->response_timeout.tv_sec;
408 374 tv.tv_usec = ctx->response_timeout.tv_usec;
409 375 p_tv = &tv;
410 376 }
411 377  
412   - while (length_to_read != 0)
413   - {
  378 + while (length_to_read != 0) {
414 379 rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
415   - if (rc == -1)
416   - {
  380 + if (rc == -1) {
417 381 _error_print(ctx, "select");
418   - if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK)
419   - {
  382 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
420 383 int saved_errno = errno;
421 384  
422   - if (errno == ETIMEDOUT)
423   - {
  385 + if (errno == ETIMEDOUT) {
424 386 _sleep_response_timeout(ctx);
425 387 modbus_flush(ctx);
426   - }
427   - else if (errno == EBADF)
428   - {
  388 + } else if (errno == EBADF) {
429 389 modbus_close(ctx);
430 390 modbus_connect(ctx);
431 391 }
... ... @@ -435,19 +395,16 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
435 395 }
436 396  
437 397 rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
438   - if (rc == 0)
439   - {
  398 + if (rc == 0) {
440 399 errno = ECONNRESET;
441 400 rc = -1;
442 401 }
443 402  
444   - if (rc == -1)
445   - {
  403 + if (rc == -1) {
446 404 _error_print(ctx, "read");
447 405 if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
448 406 (errno == ECONNRESET || errno == ECONNREFUSED ||
449   - errno == EBADF))
450   - {
  407 + errno == EBADF)) {
451 408 int saved_errno = errno;
452 409 modbus_close(ctx);
453 410 modbus_connect(ctx);
... ... @@ -458,8 +415,7 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
458 415 }
459 416  
460 417 /* Display the hex code of each character received */
461   - if (ctx->debug)
462   - {
  418 + if (ctx->debug) {
463 419 int i;
464 420 for (i=0; i < rc; i++)
465 421 printf("<%.2X>", msg[msg_length + i]);
... ... @@ -470,37 +426,34 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
470 426 /* Computes remaining bytes */
471 427 length_to_read -= rc;
472 428  
473   - if (length_to_read == 0)
474   - {
475   - switch( step )
476   - {
477   - case _STEP_FUNCTION:
478   - /* Function code position */
479   - length_to_read = compute_meta_length_after_function( msg[ctx->backend->header_length], msg_type);
480   - if (length_to_read != 0)
481   - {
482   - step = _STEP_META;
483   - break;
484   - } /* else switches straight to the next step */
485   - __attribute__((fallthrough)); ///< Let the compiler know we intentionallly created a fallthrough.
486   - case _STEP_META:
487   - length_to_read = compute_data_length_after_meta( ctx, msg, msg_type);
488   - if ( (msg_length + length_to_read) > (int)ctx->backend->max_adu_length )
489   - {
490   - errno = EMBBADDATA;
491   - _error_print(ctx, "too many data");
492   - return -1;
493   - }
494   - step = _STEP_DATA;
495   - break;
496   - default:
  429 + if (length_to_read == 0) {
  430 + switch (step) {
  431 + case _STEP_FUNCTION:
  432 + /* Function code position */
  433 + length_to_read = compute_meta_length_after_function(
  434 + msg[ctx->backend->header_length],
  435 + msg_type);
  436 + if (length_to_read != 0) {
  437 + step = _STEP_META;
497 438 break;
  439 + } /* else switches straight to the next step */
  440 + case _STEP_META:
  441 + length_to_read = compute_data_length_after_meta(
  442 + ctx, msg, msg_type);
  443 + if ((msg_length + length_to_read) > (int)ctx->backend->max_adu_length) {
  444 + errno = EMBBADDATA;
  445 + _error_print(ctx, "too many data");
  446 + return -1;
  447 + }
  448 + step = _STEP_DATA;
  449 + break;
  450 + default:
  451 + break;
498 452 }
499 453 }
500 454  
501 455 if (length_to_read > 0 &&
502   - (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0))
503   - {
  456 + (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0)) {
504 457 /* If there is no character in the buffer, the allowed timeout
505 458 interval between two consecutive bytes is defined by
506 459 byte_timeout */
... ... @@ -937,8 +890,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
937 890 }
938 891 }
939 892 break;
940   - case MODBUS_FC_REPORT_SLAVE_ID:
941   - {
  893 + case MODBUS_FC_REPORT_SLAVE_ID: {
942 894 int str_len;
943 895 int byte_count_pos;
944 896  
... ... @@ -948,8 +900,9 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
948 900 rsp[rsp_length++] = _REPORT_SLAVE_ID;
949 901 /* Run indicator status to ON */
950 902 rsp[rsp_length++] = 0xFF;
951   - str_len = 3 + strlen(MODBUS_VERSION_STRING);
952   - memcpy(rsp + rsp_length, "LMB" MODBUS_VERSION_STRING, str_len);
  903 + /* LMB + length of LIBMODBUS_VERSION_STRING */
  904 + str_len = 3 + strlen(LIBMODBUS_VERSION_STRING);
  905 + memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len);
953 906 rsp_length += str_len;
954 907 rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
955 908 }
... ... @@ -1188,8 +1141,7 @@ static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_
1188 1141 if (ctx->debug)
1189 1142 {
1190 1143 fprintf(stderr,
1191   - "ERROR Too many registers requested (%d > %d)\n",
1192   - nb, MODBUS_MAX_READ_REGISTERS);
  1144 + "ERROR Too many registers requested (%d > %d)\n", nb, MODBUS_MAX_READ_REGISTERS);
1193 1145 }
1194 1146 errno = EMBMDATA;
1195 1147 return -1;
... ... @@ -1213,10 +1165,10 @@ static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_
1213 1165  
1214 1166 offset = ctx->backend->header_length;
1215 1167  
1216   - for (i = 0; i < rc; i++) {
  1168 + for (i = 0; i < rc; i++)
  1169 + {
1217 1170 /* shift reg hi_byte to temp OR with lo_byte */
1218   - dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
1219   - rsp[offset + 3 + (i << 1)];
  1171 + dest[i] = (rsp[offset + 2 + (i << 1)] << 8) | rsp[offset + 3 + (i << 1)];
1220 1172 }
1221 1173 }
1222 1174  
... ... @@ -1234,10 +1186,8 @@ int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
1234 1186 return -1;
1235 1187 }
1236 1188  
1237   - if (nb > MODBUS_MAX_READ_REGISTERS)
1238   - {
1239   - if (ctx->debug)
1240   - {
  1189 + if (nb > MODBUS_MAX_READ_REGISTERS) {
  1190 + if (ctx->debug) {
1241 1191 fprintf(stderr,
1242 1192 "ERROR Too many registers requested (%d > %d)\n",
1243 1193 nb, MODBUS_MAX_READ_REGISTERS);
... ...
3rdparty/libmodbus/modbus.h
  View file @5735b40
  1 +/*
  2 + * Copyright © 2001-2013 Stéphane Raimbault <stephane.raimbault@gmail.com>
  3 + *
  4 + * SPDX-License-Identifier: LGPL-2.1+
  5 + */
1 6  
2   -#pragma once
  7 +#ifndef MODBUS_H
  8 +#define MODBUS_H
3 9  
4 10 /* Add this for macros that defined unix flavor */
5 11 #if (defined(__unix__) || defined(unix)) && !defined(USG)
6 12 #include <sys/param.h>
7 13 #endif
8 14  
  15 +#ifndef _MSC_VER
9 16 #include <stdint.h>
  17 +#else
  18 +#include "stdint.h"
  19 +#endif
10 20  
11   -#define MODBUS_API
  21 +#include "modbus-version.h"
12 22  
13   -#define MODBUS_VERSION_STRING "1.0.0"
  23 +#if defined(_MSC_VER)
  24 +# if defined(DLLBUILD)
  25 +/* define DLLBUILD when building the DLL */
  26 +# define MODBUS_API __declspec(dllexport)
  27 +# else
  28 +# define MODBUS_API __declspec(dllimport)
  29 +# endif
  30 +#else
  31 +# define MODBUS_API
  32 +#endif
14 33  
15 34 #ifdef __cplusplus
16   -#define MODBUS_BEGIN_DECLS extern "C" {
17   -#define MODBUS_END_DECLS }
  35 +# define MODBUS_BEGIN_DECLS extern "C" {
  36 +# define MODBUS_END_DECLS }
18 37 #else
19 38 # define MODBUS_BEGIN_DECLS
20 39 # define MODBUS_END_DECLS
... ... @@ -129,6 +148,10 @@ enum {
129 148 #define EMBMDATA (EMBXGTAR + 5)
130 149 #define EMBBADSLAVE (EMBXGTAR + 6)
131 150  
  151 +extern const unsigned int libmodbus_version_major;
  152 +extern const unsigned int libmodbus_version_minor;
  153 +extern const unsigned int libmodbus_version_micro;
  154 +
132 155 typedef struct _modbus modbus_t;
133 156  
134 157 typedef struct {
... ... @@ -185,18 +208,19 @@ MODBUS_API int modbus_write_register(modbus_t *ctx, int reg_addr, int value);
185 208 MODBUS_API int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
186 209 MODBUS_API int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
187 210 MODBUS_API int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask);
188   -MODBUS_API int modbus_write_and_read_registers(modbus_t *ctx, int write_addr, int write_nb,const uint16_t *src, int read_addr, int read_nb,uint16_t *dest);
  211 +MODBUS_API int modbus_write_and_read_registers(modbus_t *ctx, int write_addr, int write_nb,
  212 + const uint16_t *src, int read_addr, int read_nb,
  213 + uint16_t *dest);
189 214 MODBUS_API int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest);
190 215  
191   -MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address
192   -(
  216 +MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address(
193 217 unsigned int start_bits, unsigned int nb_bits,
194 218 unsigned int start_input_bits, unsigned int nb_input_bits,
195 219 unsigned int start_registers, unsigned int nb_registers,
196   - unsigned int start_input_registers, unsigned int nb_input_registers
197   -);
  220 + unsigned int start_input_registers, unsigned int nb_input_registers);
198 221  
199   -MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits, int nb_registers, int nb_input_registers);
  222 +MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits,
  223 + int nb_registers, int nb_input_registers);
200 224 MODBUS_API void modbus_mapping_free(modbus_mapping_t *mb_mapping);
201 225  
202 226 MODBUS_API int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length);
... ... @@ -205,8 +229,10 @@ MODBUS_API int modbus_receive(modbus_t *ctx, uint8_t *req);
205 229  
206 230 MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp);
207 231  
208   -MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping);
209   -MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code);
  232 +MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req,
  233 + int req_length, modbus_mapping_t *mb_mapping);
  234 +MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
  235 + unsigned int exception_code);
210 236  
211 237 /**
212 238 * UTILS FUNCTIONS
... ... @@ -260,3 +286,4 @@ MODBUS_API void modbus_set_float_cdab(float f, uint16_t *dest);
260 286  
261 287 MODBUS_END_DECLS
262 288  
  289 +#endif /* MODBUS_H */
... ...
src/ModbusAdapter.cpp
  View file @5735b40
1   -/*****************************************************************************
2   - * Copyright (c) 2022 Priva b.v.
3   - *****************************************************************************/
4 1  
5 2 #include "ConnectionConfig.h"
6   -#include "modbus.h"
7 3 #include "ModbusAdapter.h"
8   -#include "privautils/PrivaLogger.h"
9 4  
10 5 #include <cstring> /// Added for memset functionality
11 6  
12   -
13   -
14 7 ModbusAdapter::ModbusAdapter()
15 8 : m_modbus( nullptr )
16 9 {
  10 + this->InitBuffers();
17 11 }
18 12  
19 13 ModbusAdapter::~ModbusAdapter()
... ... @@ -21,25 +15,23 @@ ModbusAdapter::~ModbusAdapter()
21 15  
22 16 }
23 17  
24   -bool ModbusAdapter::ModbusConnect( const ConnectionConfig &config )
  18 +bool ModbusAdapter::ModbusConnect( const ConnectionConfig &conncfg )
25 19 {
26 20 if( m_connected )
27 21 {
28 22 this->ModbusDisconnect(); // Will already set m_connected
29 23 }
30 24  
31   - m_connType = config.getType();
  25 + m_connType = conncfg.getType();
32 26  
33 27 switch( m_connType )
34 28 {
35 29 case ConnectionType::CT_SERIAL:
36   - m_connected = this->ModbusConnectRTU( config.getPort(), config.getBaudRate(),
37   - config.getParity(), config.getDataBits(),
38   - config.getStopBits(), config.getTimeOut());
  30 + m_connected = this->ModbusConnectRTU( conncfg.getPort(), conncfg.getBaudRate(), conncfg.getParity(), conncfg.getDataBits(), conncfg.getStopBits(), conncfg.getTimeOut(), MODBUS_RTU_RTS_NONE );
39 31 break;
40 32  
41 33 case ConnectionType::CT_TCP:
42   - m_connected = this->ModbusConnectTCP( config.getIpAddress(), config.getTcpPort(), 10 );
  34 + m_connected = this->ModbusConnectTCP( conncfg.getIpAddress(), conncfg.getTcpPort(), 10 );
43 35 break;
44 36  
45 37 default:
... ... @@ -146,6 +138,7 @@ modbusData ModbusAdapter::ModbusReadHoldReg( int slaveId, int startAddress, int
146 138 }
147 139  
148 140 modbus_flush( m_modbus );
  141 + this->ClearBuffers();
149 142  
150 143 // -- End Critical Section --- ?? //
151 144  
... ... @@ -200,6 +193,8 @@ void ModbusAdapter::ModBusWriteData( int slaveId, int functionCode, int startAdd
200 193 break;
201 194 }
202 195  
  196 + delete[] data8;
  197 + delete[] data16;
203 198 modbus_flush(m_modbus); // Flush data.
204 199  
205 200 if( resultValue != noOfItems )
... ... @@ -238,18 +233,9 @@ std::string ModbusAdapter::ErrorString( int errnum ) const
238 233 }
239 234  
240 235 /* ============= PRIVATE METHODS ============= */
241   -bool ModbusAdapter::ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int timeOut )
  236 +bool ModbusAdapter::ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int RTS, int timeOut )
242 237 {
243   - { // Keep logging scope as short as possible
244   - std::string logLine( "Connecting : " + serialport + " ( "
245   - + std::to_string( baud ) + " : "
246   - + std::to_string( dataBits ) + ","
247   - + std::string( parity ) + " "
248   - + std::to_string( stopBits ) + " ) with timeout : "
249   - + std::to_string( timeout * 0.1 ) + " seconds." );
250   - PRIVALOG_INFO(this->logZone, logLine );
251   - }
252   - m_modbus = modbus_new_rtu( serialport.c_str(), baud, parity, dataBits, stopBits );
  238 + m_modbus = modbus_new_rtu( serialport.c_str(), baud, parity, dataBits, stopBits, RTS );
253 239  
254 240 #ifdef LIB_MODBUS_DEBUG_OUTPUT
255 241 // Do sensible logging through PRIVA_LOG
... ... @@ -274,7 +260,7 @@ bool ModbusAdapter::ModbusConnectRTU( const std::string &amp;serialport, int baud, c
274 260 m_connected = true;
275 261  
276 262 // Set recovery mode
277   - modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL | MODBUS_ERROR_RECOVERY_LINK );
  263 + modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL );
278 264  
279 265 // Set the response timeout
280 266 modbus_set_response_timeout( m_modbus, timeOut, 0 );
... ... @@ -315,10 +301,25 @@ bool ModbusAdapter::ModbusConnectTCP( const std::string &amp;ip, int port, int timeO
315 301 m_connected = true;
316 302  
317 303 // Set recovery mode
318   - modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL | MODBUS_ERROR_RECOVERY_LINK );
  304 + modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL );
319 305  
320 306 // Set the response timeout
321 307 modbus_set_response_timeout( m_modbus, timeOut, 0 );
322 308  
323 309 return m_connected;
324 310 }
  311 +
  312 +void ModbusAdapter::InitBuffers()
  313 +{
  314 + // Setup memory for Data.
  315 + m_dest = (uint8_t *)malloc(2000 + sizeof(uint8_t));
  316 + m_dest16 = (uint16_t *)malloc(125 * sizeof(uint16_t));
  317 +
  318 + this->ClearBuffers();
  319 +}
  320 +
  321 +void ModbusAdapter::ClearBuffers()
  322 +{
  323 + memset(m_dest, 0, 2000 * sizeof(uint8_t));
  324 + memset(m_dest16, 0, 125 * sizeof(uint16_t));
  325 +}
... ...
src/ModbusAdapter.h
  View file @5735b40
... ... @@ -6,14 +6,13 @@
6 6  
7 7 #include "ConnectionConfig.h"
8 8 #include "IModbusAdapter.h"
  9 +#include "modbus.h"
9 10  
10 11 // std
11 12 #include <memory>
12 13 #include <variant>
13 14 #include <vector>
14 15  
15   -typedef modbus_t;
16   -
17 16 /// @brief The ModbusAdapter class represents a single modbus context. Each context will
18 17 /// result in an instance of this class. It is not intended to be
19 18 /// created directly but through a factory. The factory will create
... ... @@ -34,7 +33,7 @@ public:
34 33 /*!
35 34 * \brief /// Create a modbus connection, accepting a configuration object.
36 35 */
37   - bool ModbusConnect( const ConnectionConfig &config ) override;
  36 + bool ModbusConnect( const ConnectionConfig &conncfg ) override;
38 37  
39 38 /*!
40 39 * \brief ModbusDisconnect
... ... @@ -87,34 +86,21 @@ public:
87 86 * \param errnum - error Number coming from the stack.
88 87 * \return The translated error as a string. Empty if not resolvable.
89 88 */
90   - std::string ErrorString( int errorNumber ) const override;
  89 + std::string ErrorString( int errnum ) const override;
91 90  
92 91 private: // Methods
93   - /*!
94   - * Create and connect to a serial port.
95   - *
96   - * @param serialport - The serial port by name ( i.e. /dev/ttyUSB0 )
97   - * @param baud - The port speed in a serial port context ( default = 115200 )
98   - * @param parity - The parity. ( Default : None, no parity )
99   - * @param dataBits - The number of databits. RTU uses 8 (0 - 255), ASCII uses 7 (0 - 127). Default is RTU
100   - * @param stopBits - The number of stopbits used to detect the end of the frame. ( Default = 1 )
101   - * @param timeOut - Timeout in .1 secs. See the termios documentation for deviations on this.
102   - */
103   - bool ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int timeOut );
104   -
105   - /*!
106   - * ConnectionConfig Constructor. Used to create a new TCP connection.
107   - *
108   - * @param ip - The ip address of the ModBus device we want to connect to.
109   - * @param portnum - The portnumber the ModBus device is using
110   - * @param timeOut - Timeout in which a modbus device should respond.
111   - */
  92 + bool ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int RTS, int timeOut );
112 93 bool ModbusConnectTCP( const std::string &ip, int port, int timeOut = -1 );
113 94  
  95 + void InitBuffers();
  96 + void ClearBuffers();
  97 +
114 98 private: // Members
115   - ConnectionType connectionType { ConnectionType::CT_UNKNOWN }; ///> The type of connection this instance provides. Needed for administration
116   - bool connected { false }; ///> Shows if the connection is still active.
117   - modbus_t *modbus; ///> The actual low-level modbus instance as a raw-pointer.
118   - /// ( unique_pointer gives an error on this struct )
  99 + ConnectionType m_connType { ConnectionType::CT_UNKNOWN }; ///> The type of connection this instance provides. Needed for administration
  100 + bool m_connected { false }; ///> Shows if the connection is still active.
  101 + modbus_t *m_modbus; ///> The actual low-level modbus instance as a raw-pointer. ( unique_pointer gives an error on this struct )
119 102  
  103 + // Return value Buffers ( Room for improvement )
  104 + uint8_t *m_dest;
  105 + uint16_t *m_dest16;
120 106 };
... ...
src/ModbusConnections.cpp 0 → 100644
  View file @5735b40
  1 +#include "ModbusConnections.h"
  2 +
  3 +ModbusConnections::ModbusConnections()
  4 +{
  5 +
  6 +}
  7 +
  8 +ModbusConnections::~ModbusConnections()
  9 +{
  10 + if( m_mapSerial.size() > 0 )
  11 + {
  12 + // Iterate, remove and destroy ( Searching, Seek & Destroy )
  13 + }
  14 +
  15 + if( m_mapTcp.size() > 0 )
  16 + {
  17 + // Iterate, remove and destroy ( Searching, Seek & Destroy )
  18 + }
  19 +}
  20 +
  21 +bool ModbusConnections::CreateConnection( const ConnectionConfig &config )
  22 +{
  23 + std::shared_ptr<IModbusAdapter> ptrNewAdapter = std::make_shared<ModbusAdapter>();
  24 + if( ptrNewAdapter == nullptr )
  25 + {
  26 + // Log a message and bail out
  27 + return false;
  28 + }
  29 +
  30 + // It looks like the pointer is valid. Time to connect.
  31 + ptrNewAdapter->ModbusConnect( config );
  32 + if( !ptrNewAdapter->isConnected() )
  33 + {
  34 + // Unsuccessful. Delete the object and return false
  35 + ptrNewAdapter.reset();
  36 + return false;
  37 + }
  38 +
  39 + std::shared_ptr<IModbusAdapter> ptr = connectionExist( config.getPortEnum(), createEndPoint( config ) );
  40 + if( ptr != nullptr )
  41 + {
  42 + if( !DeleteConnection( config.getPortEnum(), createEndPoint( config ) ) )
  43 + {
  44 + // Something went wrong here.. Our administration is "wonky" so report false and bail.
  45 + // New connection is not created.
  46 + ptrNewAdapter.reset();
  47 + return false;
  48 + }
  49 + }
  50 +
  51 + if( config.getType() == ConnectionType::CT_TCP )
  52 + {
  53 + // === Critical Section ===
  54 + std::lock_guard<std::mutex> guard( m_mapTcpMutex );
  55 + m_mapTcp.insert( { createEndPoint( config ), ptrNewAdapter } );
  56 + // === End Critical Section ===
  57 + }
  58 + else if( config.getType() == ConnectionType::CT_SERIAL )
  59 + {
  60 + // === Critical Section ===
  61 + std::lock_guard<std::mutex> guard( m_mapSerialMutex );
  62 + m_mapSerial.insert( { config.getPortEnum(), ptrNewAdapter } );
  63 + // === End Critical Section ===
  64 + }
  65 + else
  66 + {
  67 + // No idea what the type is but not something we recognize.
  68 + ptrNewAdapter.reset();
  69 + return false;
  70 + }
  71 + return true;
  72 +}
  73 +
  74 +bool ModbusConnections::DeleteConnection( const ConnectionPort portName, const std::string &endpoint )
  75 +{
  76 + std::shared_ptr<IModbusAdapter> ptr = this->connectionExist( portName, endpoint );
  77 + if( ptr == nullptr )
  78 + {
  79 + // Seems like it is already gone. Shouldn't happen, but ok.
  80 + return false;
  81 + }
  82 +
  83 + // First remove it from the map.
  84 + if( portName == ConnectionPort::CP_TCP )
  85 + {
  86 + // === Critical Section ===
  87 + std::lock_guard<std::mutex> guard( m_mapTcpMutex );
  88 + m_mapTcp.erase( endpoint );
  89 + // === End Critical Section ===
  90 + }
  91 + else
  92 + {
  93 + // === Critical Section ===
  94 + std::lock_guard<std::mutex> guard( m_mapSerialMutex );
  95 + m_mapSerial.erase( portName );
  96 + // === End Critical Section ===
  97 + }
  98 +
  99 + // Call the disconnect for a clean exit.
  100 + ptr->ModbusDisconnect();
  101 +
  102 + // Delete the pointer or decrease the ref-count
  103 + ptr.reset();
  104 +
  105 + return true;
  106 +}
  107 +
  108 +int ModbusConnections::ConnectionCount()
  109 +{
  110 + return ( m_mapSerial.size() + m_mapTcp.size() );
  111 +}
  112 +
  113 +std::shared_ptr<IModbusAdapter> ModbusConnections::getConnection( const ConnectionPort portName, const std::string &endpoint )
  114 +{
  115 + return this->connectionExist( portName, endpoint );
  116 +}
  117 +
  118 +std::shared_ptr<IModbusAdapter> ModbusConnections::getConnection( const ConnectionPort portName, const std::string &ipAddress, int tcpPortNumber )
  119 +{
  120 + return this->connectionExist( portName, this->createEndPoint( ipAddress, tcpPortNumber ) );
  121 +}
  122 +
  123 +AdapterList ModbusConnections::getConnections()
  124 +{
  125 + AdapterList lstResult;
  126 +
  127 + {
  128 + // === Critical Section ===
  129 + std::lock_guard<std::mutex> guard(m_mapSerialMutex);
  130 + // First we pick up the Serial Map
  131 + for( auto const& [key, value] : m_mapSerial )
  132 + {
  133 + lstResult.push_back( value );
  134 + }
  135 + // === End Critical Section ===
  136 + }
  137 +
  138 + {
  139 + // === Critical Section ===
  140 + std::lock_guard<std::mutex> guard(m_mapTcpMutex);
  141 + // Next we pick all the entries in the tcp-map
  142 + for( auto const& [key, value] : m_mapTcp )
  143 + {
  144 + lstResult.push_back( value );
  145 + }
  146 + // === End Critical Section ===
  147 + }
  148 +
  149 + return lstResult;
  150 +}
  151 +
  152 +std::shared_ptr<IModbusAdapter> ModbusConnections::connectionExist( const ConnectionPort portName, const std::string &endpoint )
  153 +{
  154 + if( portName == ConnectionPort::CP_TCP )
  155 + {
  156 + auto search = m_mapTcp.find( endpoint );
  157 + if( search != m_mapTcp.end() )
  158 + {
  159 + return search->second;
  160 + }
  161 + else
  162 + {
  163 + return nullptr;
  164 + }
  165 + }
  166 + else
  167 + {
  168 + auto search = m_mapSerial.find( portName );
  169 + if( search != m_mapSerial.end() )
  170 + {
  171 + return search->second;
  172 + }
  173 + else
  174 + {
  175 + return nullptr;
  176 + }
  177 + }
  178 +}
  179 +
  180 +std::string ModbusConnections::createEndPoint( const std::string &ipAddress, int portNumber )
  181 +{
  182 + if( portNumber > 0 && !ipAddress.empty() )
  183 + {
  184 + return std::string( "tcp://" + ipAddress + ":" + std::to_string( portNumber ) );
  185 + }
  186 +
  187 + return std::string();
  188 +}
  189 +
  190 +std::string ModbusConnections::createEndPoint( const ConnectionConfig &config )
  191 +{
  192 + if( config.getType() != ConnectionType::CT_TCP )
  193 + {
  194 + // Early opt-out
  195 + return std::string();
  196 + }
  197 +
  198 + return createEndPoint( config.getIpAddress(), config.getTcpPort() );
  199 +}
... ...
src/ModbusConnections.h 0 → 100644
  View file @5735b40
  1 +/****************************************************************************
  2 + * Copyright (c) 2022 Priva B.V.
  3 + ****************************************************************************/
  4 +#pragma once
  5 +
  6 +// Flexblox
  7 +#include "IModbusAdapter.h"
  8 +#include "ModbusAdapter.h"
  9 +
  10 +// std
  11 +#include <memory>
  12 +#include <mutex>
  13 +#include <string>
  14 +#include <unordered_map>
  15 +
  16 +/// Easy replacement of template construction
  17 +using AdapterList = std::vector<std::shared_ptr<IModbusAdapter>>;
  18 +
  19 +/*!
  20 + * \brief The ModbusConnections class conatins a list of all modbus connections, Serial and TCP.
  21 + * To access a specific connection, use its portname and / or endpoint to return it's interface.
  22 + */
  23 +class ModbusConnections
  24 +{
  25 +public:
  26 + /*!
  27 + * \brief Default CTor
  28 + */
  29 + explicit ModbusConnections();
  30 +
  31 + /*!
  32 + * \brief Default DTor
  33 + */
  34 + virtual ~ModbusConnections();
  35 +
  36 + /*!
  37 + * \brief Create a modbus connection as described by the connection object.
  38 + * \param config - The connection object describing the connection in detail.
  39 + * \return The result of the creation.
  40 + * True = succesful.
  41 + * False = Failed.
  42 + */
  43 + bool CreateConnection( const ConnectionConfig &config );
  44 +
  45 + /*!
  46 + * \brief Remove the connection described by its portname and/or endpoint.
  47 + * \param portName - The portname as its enum
  48 + * \param endpoint - combination of ip-address and portname in endpoint-format
  49 + * ( tcp://<ip_address>:<portNumber> )
  50 + * ( i.e. tcp://127.0.0.1:501 )
  51 + * \return
  52 + */
  53 + bool DeleteConnection( const ConnectionPort portName, const std::string &endpoint = std::string() );
  54 +
  55 + /*!
  56 + * \brief Give the number of registered serial and tcp-connections combined.
  57 + */
  58 + int ConnectionCount();
  59 +
  60 + /*!
  61 + * \brief Get the connection give by its parameters.
  62 + * \param portName - The portname by its enum
  63 + * \param endpoint - combination of ip-address and portname in endpoint-format
  64 + * ( tcp://<ip_address>:<portNumber> )
  65 + * ( i.e. tcp://127.0.0.1:501 )
  66 + * \return Valid Pointer to the Modbus Connection Interface. nullptr if the connection wasn't found.
  67 + */
  68 + std::shared_ptr<IModbusAdapter> getConnection( const ConnectionPort portName, const std::string &endpoint = std::string() );
  69 +
  70 + // Convenient functions
  71 + /*!
  72 + * \brief Get the connection given by its parameters. If applicable, ipaddress and portnumber will be used to create the endpoint.
  73 + * \param portName - The portname by its enum
  74 + * \param ipAddress - The ipaddress of the TCP-connection
  75 + * \param tcpPortNumber - The portnumber of the TCP-connection
  76 + * \return Valid Pointer to the Modbus Connection Interface. nullptr if the connection wasn't found.
  77 + */
  78 + std::shared_ptr<IModbusAdapter> getConnection( const ConnectionPort portName, const std::string &ipAddress, int tcpPortNumber );
  79 +
  80 + /*!
  81 + * \brief Returns a list of all registered connections by their interfaces. This is a mix of Serial and TCP-connections.
  82 + */
  83 + AdapterList getConnections();
  84 +
  85 +private:
  86 + /*!
  87 + * \brief Check if a connection already exist.
  88 + * \param portName - The portName by its enum
  89 + * \param endpoint - The endpoint this connection was registered with.
  90 + * \return A valid pointer to the Interface if the connection exist. If the connection is unknown, it will return a nullptr.
  91 + * shared_ptr can manifest themselves as booleans, so "!ptr" is sufficient to check.
  92 + */
  93 + std::shared_ptr<IModbusAdapter> connectionExist( const ConnectionPort portName, const std::string &endpoint = std::string() );
  94 +
  95 + /*!
  96 + * \brief Create the TCP endpoint based on the connections IP-address and portnumber
  97 + * \param ipAddress - The ipAddress as string.
  98 + * \param portNumber - The portnumber as integer.
  99 + * \return The endpoint in format : tcp://<ip_address>:<portNumber> i.e. tcp://127.0.0.1:501
  100 + */
  101 + std::string createEndPoint( const std::string &ipAddress, int portNumber );
  102 +
  103 + /*!
  104 + * \brief Create the TCP endpoint based on the connections IP-address and portnumber extracted from the connection configuration.
  105 + * \param config - The configuration object used to create the connection.
  106 + * \return The endpoint in format : tcp://<ip_address>:<portNumber> i.e. tcp://127.0.0.1:501
  107 + */
  108 + std::string createEndPoint( const ConnectionConfig &config );
  109 +
  110 +private:
  111 + std::mutex m_mapSerialMutex; ///< Mutex protecting the serialmap
  112 + std::mutex m_mapTcpMutex; ///< Mutex protecting the TCPmap
  113 + std::unordered_map<ConnectionPort, std::shared_ptr<IModbusAdapter>> m_mapSerial; ///< Unordered map holding the Modbus connections By PortName
  114 + std::unordered_map<std::string, std::shared_ptr<IModbusAdapter>> m_mapTcp; ///< Unordered map holding the Modbus connections By tcp://endpoint:port
  115 +};
... ...
src/main.cpp 0 → 100644
  View file @5735b40
  1 +#include <iostream>
  2 +
  3 +#include "ModbusAdapter.h"
  4 +
  5 +int main( int argc, char* argv[] )
  6 +{
  7 + ModbusAdapter oModbus;
  8 + ConnectionConfig oConfig( ConnectionPort::CP_EXTERNAL, 9600, Parity::PAR_NONE, 8, 1, 1 );
  9 +
  10 + std::cout << "========================= [START] Connection test ================================" << std::endl;
  11 + oModbus.ModbusConnect( oConfig );
  12 + if( oModbus.isConnected() )
  13 + {
  14 + std::cout << "Successful connected to : " << oConfig.getPort() << std::endl;
  15 + }
  16 + else
  17 + {
  18 + std::cout << "There was a problem connecting to : " << oConfig.getPort() << std::endl;
  19 + }
  20 +
  21 + oModbus.ModbusDisconnect();
  22 + if( !oModbus.isConnected() )
  23 + {
  24 + std::cout << "Successful disconnected from : " << oConfig.getPort() << std::endl;
  25 + }
  26 + else
  27 + {
  28 + std::cout << "There was a problem disconnecting from : " << oConfig.getPort() << std::endl;
  29 + }
  30 + std::cout << "========================= [END] Connection test ===============================" << std::endl;
  31 +
  32 + std::cout << "========================= [START] Reading test ================================" << std::endl;
  33 + std::cout << "== Reading the Hold Registers ==" << std::endl;
  34 + oModbus.ModbusConnect( oConfig );
  35 + if( oModbus.isConnected() )
  36 + {
  37 + std::cout << "Successful connected to : " << oConfig.getPort() << std::endl;
  38 + modbusData returnValues = oModbus.ModbusReadHoldReg( 1, MODBUS_FC_READ_HOLDING_REGISTERS, 2 );
  39 + if( returnValues.size() == 2 )
  40 + {
  41 + std::cout << "2 items returned from MODBUS_FC_READ_HOLDING_REGISTERS " << std::endl;
  42 + }
  43 + else
  44 + {
  45 + std::cout << "There was an error reading the Hold Registers " << std::endl;
  46 + std::cout << "Number of items returned : " << returnValues.size() << std::endl;
  47 + }
  48 +
  49 + std::cout << "== Reading the Temperature ==" << std::endl;
  50 + returnValues = oModbus.ModbusReadData( 0x01, MODBUS_FC_READ_INPUT_REGISTERS, 0x00, 0x02 );
  51 + if( returnValues.size() == 0 )
  52 + {
  53 + std::cout << "No values returned " << std::endl;
  54 + }
  55 + else
  56 + {
  57 + std::cout << "Number of items returned : " << returnValues.size() << std::endl;
  58 + }
  59 + }
  60 + else
  61 + {
  62 + std::cout << "There was a problem connecting to : " << oConfig.getPort() << std::endl;
  63 + return -1;
  64 + }
  65 + oModbus.ModbusDisconnect();
  66 + std::cout << "=========================== [END] Reading test ================================" << std::endl;
  67 +}
... ...
stack/config.h 0 → 100644
  View file @5735b40
  1 +/* Define to 1 if you have the <arpa/inet.h> header file. */
  2 +#define HAVE_ARPA_INET_H 1
  3 +
  4 +/* Define to 1 if you have the declaration of `TIOCSRS485', and to 0 if you don't. */
  5 +#define HAVE_DECL_TIOCSRS485 0
  6 +
  7 +/* Define to 1 if you have the declaration of `TIOCM_RTS', and to 0 if you don't. */
  8 +#define HAVE_DECL_TIOCM_RTS 0
  9 +
  10 +/* Define to 1 if you have the declaration of `__CYGWIN__', and to 0 if you don't. */
  11 +#define HAVE_DECL___CYGWIN__ 0
  12 +
  13 +/* Define to 1 if you have the <dlfcn.h> header file. */
  14 +#define HAVE_DLFCN_H 1
  15 +
  16 +/* Define to 1 if you have the <errno.h> header file. */
  17 +#define HAVE_ERRNO_H 1
  18 +
  19 +/* Define to 1 if you have the <fcntl.h> header file. */
  20 +#define HAVE_FCNTL_H 1
  21 +
  22 +/* Define to 1 if you have the `fork' function. */
  23 +#define HAVE_FORK 1
  24 +
  25 +/* Define to 1 if you have the `getaddrinfo' function. */
  26 +#define HAVE_GETADDRINFO 1
  27 +
  28 +/* Define to 1 if you have the `gettimeofday' function. */
  29 +#define HAVE_GETTIMEOFDAY 1
  30 +
  31 +/* Define to 1 if you have the `inet_ntoa' function. */
  32 +#define HAVE_INET_NTOA 1
  33 +
  34 +/* Define to 1 if you have the <inttypes.h> header file. */
  35 +#define HAVE_INTTYPES_H 1
  36 +
  37 +/* Define to 1 if you have the <limits.h> header file. */
  38 +#define HAVE_LIMITS_H 1
  39 +
  40 +/* Define to 1 if you have the <linux/serial.h> header file. */
  41 +#define HAVE_LINUX_SERIAL_H 1
  42 +
  43 +/* Define to 1 if you have the <memory.h> header file. */
  44 +#define HAVE_MEMORY_H 1
  45 +
  46 +/* Define to 1 if you have the `memset' function. */
  47 +#define HAVE_MEMSET 1
  48 +
  49 +/* Define to 1 if you have the <netdb.h> header file. */
  50 +#define HAVE_NETDB_H 1
  51 +
  52 +/* Define to 1 if you have the <netinet/in.h> header file. */
  53 +#define HAVE_NETINET_IN_H 1
  54 +
  55 +/* Define to 1 if you have the <netinet/tcp.h> header file. */
  56 +#define HAVE_NETINET_TCP_H 1
  57 +
  58 +/* Define to 1 if you have the `select' function. */
  59 +#define HAVE_SELECT 1
  60 +
  61 +/* Define to 1 if you have the `socket' function. */
  62 +#define HAVE_SOCKET 1
  63 +
  64 +/* Define to 1 if you have the <stdint.h> header file. */
  65 +#define HAVE_STDINT_H 1
  66 +
  67 +/* Define to 1 if you have the <stdlib.h> header file. */
  68 +#define HAVE_STDLIB_H 1
  69 +
  70 +/* Define to 1 if you have the `strerror' function. */
  71 +#define HAVE_STRERROR 1
  72 +
  73 +/* Define to 1 if you have the <strings.h> header file. */
  74 +#define HAVE_STRINGS_H 1
  75 +
  76 +/* Define to 1 if you have the <string.h> header file. */
  77 +#define HAVE_STRING_H 1
  78 +
  79 +/* Define to 1 if you have the `strlcpy' function. */
  80 +/* #undef HAVE_STRLCPY */
  81 +
  82 +/* Define to 1 if you have the <sys/ioctl.h> header file. */
  83 +#define HAVE_SYS_IOCTL_H 1
  84 +
  85 +/* Define to 1 if you have the <sys/socket.h> header file. */
  86 +#define HAVE_SYS_SOCKET_H 1
  87 +
  88 +/* Define to 1 if you have the <sys/stat.h> header file. */
  89 +#define HAVE_SYS_STAT_H 1
  90 +
  91 +/* Define to 1 if you have the <sys/time.h> header file. */
  92 +#define HAVE_SYS_TIME_H 1
  93 +
  94 +/* Define to 1 if you have the <sys/types.h> header file. */
  95 +#define HAVE_SYS_TYPES_H 1
  96 +
  97 +/* Define to 1 if you have the <termios.h> header file. */
  98 +#define HAVE_TERMIOS_H 1
  99 +
  100 +/* Define to 1 if you have the <time.h> header file. */
  101 +#define HAVE_TIME_H 1
  102 +
  103 +/* Define to 1 if you have the <unistd.h> header file. */
  104 +#define HAVE_UNISTD_H 1
  105 +
  106 +/* Define to 1 if you have the `vfork' function. */
  107 +#define HAVE_VFORK 1
  108 +
  109 +/* Define to 1 if you have the <vfork.h> header file. */
  110 +/* #undef HAVE_VFORK_H */
  111 +
  112 +/* Define to 1 if you have the <winsock2.h> header file. */
  113 +/* #undef HAVE_WINSOCK2_H */
  114 +
  115 +/* Define to 1 if `fork' works. */
  116 +#define HAVE_WORKING_FORK 1
  117 +
  118 +/* Define to 1 if `vfork' works. */
  119 +#define HAVE_WORKING_VFORK 1
  120 +
  121 +/* Define to the sub-directory in which libtool stores uninstalled libraries.
  122 + */
  123 +#define LT_OBJDIR ".libs/"
  124 +
  125 +/* Name of package */
  126 +#define PACKAGE "libmodbus"
  127 +
  128 +/* Define to the address where bug reports for this package should be sent. */
  129 +#define PACKAGE_BUGREPORT "https://github.com/stephane/libmodbus/issues"
  130 +
  131 +/* Define to the full name of this package. */
  132 +#define PACKAGE_NAME "libmodbus"
  133 +
  134 +/* Define to the full name and version of this package. */
  135 +#define PACKAGE_STRING "libmodbus 3.1.0-1"
  136 +
  137 +/* Define to the one symbol short name of this package. */
  138 +#define PACKAGE_TARNAME "modbus"
  139 +
  140 +/* Define to the home page for this package. */
  141 +#define PACKAGE_URL ""
  142 +
  143 +/* Define to the version of this package. */
  144 +#define PACKAGE_VERSION "1.0.0"
  145 +
  146 +/* Define to 1 if you have the ANSI C header files. */
  147 +#define STDC_HEADERS 1
  148 +
  149 +/* Define to 1 if you can safely include both <sys/time.h> and <time.h>. */
  150 +#define TIME_WITH_SYS_TIME 1
  151 +
  152 +/* Version number of package */
  153 +#define VERSION "1.0.0"
  154 +
  155 +/* Define to empty if `const' does not conform to ANSI C. */
  156 +/* #undef const */
  157 +
  158 +/* Define to `int' if <sys/types.h> does not define. */
  159 +/* #undef pid_t */
  160 +
  161 +/* Define to `unsigned int' if <sys/types.h> does not define. */
  162 +/* #undef size_t */
  163 +
  164 +/* Define as `fork' if `vfork' does not work. */
  165 +/* #undef vfork */
... ...
stack/modbus-data.c 0 → 100644
  View file @5735b40
  1 +
  2 +#include <stdlib.h>
  3 +
  4 +#include "stdint.h"
  5 +
  6 +#include <string.h>
  7 +#include <assert.h>
  8 +#include <arpa/inet.h>
  9 +#include <config.h>
  10 +
  11 +#include "modbus.h"
  12 +
  13 +#if defined(HAVE_BYTESWAP_H)
  14 +# include <byteswap.h>
  15 +#endif
  16 +
  17 +#if defined(__GNUC__)
  18 +# define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
  19 +# if GCC_VERSION >= 430
  20 +// Since GCC >= 4.30, GCC provides __builtin_bswapXX() alternatives so we switch to them
  21 +# undef bswap_32
  22 +# define bswap_32 __builtin_bswap32
  23 +# endif
  24 +#endif
  25 +
  26 +#if !defined(__CYGWIN__) && !defined(bswap_16)
  27 +#pragma message "Fallback on C functions for bswap_16"
  28 +static inline uint16_t bswap_16(uint16_t x)
  29 +{
  30 + return (x >> 8) | (x << 8);
  31 +}
  32 +#endif
  33 +
  34 +#if !defined(bswap_32)
  35 +#pragma message "Fallback on C functions for bswap_32"
  36 +static inline uint32_t bswap_32(uint32_t x)
  37 +{
  38 + return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
  39 +}
  40 +#endif
  41 +
  42 +/* Sets many bits from a single byte value (all 8 bits of the byte value are
  43 + set) */
  44 +void modbus_set_bits_from_byte( uint8_t *dest, int idx, const uint8_t value )
  45 +{
  46 + int i;
  47 +
  48 + for (i=0; i < 8; i++)
  49 + {
  50 + dest[idx+i] = (value & (1 << i)) ? 1 : 0;
  51 + }
  52 +}
  53 +
  54 +/* Sets many bits from a table of bytes (only the bits between idx and
  55 + idx + nb_bits are set) */
  56 +void modbus_set_bits_from_bytes( uint8_t *dest, int idx, unsigned int nb_bits, const uint8_t *tab_byte )
  57 +{
  58 + unsigned int i;
  59 + int shift = 0;
  60 +
  61 + for ( i = idx; i < idx + nb_bits; i++ )
  62 + {
  63 + dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
  64 + /* gcc doesn't like: shift = (++shift) % 8; */
  65 + shift++;
  66 + shift %= 8;
  67 + }
  68 +}
  69 +
  70 +/* Gets the byte value from many bits.
  71 + To obtain a full byte, set nb_bits to 8. */
  72 +uint8_t modbus_get_byte_from_bits( const uint8_t *src, int idx, unsigned int nb_bits )
  73 +{
  74 + unsigned int i;
  75 + uint8_t value = 0;
  76 +
  77 + if (nb_bits > 8)
  78 + {
  79 + /* Assert is ignored if NDEBUG is set */
  80 + assert(nb_bits < 8);
  81 + nb_bits = 8;
  82 + }
  83 +
  84 + for (i=0; i < nb_bits; i++)
  85 + {
  86 + value |= (src[idx+i] << i);
  87 + }
  88 +
  89 + return value;
  90 +}
  91 +
  92 +/* Get a float from 4 bytes (Modbus) without any conversion (ABCD) */
  93 +float modbus_get_float_abcd(const uint16_t *src)
  94 +{
  95 + float f;
  96 + uint32_t i;
  97 +
  98 + i = ntohl(((uint32_t)src[0] << 16) + src[1]);
  99 + memcpy(&f, &i, sizeof(float));
  100 +
  101 + return f;
  102 +}
  103 +
  104 +/* Get a float from 4 bytes (Modbus) in inversed format (DCBA) */
  105 +float modbus_get_float_dcba(const uint16_t *src)
  106 +{
  107 + float f;
  108 + uint32_t i;
  109 +
  110 + i = ntohl(bswap_32((((uint32_t)src[0]) << 16) + src[1]));
  111 + memcpy(&f, &i, sizeof(float));
  112 +
  113 + return f;
  114 +}
  115 +
  116 +/* Get a float from 4 bytes (Modbus) with swapped bytes (BADC) */
  117 +float modbus_get_float_badc(const uint16_t *src)
  118 +{
  119 + float f;
  120 + uint32_t i;
  121 +
  122 + i = ntohl((uint32_t)(bswap_16(src[0]) << 16) + bswap_16(src[1]));
  123 + memcpy(&f, &i, sizeof(float));
  124 +
  125 + return f;
  126 +}
  127 +
  128 +/* Get a float from 4 bytes (Modbus) with swapped words (CDAB) */
  129 +float modbus_get_float_cdab(const uint16_t *src)
  130 +{
  131 + float f;
  132 + uint32_t i;
  133 +
  134 + i = ntohl((((uint32_t)src[1]) << 16) + src[0]);
  135 + memcpy(&f, &i, sizeof(float));
  136 +
  137 + return f;
  138 +}
  139 +
  140 +/* DEPRECATED - Get a float from 4 bytes in sort of Modbus format */
  141 +float modbus_get_float(const uint16_t *src)
  142 +{
  143 + float f;
  144 + uint32_t i;
  145 +
  146 + i = (((uint32_t)src[1]) << 16) + src[0];
  147 + memcpy(&f, &i, sizeof(float));
  148 +
  149 + return f;
  150 +}
  151 +
  152 +/* Set a float to 4 bytes for Modbus w/o any conversion (ABCD) */
  153 +void modbus_set_float_abcd(float f, uint16_t *dest)
  154 +{
  155 + uint32_t i;
  156 +
  157 + memcpy(&i, &f, sizeof(uint32_t));
  158 + i = htonl(i);
  159 + dest[0] = (uint16_t)(i >> 16);
  160 + dest[1] = (uint16_t)i;
  161 +}
  162 +
  163 +/* Set a float to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
  164 +void modbus_set_float_dcba(float f, uint16_t *dest)
  165 +{
  166 + uint32_t i;
  167 +
  168 + memcpy(&i, &f, sizeof(uint32_t));
  169 + i = bswap_32(htonl(i));
  170 + dest[0] = (uint16_t)(i >> 16);
  171 + dest[1] = (uint16_t)i;
  172 +}
  173 +
  174 +/* Set a float to 4 bytes for Modbus with byte swap conversion (BADC) */
  175 +void modbus_set_float_badc(float f, uint16_t *dest)
  176 +{
  177 + uint32_t i;
  178 +
  179 + memcpy(&i, &f, sizeof(uint32_t));
  180 + i = htonl(i);
  181 + dest[0] = (uint16_t)bswap_16(i >> 16);
  182 + dest[1] = (uint16_t)bswap_16(i & 0xFFFF);
  183 +}
  184 +
  185 +/* Set a float to 4 bytes for Modbus with word swap conversion (CDAB) */
  186 +void modbus_set_float_cdab(float f, uint16_t *dest)
  187 +{
  188 + uint32_t i;
  189 +
  190 + memcpy(&i, &f, sizeof(uint32_t));
  191 + i = htonl(i);
  192 + dest[0] = (uint16_t)i;
  193 + dest[1] = (uint16_t)(i >> 16);
  194 +}
  195 +
  196 +/* DEPRECATED - Set a float to 4 bytes in a sort of Modbus format! */
  197 +void modbus_set_float(float f, uint16_t *dest)
  198 +{
  199 + uint32_t i;
  200 +
  201 + memcpy(&i, &f, sizeof(uint32_t));
  202 + dest[0] = (uint16_t)i;
  203 + dest[1] = (uint16_t)(i >> 16);
  204 +}
... ...
stack/modbus-private.h 0 → 100644
  View file @5735b40
  1 +
  2 +#pragma once
  3 +
  4 +# include <stdint.h>
  5 +# include <sys/time.h>
  6 +#include <sys/types.h>
  7 +#include <config.h>
  8 +
  9 +#include "modbus.h"
  10 +
  11 +MODBUS_BEGIN_DECLS
  12 +
  13 +/* It's not really the minimal length (the real one is report slave ID
  14 + * in RTU (4 bytes)) but it's a convenient size to use in RTU or TCP
  15 + * communications to read many values or write a single one.
  16 + * Maximum between :
  17 + * - HEADER_LENGTH_TCP (7) + function (1) + address (2) + number (2)
  18 + * - HEADER_LENGTH_RTU (1) + function (1) + address (2) + number (2) + CRC (2)
  19 + */
  20 +#define _MIN_REQ_LENGTH 12
  21 +#define _REPORT_SLAVE_ID 180
  22 +#define _MODBUS_EXCEPTION_RSP_LENGTH 5
  23 +
  24 +/* Timeouts in microsecond (0.5 s) */
  25 +#define _RESPONSE_TIMEOUT 500000
  26 +#define _BYTE_TIMEOUT 500000
  27 +
  28 +typedef enum
  29 +{
  30 + _MODBUS_BACKEND_TYPE_RTU=0,
  31 + _MODBUS_BACKEND_TYPE_TCP
  32 +} modbus_backend_type_t;
  33 +
  34 +/*
  35 + * ---------- Request Indication ----------
  36 + * | Client | ---------------------->| Server |
  37 + * ---------- Confirmation Response ----------
  38 + */
  39 +typedef enum
  40 +{
  41 + /* Request message on the server side */
  42 + MSG_INDICATION,
  43 + /* Request message on the client side */
  44 + MSG_CONFIRMATION
  45 +} msg_type_t;
  46 +
  47 +/* This structure reduces the number of params in functions and so
  48 + * optimizes the speed of execution (~ 37%). */
  49 +typedef struct _sft
  50 +{
  51 + int slave;
  52 + int function;
  53 + int t_id;
  54 +} sft_t;
  55 +
  56 +typedef struct _modbus_backend
  57 +{
  58 + unsigned int backend_type;
  59 + unsigned int header_length;
  60 + unsigned int checksum_length;
  61 + unsigned int max_adu_length;
  62 + int (*set_slave) (modbus_t *ctx, int slave);
  63 + int (*build_request_basis) (modbus_t *ctx, int function, int addr, int nb, uint8_t *req);
  64 + int (*build_response_basis) (sft_t *sft, uint8_t *rsp);
  65 + int (*prepare_response_tid) (const uint8_t *req, int *req_length);
  66 + int (*send_msg_pre) (uint8_t *req, int req_length);
  67 + ssize_t (*send) (modbus_t *ctx, const uint8_t *req, int req_length);
  68 + int (*receive) (modbus_t *ctx, uint8_t *req);
  69 + ssize_t (*recv) (modbus_t *ctx, uint8_t *rsp, int rsp_length);
  70 + int (*check_integrity) (modbus_t *ctx, uint8_t *msg, const int msg_length);
  71 + int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length);
  72 + int (*connect) (modbus_t *ctx);
  73 + void (*close) (modbus_t *ctx);
  74 + int (*flush) (modbus_t *ctx);
  75 + int (*select) (modbus_t *ctx, fd_set *rset, struct timeval *tv, int msg_length);
  76 + void (*free) (modbus_t *ctx);
  77 +} modbus_backend_t;
  78 +
  79 +struct _modbus
  80 +{
  81 + /* Slave address */
  82 + int slave;
  83 + /* Socket or file descriptor */
  84 + int s;
  85 + int debug;
  86 + int error_recovery;
  87 + struct timeval response_timeout;
  88 + struct timeval byte_timeout;
  89 + const modbus_backend_t *backend;
  90 + void *backend_data;
  91 +};
  92 +
  93 +void _modbus_init_common(modbus_t *ctx);
  94 +void _error_print(modbus_t *ctx, const char *context);
  95 +int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type);
  96 +
  97 +#ifndef HAVE_STRLCPY
  98 +size_t strlcpy(char *dest, const char *src, size_t dest_size);
  99 +#endif
  100 +
  101 +MODBUS_END_DECLS
... ...
stack/modbus-rtu-private.h 0 → 100644
  View file @5735b40
  1 +
  2 +#pragma once
  3 +
  4 +#include <stdint.h>
  5 +#include <termios.h>
  6 +
  7 +#define _MODBUS_RTU_HEADER_LENGTH 1
  8 +#define _MODBUS_RTU_PRESET_REQ_LENGTH 6
  9 +#define _MODBUS_RTU_PRESET_RSP_LENGTH 2
  10 +
  11 +#define _MODBUS_RTU_CHECKSUM_LENGTH 2
  12 +
  13 +typedef struct _modbus_rtu
  14 +{
  15 + /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
  16 + char *device;
  17 + /* Bauds: 9600, 19200, 57600, 115200, etc */
  18 + int baud;
  19 + /* Data bit */
  20 + uint8_t data_bit;
  21 + /* Stop bit */
  22 + uint8_t stop_bit;
  23 + /* Parity: 'N', 'O', 'E' */
  24 + char parity;
  25 + /* Save old termios settings */
  26 + struct termios old_tios;
  27 +
  28 +#if HAVE_DECL_TIOCSRS485
  29 + int serial_mode;
  30 +#endif
  31 +
  32 + /* To handle many slaves on the same link */
  33 + int confirmation_to_ignore;
  34 +} modbus_rtu_t;
... ...
stack/modbus-rtu.c 0 → 100644
  View file @5735b40
  1 +
  2 +#include <stdio.h>
  3 +#include <stdlib.h>
  4 +#include <errno.h>
  5 +#include <fcntl.h>
  6 +#include <string.h>
  7 +#include <unistd.h>
  8 +#include <assert.h>
  9 +
  10 +#include "modbus-private.h"
  11 +
  12 +#include "modbus-rtu.h"
  13 +#include "modbus-rtu-private.h"
  14 +
  15 +#if HAVE_DECL_TIOCSRS485 || HAVE_DECL_TIOCM_RTS
  16 +#include <sys/ioctl.h>
  17 +#endif
  18 +
  19 +#if HAVE_DECL_TIOCSRS485
  20 +#include <linux/serial.h>
  21 +#endif
  22 +
  23 +/* Table of CRC values for high-order byte */
  24 +static const uint8_t table_crc_hi[] = {
  25 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
  26 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  27 + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  28 + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
  29 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
  30 + 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
  31 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
  32 + 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  33 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
  34 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
  35 + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
  36 + 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
  37 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
  38 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
  39 + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  40 + 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
  41 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
  42 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  43 + 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  44 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  45 + 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
  46 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
  47 + 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
  48 + 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
  49 + 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
  50 + 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
  51 +};
  52 +
  53 +/* Table of CRC values for low-order byte */
  54 +static const uint8_t table_crc_lo[] = {
  55 + 0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
  56 + 0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
  57 + 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
  58 + 0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
  59 + 0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
  60 + 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
  61 + 0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
  62 + 0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
  63 + 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
  64 + 0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
  65 + 0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
  66 + 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
  67 + 0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
  68 + 0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
  69 + 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
  70 + 0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
  71 + 0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
  72 + 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
  73 + 0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
  74 + 0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
  75 + 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
  76 + 0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
  77 + 0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
  78 + 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
  79 + 0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
  80 + 0x43, 0x83, 0x41, 0x81, 0x80, 0x40
  81 +};
  82 +
  83 +/* Define the slave ID of the remote device to talk in master mode or set the
  84 + * internal slave ID in slave mode */
  85 +static int _modbus_set_slave( modbus_t *ctx, int slave )
  86 +{
  87 + /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
  88 + if (slave >= 0 && slave <= 247)
  89 + {
  90 + ctx->slave = slave;
  91 + }
  92 + else
  93 + {
  94 + errno = EINVAL;
  95 + return -1;
  96 + }
  97 +
  98 + return 0;
  99 +}
  100 +
  101 +/* Builds a RTU request header */
  102 +static int _modbus_rtu_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
  103 +{
  104 + assert(ctx->slave != -1);
  105 + req[0] = ctx->slave;
  106 + req[1] = function;
  107 + req[2] = addr >> 8;
  108 + req[3] = addr & 0x00ff;
  109 + req[4] = nb >> 8;
  110 + req[5] = nb & 0x00ff;
  111 +
  112 + return _MODBUS_RTU_PRESET_REQ_LENGTH;
  113 +}
  114 +
  115 +/* Builds a RTU response header */
  116 +static int _modbus_rtu_build_response_basis( sft_t *sft, uint8_t *rsp )
  117 +{
  118 + /* In this case, the slave is certainly valid because a check is already
  119 + * done in _modbus_rtu_listen */
  120 + rsp[0] = sft->slave;
  121 + rsp[1] = sft->function;
  122 +
  123 + return _MODBUS_RTU_PRESET_RSP_LENGTH;
  124 +}
  125 +
  126 +static uint16_t crc16( uint8_t *buffer, uint16_t buffer_length )
  127 +{
  128 + uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
  129 + uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
  130 + unsigned int i; /* will index into CRC lookup */
  131 +
  132 + /* pass through message buffer */
  133 + while (buffer_length--) {
  134 + i = crc_hi ^ *buffer++; /* calculate the CRC */
  135 + crc_hi = crc_lo ^ table_crc_hi[i];
  136 + crc_lo = table_crc_lo[i];
  137 + }
  138 +
  139 + return (crc_hi << 8 | crc_lo);
  140 +}
  141 +
  142 +static int _modbus_rtu_prepare_response_tid( const uint8_t *req, int *req_length )
  143 +{
  144 + (*req_length) -= _MODBUS_RTU_CHECKSUM_LENGTH;
  145 + /* No TID */
  146 + return 0;
  147 +}
  148 +
  149 +static int _modbus_rtu_send_msg_pre( uint8_t *req, int req_length )
  150 +{
  151 + uint16_t crc = crc16( req, req_length );
  152 + req[req_length++] = crc >> 8;
  153 + req[req_length++] = crc & 0x00FF;
  154 +
  155 + return req_length;
  156 +}
  157 +
  158 +#if HAVE_DECL_TIOCM_RTS
  159 +static void _modbus_rtu_ioctl_rts( modbus_t *ctx, int on )
  160 +{
  161 + int fd = ctx->s;
  162 + int flags;
  163 +
  164 + ioctl(fd, TIOCMGET, &flags);
  165 + if (on)
  166 + {
  167 + flags |= TIOCM_RTS;
  168 + }
  169 + else
  170 + {
  171 + flags &= ~TIOCM_RTS;
  172 + }
  173 + ioctl(fd, TIOCMSET, &flags);
  174 +}
  175 +#endif
  176 +
  177 +static ssize_t _modbus_rtu_send( modbus_t *ctx, const uint8_t *req, int req_length )
  178 +{
  179 +#if HAVE_DECL_TIOCM_RTS
  180 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  181 + if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE)
  182 + {
  183 + ssize_t size;
  184 +
  185 + if (ctx->debug) {
  186 + fprintf(stderr, "Sending request using RTS signal\n");
  187 + }
  188 +
  189 + ctx_rtu->set_rts(ctx, ctx_rtu->rts == MODBUS_RTU_RTS_UP);
  190 + usleep(ctx_rtu->rts_delay);
  191 +
  192 + size = write(ctx->s, req, req_length);
  193 +
  194 + usleep(ctx_rtu->onebyte_time * req_length + ctx_rtu->rts_delay);
  195 + ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
  196 +
  197 + return size;
  198 + }
  199 + else
  200 + {
  201 +#endif
  202 + return write(ctx->s, req, req_length);
  203 +#if HAVE_DECL_TIOCM_RTS
  204 + }
  205 +#endif
  206 +}
  207 +
  208 +static int _modbus_rtu_receive( modbus_t *ctx, uint8_t *req )
  209 +{
  210 + int rc;
  211 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  212 +
  213 + if( ctx_rtu->confirmation_to_ignore )
  214 + {
  215 + _modbus_receive_msg(ctx, req, MSG_CONFIRMATION);
  216 + /* Ignore errors and reset the flag */
  217 + ctx_rtu->confirmation_to_ignore = FALSE;
  218 + rc = 0;
  219 + if( ctx->debug )
  220 + {
  221 + printf("Confirmation to ignore\n");
  222 + }
  223 + }
  224 + else
  225 + {
  226 + rc = _modbus_receive_msg( ctx, req, MSG_INDICATION );
  227 + if( rc == 0 )
  228 + {
  229 + /* The next expected message is a confirmation to ignore */
  230 + ctx_rtu->confirmation_to_ignore = TRUE;
  231 + }
  232 + }
  233 + return rc;
  234 +}
  235 +
  236 +static ssize_t _modbus_rtu_recv( modbus_t *ctx, uint8_t *rsp, int rsp_length )
  237 +{
  238 + return read(ctx->s, rsp, rsp_length);
  239 +}
  240 +
  241 +static int _modbus_rtu_flush(modbus_t *);
  242 +
  243 +static int _modbus_rtu_pre_check_confirmation(modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length)
  244 +{
  245 + /* Check responding slave is the slave we requested (except for broacast request) */
  246 + if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS)
  247 + {
  248 + if (ctx->debug)
  249 + {
  250 + fprintf(stderr, "The responding slave %d isn't the requested slave %d\n", rsp[0], req[0]);
  251 + }
  252 + errno = EMBBADSLAVE;
  253 + return -1;
  254 + }
  255 +
  256 + return 0;
  257 +}
  258 +
  259 +/* The check_crc16 function shall return 0 is the message is ignored and the
  260 + message length if the CRC is valid. Otherwise it shall return -1 and set
  261 + errno to EMBADCRC. */
  262 +static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
  263 +{
  264 + uint16_t crc_calculated;
  265 + uint16_t crc_received;
  266 + int slave = msg[0];
  267 +
  268 + /* Filter on the Modbus unit identifier (slave) in RTU mode to avoid useless
  269 + * CRC computing. */
  270 + if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS)
  271 + {
  272 + if (ctx->debug)
  273 + {
  274 + printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave);
  275 + }
  276 + /* Following call to check_confirmation handles this error */
  277 + return 0;
  278 + }
  279 +
  280 + crc_calculated = crc16(msg, msg_length - 2);
  281 + crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
  282 +
  283 + /* Check CRC of msg */
  284 + if (crc_calculated == crc_received)
  285 + {
  286 + return msg_length;
  287 + }
  288 + else
  289 + {
  290 + if (ctx->debug)
  291 + {
  292 + fprintf(stderr, "ERROR CRC received 0x%0X != CRC calculated 0x%0X\n",
  293 + crc_received, crc_calculated);
  294 + }
  295 +
  296 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL)
  297 + {
  298 + _modbus_rtu_flush(ctx);
  299 + }
  300 + errno = EMBBADCRC;
  301 + return -1;
  302 + }
  303 +}
  304 +
  305 +/* Sets up a serial port for RTU communications */
  306 +static int _modbus_rtu_connect(modbus_t *ctx)
  307 +{
  308 + struct termios tios;
  309 + speed_t speed;
  310 + int flags;
  311 +
  312 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  313 +
  314 + if (ctx->debug)
  315 + {
  316 + printf("Opening %s at %d bauds (%c, %d, %d)\n",
  317 + ctx_rtu->device, ctx_rtu->baud, ctx_rtu->parity, ctx_rtu->data_bit, ctx_rtu->stop_bit);
  318 + }
  319 +
  320 + /* The O_NOCTTY flag tells UNIX that this program doesn't want
  321 + to be the "controlling terminal" for that port. If you
  322 + don't specify this then any input (such as keyboard abort
  323 + signals and so forth) will affect your process
  324 +
  325 + Timeouts are ignored in canonical input mode or when the
  326 + NDELAY option is set on the file via open or fcntl */
  327 + flags = O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL;
  328 +#ifdef O_CLOEXEC
  329 + flags |= O_CLOEXEC;
  330 +#endif
  331 +
  332 + ctx->s = open(ctx_rtu->device, flags);
  333 + if (ctx->s == -1) {
  334 + if (ctx->debug) {
  335 + fprintf(stderr, "ERROR Can't open the device %s (%s)\n",
  336 + ctx_rtu->device, strerror(errno));
  337 + }
  338 + return -1;
  339 + }
  340 +
  341 + /* Save */
  342 + tcgetattr(ctx->s, &ctx_rtu->old_tios);
  343 +
  344 + memset(&tios, 0, sizeof(struct termios));
  345 +
  346 + /* C_ISPEED Input baud (new interface)
  347 + C_OSPEED Output baud (new interface)
  348 + */
  349 + switch (ctx_rtu->baud)
  350 + {
  351 + case 110:
  352 + speed = B110;
  353 + break;
  354 + case 300:
  355 + speed = B300;
  356 + break;
  357 + case 600:
  358 + speed = B600;
  359 + break;
  360 + case 1200:
  361 + speed = B1200;
  362 + break;
  363 + case 2400:
  364 + speed = B2400;
  365 + break;
  366 + case 4800:
  367 + speed = B4800;
  368 + break;
  369 + case 9600:
  370 + speed = B9600;
  371 + break;
  372 + case 19200:
  373 + speed = B19200;
  374 + break;
  375 + case 38400:
  376 + speed = B38400;
  377 + break;
  378 +#ifdef B57600
  379 + case 57600:
  380 + speed = B57600;
  381 + break;
  382 +#endif
  383 +#ifdef B115200
  384 + case 115200:
  385 + speed = B115200;
  386 + break;
  387 +#endif
  388 +#ifdef B230400
  389 + case 230400:
  390 + speed = B230400;
  391 + break;
  392 +#endif
  393 +#ifdef B460800
  394 + case 460800:
  395 + speed = B460800;
  396 + break;
  397 +#endif
  398 +#ifdef B500000
  399 + case 500000:
  400 + speed = B500000;
  401 + break;
  402 +#endif
  403 +#ifdef B576000
  404 + case 576000:
  405 + speed = B576000;
  406 + break;
  407 +#endif
  408 +#ifdef B921600
  409 + case 921600:
  410 + speed = B921600;
  411 + break;
  412 +#endif
  413 +#ifdef B1000000
  414 + case 1000000:
  415 + speed = B1000000;
  416 + break;
  417 +#endif
  418 +#ifdef B1152000
  419 + case 1152000:
  420 + speed = B1152000;
  421 + break;
  422 +#endif
  423 +#ifdef B1500000
  424 + case 1500000:
  425 + speed = B1500000;
  426 + break;
  427 +#endif
  428 +#ifdef B2500000
  429 + case 2500000:
  430 + speed = B2500000;
  431 + break;
  432 +#endif
  433 +#ifdef B3000000
  434 + case 3000000:
  435 + speed = B3000000;
  436 + break;
  437 +#endif
  438 +#ifdef B3500000
  439 + case 3500000:
  440 + speed = B3500000;
  441 + break;
  442 +#endif
  443 +#ifdef B4000000
  444 + case 4000000:
  445 + speed = B4000000;
  446 + break;
  447 +#endif
  448 + default:
  449 + speed = B9600;
  450 + if (ctx->debug) {
  451 + fprintf(stderr,
  452 + "WARNING Unknown baud rate %d for %s (B9600 used)\n",
  453 + ctx_rtu->baud, ctx_rtu->device);
  454 + }
  455 + }
  456 +
  457 + /* Set the baud rate */
  458 + if ((cfsetispeed(&tios, speed) < 0) ||
  459 + (cfsetospeed(&tios, speed) < 0)) {
  460 + close(ctx->s);
  461 + ctx->s = -1;
  462 + return -1;
  463 + }
  464 +
  465 + /* C_CFLAG Control options
  466 + CLOCAL Local line - do not change "owner" of port
  467 + CREAD Enable receiver
  468 + */
  469 + tios.c_cflag |= (CREAD | CLOCAL);
  470 + /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
  471 +
  472 + /* Set data bits (5, 6, 7, 8 bits)
  473 + CSIZE Bit mask for data bits
  474 + */
  475 + tios.c_cflag &= ~CSIZE;
  476 + switch (ctx_rtu->data_bit) {
  477 + case 5:
  478 + tios.c_cflag |= CS5;
  479 + break;
  480 + case 6:
  481 + tios.c_cflag |= CS6;
  482 + break;
  483 + case 7:
  484 + tios.c_cflag |= CS7;
  485 + break;
  486 + case 8:
  487 + default:
  488 + tios.c_cflag |= CS8;
  489 + break;
  490 + }
  491 +
  492 + /* Stop bit (1 or 2) */
  493 + if (ctx_rtu->stop_bit == 1)
  494 + tios.c_cflag &=~ CSTOPB;
  495 + else /* 2 */
  496 + tios.c_cflag |= CSTOPB;
  497 +
  498 + /* PARENB Enable parity bit
  499 + PARODD Use odd parity instead of even */
  500 + if (ctx_rtu->parity == 'N') {
  501 + /* None */
  502 + tios.c_cflag &=~ PARENB;
  503 + } else if (ctx_rtu->parity == 'E') {
  504 + /* Even */
  505 + tios.c_cflag |= PARENB;
  506 + tios.c_cflag &=~ PARODD;
  507 + } else {
  508 + /* Odd */
  509 + tios.c_cflag |= PARENB;
  510 + tios.c_cflag |= PARODD;
  511 + }
  512 +
  513 + /* Read the man page of termios if you need more information. */
  514 +
  515 + /* This field isn't used on POSIX systems
  516 + tios.c_line = 0;
  517 + */
  518 +
  519 + /* C_LFLAG Line options
  520 +
  521 + ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
  522 + ICANON Enable canonical input (else raw)
  523 + XCASE Map uppercase \lowercase (obsolete)
  524 + ECHO Enable echoing of input characters
  525 + ECHOE Echo erase character as BS-SP-BS
  526 + ECHOK Echo NL after kill character
  527 + ECHONL Echo NL
  528 + NOFLSH Disable flushing of input buffers after
  529 + interrupt or quit characters
  530 + IEXTEN Enable extended functions
  531 + ECHOCTL Echo control characters as ^char and delete as ~?
  532 + ECHOPRT Echo erased character as character erased
  533 + ECHOKE BS-SP-BS entire line on line kill
  534 + FLUSHO Output being flushed
  535 + PENDIN Retype pending input at next read or input char
  536 + TOSTOP Send SIGTTOU for background output
  537 +
  538 + Canonical input is line-oriented. Input characters are put
  539 + into a buffer which can be edited interactively by the user
  540 + until a CR (carriage return) or LF (line feed) character is
  541 + received.
  542 +
  543 + Raw input is unprocessed. Input characters are passed
  544 + through exactly as they are received, when they are
  545 + received. Generally you'll deselect the ICANON, ECHO,
  546 + ECHOE, and ISIG options when using raw input
  547 + */
  548 +
  549 + /* Raw input */
  550 + tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
  551 +
  552 + /* C_IFLAG Input options
  553 +
  554 + Constant Description
  555 + INPCK Enable parity check
  556 + IGNPAR Ignore parity errors
  557 + PARMRK Mark parity errors
  558 + ISTRIP Strip parity bits
  559 + IXON Enable software flow control (outgoing)
  560 + IXOFF Enable software flow control (incoming)
  561 + IXANY Allow any character to start flow again
  562 + IGNBRK Ignore break condition
  563 + BRKINT Send a SIGINT when a break condition is detected
  564 + INLCR Map NL to CR
  565 + IGNCR Ignore CR
  566 + ICRNL Map CR to NL
  567 + IUCLC Map uppercase to lowercase
  568 + IMAXBEL Echo BEL on input line too long
  569 + */
  570 + if (ctx_rtu->parity == 'N') {
  571 + /* None */
  572 + tios.c_iflag &= ~INPCK;
  573 + } else {
  574 + tios.c_iflag |= INPCK;
  575 + }
  576 +
  577 + /* Software flow control is disabled */
  578 + tios.c_iflag &= ~(IXON | IXOFF | IXANY);
  579 +
  580 + /* C_OFLAG Output options
  581 + OPOST Postprocess output (not set = raw output)
  582 + ONLCR Map NL to CR-NL
  583 +
  584 + ONCLR ant others needs OPOST to be enabled
  585 + */
  586 +
  587 + /* Raw ouput */
  588 + tios.c_oflag &=~ OPOST;
  589 +
  590 + /* C_CC Control characters
  591 + VMIN Minimum number of characters to read
  592 + VTIME Time to wait for data (tenths of seconds)
  593 +
  594 + UNIX serial interface drivers provide the ability to
  595 + specify character and packet timeouts. Two elements of the
  596 + c_cc array are used for timeouts: VMIN and VTIME. Timeouts
  597 + are ignored in canonical input mode or when the NDELAY
  598 + option is set on the file via open or fcntl.
  599 +
  600 + VMIN specifies the minimum number of characters to read. If
  601 + it is set to 0, then the VTIME value specifies the time to
  602 + wait for every character read. Note that this does not mean
  603 + that a read call for N bytes will wait for N characters to
  604 + come in. Rather, the timeout will apply to the first
  605 + character and the read call will return the number of
  606 + characters immediately available (up to the number you
  607 + request).
  608 +
  609 + If VMIN is non-zero, VTIME specifies the time to wait for
  610 + the first character read. If a character is read within the
  611 + time given, any read will block (wait) until all VMIN
  612 + characters are read. That is, once the first character is
  613 + read, the serial interface driver expects to receive an
  614 + entire packet of characters (VMIN bytes total). If no
  615 + character is read within the time allowed, then the call to
  616 + read returns 0. This method allows you to tell the serial
  617 + driver you need exactly N bytes and any read call will
  618 + return 0 or N bytes. However, the timeout only applies to
  619 + the first character read, so if for some reason the driver
  620 + misses one character inside the N byte packet then the read
  621 + call could block forever waiting for additional input
  622 + characters.
  623 +
  624 + VTIME specifies the amount of time to wait for incoming
  625 + characters in tenths of seconds. If VTIME is set to 0 (the
  626 + default), reads will block (wait) indefinitely unless the
  627 + NDELAY option is set on the port with open or fcntl.
  628 + */
  629 + /* Unused because we use open with the NDELAY option */
  630 + tios.c_cc[VMIN] = 0;
  631 + tios.c_cc[VTIME] = 10; // Set per default to 1 second
  632 +
  633 + if (tcsetattr(ctx->s, TCSANOW, &tios) < 0) {
  634 + close(ctx->s);
  635 + ctx->s = -1;
  636 + return -1;
  637 + }
  638 + return 0;
  639 +}
  640 +
  641 +int modbus_rtu_set_serial_mode(modbus_t *ctx, int mode)
  642 +{
  643 + if (ctx == NULL) {
  644 + errno = EINVAL;
  645 + return -1;
  646 + }
  647 +
  648 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  649 +#if HAVE_DECL_TIOCSRS485
  650 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  651 + struct serial_rs485 rs485conf;
  652 + memset(&rs485conf, 0x0, sizeof(struct serial_rs485));
  653 +
  654 + if (mode == MODBUS_RTU_RS485) {
  655 + rs485conf.flags = SER_RS485_ENABLED;
  656 + if (ioctl(ctx->s, TIOCSRS485, &rs485conf) < 0) {
  657 + return -1;
  658 + }
  659 +
  660 + ctx_rtu->serial_mode = MODBUS_RTU_RS485;
  661 + return 0;
  662 + } else if (mode == MODBUS_RTU_RS232) {
  663 + /* Turn off RS485 mode only if required */
  664 + if (ctx_rtu->serial_mode == MODBUS_RTU_RS485) {
  665 + /* The ioctl call is avoided because it can fail on some RS232 ports */
  666 + if (ioctl(ctx->s, TIOCSRS485, &rs485conf) < 0) {
  667 + return -1;
  668 + }
  669 + }
  670 + ctx_rtu->serial_mode = MODBUS_RTU_RS232;
  671 + return 0;
  672 + }
  673 +#else
  674 + if (ctx->debug) {
  675 + fprintf(stderr, "This function isn't supported on your platform\n");
  676 + }
  677 + errno = ENOTSUP;
  678 + return -1;
  679 +#endif
  680 + }
  681 +
  682 + /* Wrong backend and invalid mode specified */
  683 + errno = EINVAL;
  684 + return -1;
  685 +}
  686 +
  687 +int modbus_rtu_get_serial_mode(modbus_t *ctx)
  688 +{
  689 + if (ctx == NULL) {
  690 + errno = EINVAL;
  691 + return -1;
  692 + }
  693 +
  694 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  695 +#if HAVE_DECL_TIOCSRS485
  696 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  697 + return ctx_rtu->serial_mode;
  698 +#else
  699 + if (ctx->debug) {
  700 + fprintf(stderr, "This function isn't supported on your platform\n");
  701 + }
  702 + errno = ENOTSUP;
  703 + return -1;
  704 +#endif
  705 + } else {
  706 + errno = EINVAL;
  707 + return -1;
  708 + }
  709 +}
  710 +
  711 +int modbus_rtu_get_rts(modbus_t *ctx)
  712 +{
  713 + if (ctx == NULL) {
  714 + errno = EINVAL;
  715 + return -1;
  716 + }
  717 +
  718 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  719 +#if HAVE_DECL_TIOCM_RTS
  720 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  721 + return ctx_rtu->rts;
  722 +#else
  723 + if (ctx->debug) {
  724 + fprintf(stderr, "This function isn't supported on your platform\n");
  725 + }
  726 + errno = ENOTSUP;
  727 + return -1;
  728 +#endif
  729 + } else {
  730 + errno = EINVAL;
  731 + return -1;
  732 + }
  733 +}
  734 +
  735 +int modbus_rtu_set_rts(modbus_t *ctx, int mode)
  736 +{
  737 + if (ctx == NULL) {
  738 + errno = EINVAL;
  739 + return -1;
  740 + }
  741 +
  742 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  743 +#if HAVE_DECL_TIOCM_RTS
  744 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  745 +
  746 + if (mode == MODBUS_RTU_RTS_NONE || mode == MODBUS_RTU_RTS_UP ||
  747 + mode == MODBUS_RTU_RTS_DOWN) {
  748 + ctx_rtu->rts = mode;
  749 +
  750 + /* Set the RTS bit in order to not reserve the RS485 bus */
  751 + ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
  752 +
  753 + return 0;
  754 + } else {
  755 + errno = EINVAL;
  756 + return -1;
  757 + }
  758 +#else
  759 + if (ctx->debug) {
  760 + fprintf(stderr, "This function isn't supported on your platform\n");
  761 + }
  762 + errno = ENOTSUP;
  763 + return -1;
  764 +#endif
  765 + }
  766 + /* Wrong backend or invalid mode specified */
  767 + errno = EINVAL;
  768 + return -1;
  769 +}
  770 +
  771 +int modbus_rtu_set_custom_rts(modbus_t *ctx, void (*set_rts) (modbus_t *ctx, int on))
  772 +{
  773 + if (ctx == NULL) {
  774 + errno = EINVAL;
  775 + return -1;
  776 + }
  777 +
  778 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  779 +#if HAVE_DECL_TIOCM_RTS
  780 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  781 + ctx_rtu->set_rts = set_rts;
  782 + return 0;
  783 +#else
  784 + if (ctx->debug) {
  785 + fprintf(stderr, "This function isn't supported on your platform\n");
  786 + }
  787 + errno = ENOTSUP;
  788 + return -1;
  789 +#endif
  790 + } else {
  791 + errno = EINVAL;
  792 + return -1;
  793 + }
  794 +}
  795 +
  796 +int modbus_rtu_get_rts_delay(modbus_t *ctx)
  797 +{
  798 + if (ctx == NULL) {
  799 + errno = EINVAL;
  800 + return -1;
  801 + }
  802 +
  803 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  804 +#if HAVE_DECL_TIOCM_RTS
  805 + modbus_rtu_t *ctx_rtu;
  806 + ctx_rtu = (modbus_rtu_t *)ctx->backend_data;
  807 + return ctx_rtu->rts_delay;
  808 +#else
  809 + if (ctx->debug) {
  810 + fprintf(stderr, "This function isn't supported on your platform\n");
  811 + }
  812 + errno = ENOTSUP;
  813 + return -1;
  814 +#endif
  815 + } else {
  816 + errno = EINVAL;
  817 + return -1;
  818 + }
  819 +}
  820 +
  821 +int modbus_rtu_set_rts_delay(modbus_t *ctx, int us)
  822 +{
  823 + if (ctx == NULL || us < 0) {
  824 + errno = EINVAL;
  825 + return -1;
  826 + }
  827 +
  828 + if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
  829 +#if HAVE_DECL_TIOCM_RTS
  830 + modbus_rtu_t *ctx_rtu;
  831 + ctx_rtu = (modbus_rtu_t *)ctx->backend_data;
  832 + ctx_rtu->rts_delay = us;
  833 + return 0;
  834 +#else
  835 + if (ctx->debug) {
  836 + fprintf(stderr, "This function isn't supported on your platform\n");
  837 + }
  838 + errno = ENOTSUP;
  839 + return -1;
  840 +#endif
  841 + } else {
  842 + errno = EINVAL;
  843 + return -1;
  844 + }
  845 +}
  846 +
  847 +static void _modbus_rtu_close(modbus_t *ctx)
  848 +{
  849 + /* Restore line settings and close file descriptor in RTU mode */
  850 + modbus_rtu_t *ctx_rtu = ctx->backend_data;
  851 +
  852 + if (ctx->s != -1) {
  853 + tcsetattr(ctx->s, TCSANOW, &ctx_rtu->old_tios);
  854 + close(ctx->s);
  855 + ctx->s = -1;
  856 + }
  857 +}
  858 +
  859 +static int _modbus_rtu_flush(modbus_t *ctx)
  860 +{
  861 + return tcflush(ctx->s, TCIOFLUSH);
  862 +}
  863 +
  864 +static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
  865 +{
  866 + int s_rc;
  867 + while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1)
  868 + {
  869 + if (errno == EINTR)
  870 + {
  871 + if (ctx->debug)
  872 + {
  873 + fprintf(stderr, "A non blocked signal was caught\n");
  874 + }
  875 + /* Necessary after an error */
  876 + FD_ZERO(rset);
  877 + FD_SET(ctx->s, rset);
  878 + }
  879 + else
  880 + {
  881 + return -1;
  882 + }
  883 + }
  884 +
  885 + if (s_rc == 0) {
  886 + /* Timeout */
  887 + errno = ETIMEDOUT;
  888 + return -1;
  889 + }
  890 +
  891 + return s_rc;
  892 +}
  893 +
  894 +static void _modbus_rtu_free(modbus_t *ctx) {
  895 + free(((modbus_rtu_t*)ctx->backend_data)->device);
  896 + free(ctx->backend_data);
  897 + free(ctx);
  898 +}
  899 +
  900 +const modbus_backend_t _modbus_rtu_backend = {
  901 + _MODBUS_BACKEND_TYPE_RTU,
  902 + _MODBUS_RTU_HEADER_LENGTH,
  903 + _MODBUS_RTU_CHECKSUM_LENGTH,
  904 + MODBUS_RTU_MAX_ADU_LENGTH,
  905 + _modbus_set_slave,
  906 + _modbus_rtu_build_request_basis,
  907 + _modbus_rtu_build_response_basis,
  908 + _modbus_rtu_prepare_response_tid,
  909 + _modbus_rtu_send_msg_pre,
  910 + _modbus_rtu_send,
  911 + _modbus_rtu_receive,
  912 + _modbus_rtu_recv,
  913 + _modbus_rtu_check_integrity,
  914 + _modbus_rtu_pre_check_confirmation,
  915 + _modbus_rtu_connect,
  916 + _modbus_rtu_close,
  917 + _modbus_rtu_flush,
  918 + _modbus_rtu_select,
  919 + _modbus_rtu_free
  920 +};
  921 +
  922 +modbus_t* modbus_new_rtu(const char *device,
  923 + int baud, char parity, int data_bit,
  924 + int stop_bit)
  925 +{
  926 + modbus_t *ctx;
  927 + modbus_rtu_t *ctx_rtu;
  928 +
  929 + /* Check device argument */
  930 + if (device == NULL || *device == 0) {
  931 + fprintf(stderr, "The device string is empty\n");
  932 + errno = EINVAL;
  933 + return NULL;
  934 + }
  935 +
  936 + /* Check baud argument */
  937 + if (baud == 0) {
  938 + fprintf(stderr, "The baud rate value must not be zero\n");
  939 + errno = EINVAL;
  940 + return NULL;
  941 + }
  942 +
  943 + ctx = (modbus_t *)malloc(sizeof(modbus_t));
  944 + _modbus_init_common(ctx);
  945 + ctx->backend = &_modbus_rtu_backend;
  946 + ctx->backend_data = (modbus_rtu_t *)malloc(sizeof(modbus_rtu_t));
  947 + ctx_rtu = (modbus_rtu_t *)ctx->backend_data;
  948 + ctx_rtu->device = NULL;
  949 +
  950 + /* Device name and \0 */
  951 + ctx_rtu->device = (char *)malloc((strlen(device) + 1) * sizeof(char));
  952 + strcpy(ctx_rtu->device, device);
  953 +
  954 + ctx_rtu->baud = baud;
  955 + if (parity == 'N' || parity == 'E' || parity == 'O') {
  956 + ctx_rtu->parity = parity;
  957 + } else {
  958 + modbus_free(ctx);
  959 + errno = EINVAL;
  960 + return NULL;
  961 + }
  962 + ctx_rtu->data_bit = data_bit;
  963 + ctx_rtu->stop_bit = stop_bit;
  964 +
  965 +#if HAVE_DECL_TIOCSRS485
  966 + /* The RS232 mode has been set by default */
  967 + ctx_rtu->serial_mode = MODBUS_RTU_RS232;
  968 +#endif
  969 +
  970 +#if HAVE_DECL_TIOCM_RTS
  971 + /* The RTS use has been set by default */
  972 + ctx_rtu->rts = MODBUS_RTU_RTS_NONE;
  973 +
  974 + /* Calculate estimated time in micro second to send one byte */
  975 + ctx_rtu->onebyte_time = 1000000 * (1 + data_bit + (parity == 'N' ? 0 : 1) + stop_bit) / baud;
  976 +
  977 + /* The internal function is used by default to set RTS */
  978 + ctx_rtu->set_rts = _modbus_rtu_ioctl_rts;
  979 +
  980 + /* The delay before and after transmission when toggling the RTS pin */
  981 + ctx_rtu->rts_delay = ctx_rtu->onebyte_time;
  982 +#endif
  983 +
  984 + ctx_rtu->confirmation_to_ignore = FALSE;
  985 +
  986 + return ctx;
  987 +}
... ...
stack/modbus-rtu.h 0 → 100644
  View file @5735b40
  1 +
  2 +#pragma once
  3 +
  4 +#include "modbus.h"
  5 +
  6 +MODBUS_BEGIN_DECLS
  7 +
  8 +/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
  9 + * RS232 / RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes
  10 + */
  11 +#define MODBUS_RTU_MAX_ADU_LENGTH 256
  12 +
  13 +MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity, int data_bit, int stop_bit);
  14 +
  15 +#define MODBUS_RTU_RS232 0
  16 +#define MODBUS_RTU_RS485 1
  17 +
  18 +MODBUS_API int modbus_rtu_set_serial_mode( modbus_t *ctx, int mode );
  19 +MODBUS_API int modbus_rtu_get_serial_mode( modbus_t *ctx );
  20 +
  21 +#define MODBUS_RTU_RTS_NONE 0
  22 +#define MODBUS_RTU_RTS_UP 1
  23 +#define MODBUS_RTU_RTS_DOWN 2
  24 +
  25 +MODBUS_API int modbus_rtu_set_rts( modbus_t *ctx, int mode );
  26 +MODBUS_API int modbus_rtu_get_rts( modbus_t *ctx );
  27 +
  28 +MODBUS_API int modbus_rtu_set_custom_rts( modbus_t *ctx, void ( *set_rts ) ( modbus_t *ctx, int on ) );
  29 +
  30 +MODBUS_API int modbus_rtu_set_rts_delay( modbus_t *ctx, int us );
  31 +MODBUS_API int modbus_rtu_get_rts_delay( modbus_t *ctx );
  32 +
  33 +MODBUS_END_DECLS
  34 +
... ...
stack/modbus-tcp-private.h 0 → 100644
  View file @5735b40
  1 +#pragma once
  2 +
  3 +#define _MODBUS_TCP_HEADER_LENGTH 7
  4 +#define _MODBUS_TCP_PRESET_REQ_LENGTH 12
  5 +#define _MODBUS_TCP_PRESET_RSP_LENGTH 8
  6 +
  7 +#define _MODBUS_TCP_CHECKSUM_LENGTH 0
  8 +
  9 +/* In both structures, the transaction ID must be placed on first position
  10 + to have a quick access not dependant of the TCP backend */
  11 +typedef struct _modbus_tcp
  12 +{
  13 + /* Extract from MODBUS Messaging on TCP/IP Implementation Guide V1.0b
  14 + (page 23/46):
  15 + The transaction identifier is used to associate the future response
  16 + with the request. This identifier is unique on each TCP connection. */
  17 + uint16_t t_id;
  18 + /* TCP port */
  19 + int port;
  20 + /* IP address */
  21 + char ip[16];
  22 +} modbus_tcp_t;
  23 +
  24 +#define _MODBUS_TCP_PI_NODE_LENGTH 1025
  25 +#define _MODBUS_TCP_PI_SERVICE_LENGTH 32
  26 +
  27 +typedef struct _modbus_tcp_pi
  28 +{
  29 + /* Transaction ID */
  30 + uint16_t t_id;
  31 + /* TCP port */
  32 + int port;
  33 + /* Node */
  34 + char node[_MODBUS_TCP_PI_NODE_LENGTH];
  35 + /* Service */
  36 + char service[_MODBUS_TCP_PI_SERVICE_LENGTH];
  37 +} modbus_tcp_pi_t;
  38 +
  39 +
... ...
stack/modbus-tcp.c 0 → 100644
  View file @5735b40
  1 +
  2 +#include <stdio.h>
  3 +#include <stdlib.h>
  4 +#include <string.h>
  5 +#include <errno.h>
  6 +#include <unistd.h>
  7 +#include <signal.h>
  8 +#include <sys/types.h>
  9 +
  10 +# include <sys/socket.h>
  11 +# include <sys/ioctl.h>
  12 +
  13 +#if defined(__OpenBSD__) || (defined(__FreeBSD__) && __FreeBSD__ < 5)
  14 +# define OS_BSD
  15 +# include <netinet/in_systm.h>
  16 +#endif
  17 +
  18 +# include <netinet/in.h>
  19 +# include <netinet/ip.h>
  20 +# include <netinet/tcp.h>
  21 +# include <arpa/inet.h>
  22 +# include <netdb.h>
  23 +
  24 +#if !defined(MSG_NOSIGNAL)
  25 +#define MSG_NOSIGNAL 0
  26 +#endif
  27 +
  28 +#include "modbus-private.h"
  29 +
  30 +#include "modbus-tcp.h"
  31 +#include "modbus-tcp-private.h"
  32 +
  33 +static int _modbus_set_slave(modbus_t *ctx, int slave)
  34 +{
  35 + /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
  36 + if ( slave >= 0 && slave <= 247)
  37 + {
  38 + ctx->slave = slave;
  39 + }
  40 + else if ( slave == MODBUS_TCP_SLAVE )
  41 + {
  42 + /* The special value MODBUS_TCP_SLAVE (0xFF) can be used in TCP mode to
  43 + * restore the default value. */
  44 + ctx->slave = slave;
  45 + }
  46 + else
  47 + {
  48 + errno = EINVAL;
  49 + return -1;
  50 + }
  51 +
  52 + return 0;
  53 +}
  54 +
  55 +/* Builds a TCP request header */
  56 +static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
  57 +{
  58 + modbus_tcp_t *ctx_tcp = ctx->backend_data;
  59 +
  60 + /* Increase transaction ID */
  61 + if (ctx_tcp->t_id < UINT16_MAX)
  62 + ctx_tcp->t_id++;
  63 + else
  64 + ctx_tcp->t_id = 0;
  65 +
  66 + req[0] = ctx_tcp->t_id >> 8;
  67 + req[1] = ctx_tcp->t_id & 0x00ff;
  68 +
  69 + /* Protocol Modbus */
  70 + req[2] = 0;
  71 + req[3] = 0;
  72 +
  73 + /* Length will be defined later by set_req_length_tcp at offsets 4
  74 + and 5 */
  75 +
  76 + req[6] = ctx->slave;
  77 + req[7] = function;
  78 + req[8] = addr >> 8;
  79 + req[9] = addr & 0x00ff;
  80 + req[10] = nb >> 8;
  81 + req[11] = nb & 0x00ff;
  82 +
  83 + return _MODBUS_TCP_PRESET_REQ_LENGTH;
  84 +}
  85 +
  86 +/* Builds a TCP response header */
  87 +static int _modbus_tcp_build_response_basis( sft_t *sft, uint8_t *rsp )
  88 +{
  89 + /* Extract from MODBUS Messaging on TCP/IP Implementation
  90 + Guide V1.0b (page 23/46):
  91 + The transaction identifier is used to associate the future
  92 + response with the request. */
  93 + rsp[0] = sft->t_id >> 8;
  94 + rsp[1] = sft->t_id & 0x00ff;
  95 +
  96 + /* Protocol Modbus */
  97 + rsp[2] = 0;
  98 + rsp[3] = 0;
  99 +
  100 + /* Length will be set later by send_msg (4 and 5) */
  101 +
  102 + /* The slave ID is copied from the indication */
  103 + rsp[6] = sft->slave;
  104 + rsp[7] = sft->function;
  105 +
  106 + return _MODBUS_TCP_PRESET_RSP_LENGTH;
  107 +}
  108 +
  109 +
  110 +static int _modbus_tcp_prepare_response_tid( const uint8_t *req, int *req_length )
  111 +{
  112 + return (req[0] << 8) + req[1];
  113 +}
  114 +
  115 +static int _modbus_tcp_send_msg_pre( uint8_t *req, int req_length )
  116 +{
  117 + /* Substract the header length to the message length */
  118 + int mbap_length = req_length - 6;
  119 +
  120 + req[4] = mbap_length >> 8;
  121 + req[5] = mbap_length & 0x00FF;
  122 +
  123 + return req_length;
  124 +}
  125 +
  126 +static ssize_t _modbus_tcp_send( modbus_t *ctx, const uint8_t *req, int req_length )
  127 +{
  128 + /* MSG_NOSIGNAL
  129 + Requests not to send SIGPIPE on errors on stream oriented
  130 + sockets when the other end breaks the connection. The EPIPE
  131 + error is still returned. */
  132 + return send(ctx->s, (const char *)req, req_length, MSG_NOSIGNAL);
  133 +}
  134 +
  135 +static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req)
  136 +{
  137 + return _modbus_receive_msg(ctx, req, MSG_INDICATION);
  138 +}
  139 +
  140 +static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
  141 +{
  142 + return recv( ctx->s, (char *)rsp, rsp_length, 0 );
  143 +}
  144 +
  145 +static int _modbus_tcp_check_integrity( modbus_t *ctx, uint8_t *msg, const int msg_length )
  146 +{
  147 + return msg_length;
  148 +}
  149 +
  150 +static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length )
  151 +{
  152 + /* Check transaction ID */
  153 + if (req[0] != rsp[0] || req[1] != rsp[1]) {
  154 + if (ctx->debug)
  155 + {
  156 + fprintf(stderr, "Invalid transaction ID received 0x%X (not 0x%X)\n",
  157 + (rsp[0] << 8) + rsp[1], (req[0] << 8) + req[1]);
  158 + }
  159 + errno = EMBBADDATA;
  160 + return -1;
  161 + }
  162 +
  163 + /* Check protocol ID */
  164 + if (rsp[2] != 0x0 && rsp[3] != 0x0)
  165 + {
  166 + if (ctx->debug)
  167 + {
  168 + fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n",
  169 + (rsp[2] << 8) + rsp[3]);
  170 + }
  171 + errno = EMBBADDATA;
  172 + return -1;
  173 + }
  174 +
  175 + return 0;
  176 +}
  177 +
  178 +static int _modbus_tcp_set_ipv4_options( int s )
  179 +{
  180 + int rc;
  181 + int option;
  182 +
  183 + /* Set the TCP no delay flag */
  184 + /* SOL_TCP = IPPROTO_TCP */
  185 + option = 1;
  186 + rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (const void *)&option, sizeof(int) );
  187 + if (rc == -1)
  188 + {
  189 + return -1;
  190 + }
  191 +
  192 + /* If the OS does not offer SOCK_NONBLOCK, fall back to setting FIONBIO to
  193 + * make sockets non-blocking */
  194 + /* Do not care about the return value, this is optional */
  195 +#if !defined(SOCK_NONBLOCK) && defined(FIONBIO)
  196 + option = 1;
  197 + ioctl(s, FIONBIO, &option);
  198 +#endif
  199 +
  200 + /**
  201 + * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's
  202 + * necessary to workaround that problem.
  203 + **/
  204 + /* Set the IP low delay option */
  205 + option = IPTOS_LOWDELAY;
  206 + rc = setsockopt(s, IPPROTO_IP, IP_TOS,
  207 + (const void *)&option, sizeof(int));
  208 + if (rc == -1) {
  209 + return -1;
  210 + }
  211 +
  212 + return 0;
  213 +}
  214 +
  215 +static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen, const struct timeval *ro_tv )
  216 +{
  217 + int rc = connect(sockfd, addr, addrlen);
  218 +
  219 + if (rc == -1 && errno == EINPROGRESS)
  220 + {
  221 + fd_set wset;
  222 + int optval;
  223 + socklen_t optlen = sizeof(optval);
  224 + struct timeval tv = *ro_tv;
  225 +
  226 + /* Wait to be available in writing */
  227 + FD_ZERO(&wset);
  228 + FD_SET(sockfd, &wset);
  229 + rc = select(sockfd + 1, NULL, &wset, NULL, &tv);
  230 + if (rc <= 0)
  231 + {
  232 + /* Timeout or fail */
  233 + return -1;
  234 + }
  235 +
  236 + /* The connection is established if SO_ERROR and optval are set to 0 */
  237 + rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *)&optval, &optlen);
  238 + if ( rc == 0 && optval == 0 )
  239 + {
  240 + return 0;
  241 + }
  242 + else
  243 + {
  244 + errno = ECONNREFUSED;
  245 + return -1;
  246 + }
  247 + }
  248 + return rc;
  249 +}
  250 +
  251 +/* Establishes a modbus TCP connection with a Modbus server. */
  252 +static int _modbus_tcp_connect( modbus_t *ctx )
  253 +{
  254 + int rc;
  255 + /* Specialized version of sockaddr for Internet socket address (same size) */
  256 + struct sockaddr_in addr;
  257 + modbus_tcp_t *ctx_tcp = ctx->backend_data;
  258 + int flags = SOCK_STREAM;
  259 +
  260 +#ifdef SOCK_CLOEXEC
  261 + flags |= SOCK_CLOEXEC;
  262 +#endif
  263 +
  264 +#ifdef SOCK_NONBLOCK
  265 + flags |= SOCK_NONBLOCK;
  266 +#endif
  267 +
  268 + ctx->s = socket(PF_INET, flags, 0);
  269 + if (ctx->s == -1)
  270 + {
  271 + return -1;
  272 + }
  273 +
  274 + rc = _modbus_tcp_set_ipv4_options(ctx->s);
  275 + if (rc == -1)
  276 + {
  277 + close(ctx->s);
  278 + ctx->s = -1;
  279 + return -1;
  280 + }
  281 +
  282 + if (ctx->debug)
  283 + {
  284 + printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port);
  285 + }
  286 +
  287 + addr.sin_family = AF_INET;
  288 + addr.sin_port = htons(ctx_tcp->port);
  289 + addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
  290 + rc = _connect(ctx->s, (struct sockaddr *)&addr, sizeof(addr), &ctx->response_timeout);
  291 + if( rc == -1 )
  292 + {
  293 + close( ctx->s );
  294 + ctx->s = -1;
  295 + return -1;
  296 + }
  297 +
  298 + return 0;
  299 +}
  300 +
  301 +/* Establishes a modbus TCP PI connection with a Modbus server. */
  302 +static int _modbus_tcp_pi_connect( modbus_t *ctx )
  303 +{
  304 + int rc;
  305 + struct addrinfo *ai_list;
  306 + struct addrinfo *ai_ptr;
  307 + struct addrinfo ai_hints;
  308 + modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
  309 +
  310 + memset(&ai_hints, 0, sizeof(ai_hints));
  311 +#ifdef AI_ADDRCONFIG
  312 + ai_hints.ai_flags |= AI_ADDRCONFIG;
  313 +#endif
  314 + ai_hints.ai_family = AF_UNSPEC;
  315 + ai_hints.ai_socktype = SOCK_STREAM;
  316 + ai_hints.ai_addr = NULL;
  317 + ai_hints.ai_canonname = NULL;
  318 + ai_hints.ai_next = NULL;
  319 +
  320 + ai_list = NULL;
  321 + rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service, &ai_hints, &ai_list);
  322 + if (rc != 0)
  323 + {
  324 + if (ctx->debug)
  325 + {
  326 + fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
  327 + }
  328 + errno = ECONNREFUSED;
  329 + return -1;
  330 + }
  331 +
  332 + for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next)
  333 + {
  334 + int flags = ai_ptr->ai_socktype;
  335 + int s;
  336 +
  337 +#ifdef SOCK_CLOEXEC
  338 + flags |= SOCK_CLOEXEC;
  339 +#endif
  340 +
  341 +#ifdef SOCK_NONBLOCK
  342 + flags |= SOCK_NONBLOCK;
  343 +#endif
  344 +
  345 + s = socket(ai_ptr->ai_family, flags, ai_ptr->ai_protocol);
  346 + if (s < 0)
  347 + continue;
  348 +
  349 + if (ai_ptr->ai_family == AF_INET)
  350 + _modbus_tcp_set_ipv4_options(s);
  351 +
  352 + if (ctx->debug)
  353 + {
  354 + printf("Connecting to [%s]:%s\n", ctx_tcp_pi->node, ctx_tcp_pi->service);
  355 + }
  356 +
  357 + rc = _connect(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen, &ctx->response_timeout);
  358 + if (rc == -1)
  359 + {
  360 + close(s);
  361 + continue;
  362 + }
  363 +
  364 + ctx->s = s;
  365 + break;
  366 + }
  367 +
  368 + freeaddrinfo(ai_list);
  369 +
  370 + if (ctx->s < 0)
  371 + {
  372 + return -1;
  373 + }
  374 +
  375 + return 0;
  376 +}
  377 +
  378 +/* Closes the network connection and socket in TCP mode */
  379 +static void _modbus_tcp_close( modbus_t *ctx )
  380 +{
  381 + if (ctx->s != -1)
  382 + {
  383 + shutdown(ctx->s, SHUT_RDWR);
  384 + close(ctx->s);
  385 + ctx->s = -1;
  386 + }
  387 +}
  388 +
  389 +static int _modbus_tcp_flush( modbus_t *ctx )
  390 +{
  391 + int rc;
  392 + int rc_sum = 0;
  393 +
  394 + do
  395 + {
  396 + /* Extract the garbage from the socket */
  397 + char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
  398 +
  399 + rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
  400 + if ( rc > 0 )
  401 + {
  402 + rc_sum += rc;
  403 + }
  404 + } while ( rc == MODBUS_TCP_MAX_ADU_LENGTH );
  405 +
  406 + return rc_sum;
  407 +}
  408 +
  409 +/* Listens for any request from one or many modbus masters in TCP */
  410 +int modbus_tcp_listen( modbus_t *ctx, int nb_connection )
  411 +{
  412 + int new_s;
  413 + int enable;
  414 + struct sockaddr_in addr;
  415 + modbus_tcp_t *ctx_tcp;
  416 +
  417 + if (ctx == NULL)
  418 + {
  419 + errno = EINVAL;
  420 + return -1;
  421 + }
  422 +
  423 + ctx_tcp = ctx->backend_data;
  424 +
  425 + new_s = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
  426 + if ( new_s == -1 )
  427 + {
  428 + return -1;
  429 + }
  430 +
  431 + enable = 1;
  432 + if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR, (char *)&enable, sizeof(enable)) == -1)
  433 + {
  434 + close(new_s);
  435 + return -1;
  436 + }
  437 +
  438 + memset(&addr, 0, sizeof(addr));
  439 + addr.sin_family = AF_INET;
  440 +
  441 + /* If the modbus port is < to 1024, we need the setuid root. */
  442 + addr.sin_port = htons(ctx_tcp->port);
  443 + if (ctx_tcp->ip[0] == '0')
  444 + {
  445 + /* Listen any addresses */
  446 + addr.sin_addr.s_addr = htonl(INADDR_ANY);
  447 + }
  448 + else
  449 + {
  450 + /* Listen only specified IP address */
  451 + addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
  452 + }
  453 + if (bind( new_s, (struct sockaddr *)&addr, sizeof(addr)) == -1)
  454 + {
  455 + close(new_s);
  456 + return -1;
  457 + }
  458 +
  459 + if (listen(new_s, nb_connection) == -1)
  460 + {
  461 + close(new_s);
  462 + return -1;
  463 + }
  464 +
  465 + return new_s;
  466 +}
  467 +
  468 +int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
  469 +{
  470 + int rc;
  471 + struct addrinfo *ai_list;
  472 + struct addrinfo *ai_ptr;
  473 + struct addrinfo ai_hints;
  474 + const char *node;
  475 + const char *service;
  476 + int new_s;
  477 + modbus_tcp_pi_t *ctx_tcp_pi;
  478 +
  479 + if (ctx == NULL)
  480 + {
  481 + errno = EINVAL;
  482 + return -1;
  483 + }
  484 +
  485 + ctx_tcp_pi = ctx->backend_data;
  486 +
  487 + if ( ctx_tcp_pi->node[0] == 0)
  488 + {
  489 + node = NULL; /* == any */
  490 + }
  491 + else
  492 + {
  493 + node = ctx_tcp_pi->node;
  494 + }
  495 +
  496 + if ( ctx_tcp_pi->service[0] == 0 )
  497 + {
  498 + service = "502";
  499 + }
  500 + else
  501 + {
  502 + service = ctx_tcp_pi->service;
  503 + }
  504 +
  505 + memset( &ai_hints, 0, sizeof (ai_hints) );
  506 + /* If node is not NULL, than the AI_PASSIVE flag is ignored. */
  507 + ai_hints.ai_flags |= AI_PASSIVE;
  508 +#ifdef AI_ADDRCONFIG
  509 + ai_hints.ai_flags |= AI_ADDRCONFIG;
  510 +#endif
  511 + ai_hints.ai_family = AF_UNSPEC;
  512 + ai_hints.ai_socktype = SOCK_STREAM;
  513 + ai_hints.ai_addr = NULL;
  514 + ai_hints.ai_canonname = NULL;
  515 + ai_hints.ai_next = NULL;
  516 +
  517 + ai_list = NULL;
  518 + rc = getaddrinfo(node, service, &ai_hints, &ai_list);
  519 + if (rc != 0)
  520 + {
  521 + if (ctx->debug)
  522 + {
  523 + fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
  524 + }
  525 + errno = ECONNREFUSED;
  526 + return -1;
  527 + }
  528 +
  529 + new_s = -1;
  530 + for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next)
  531 + {
  532 + int s;
  533 +
  534 + s = socket( ai_ptr->ai_family, ai_ptr->ai_socktype, ai_ptr->ai_protocol );
  535 + if (s < 0)
  536 + {
  537 + if (ctx->debug)
  538 + {
  539 + perror("socket");
  540 + }
  541 + continue;
  542 + }
  543 + else
  544 + {
  545 + int enable = 1;
  546 + rc = setsockopt( s, SOL_SOCKET, SO_REUSEADDR, (void *)&enable, sizeof (enable) );
  547 + if (rc != 0)
  548 + {
  549 + close(s);
  550 + if (ctx->debug)
  551 + {
  552 + perror("setsockopt");
  553 + }
  554 + continue;
  555 + }
  556 + }
  557 +
  558 + rc = bind(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen);
  559 + if (rc != 0)
  560 + {
  561 + close(s);
  562 + if (ctx->debug)
  563 + {
  564 + perror("bind");
  565 + }
  566 + continue;
  567 + }
  568 +
  569 + rc = listen(s, nb_connection);
  570 + if (rc != 0)
  571 + {
  572 + close(s);
  573 + if (ctx->debug)
  574 + {
  575 + perror("listen");
  576 + }
  577 + continue;
  578 + }
  579 +
  580 + new_s = s;
  581 + break;
  582 + }
  583 + freeaddrinfo(ai_list);
  584 +
  585 + if (new_s < 0)
  586 + {
  587 + return -1;
  588 + }
  589 +
  590 + return new_s;
  591 +}
  592 +
  593 +int modbus_tcp_accept(modbus_t *ctx, int *s)
  594 +{
  595 + struct sockaddr_in addr;
  596 + socklen_t addrlen;
  597 +
  598 + if (ctx == NULL)
  599 + {
  600 + errno = EINVAL;
  601 + return -1;
  602 + }
  603 +
  604 + addrlen = sizeof(addr);
  605 +#ifdef HAVE_ACCEPT4
  606 + /* Inherit socket flags and use accept4 call */
  607 + ctx->s = accept4(*s, (struct sockaddr *)&addr, &addrlen, SOCK_CLOEXEC);
  608 +#else
  609 + ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
  610 +#endif
  611 +
  612 + if (ctx->s == -1)
  613 + {
  614 + close(*s);
  615 + *s = -1;
  616 + return -1;
  617 + }
  618 +
  619 + if (ctx->debug)
  620 + {
  621 + printf("The client connection from %s is accepted\n",
  622 + inet_ntoa(addr.sin_addr));
  623 + }
  624 +
  625 + return ctx->s;
  626 +}
  627 +
  628 +int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
  629 +{
  630 + struct sockaddr_storage addr;
  631 + socklen_t addrlen;
  632 +
  633 + if (ctx == NULL)
  634 + {
  635 + errno = EINVAL;
  636 + return -1;
  637 + }
  638 +
  639 + addrlen = sizeof(addr);
  640 +#ifdef HAVE_ACCEPT4
  641 + /* Inherit socket flags and use accept4 call */
  642 + ctx->s = accept4(*s, (struct sockaddr *)&addr, &addrlen, SOCK_CLOEXEC);
  643 +#else
  644 + ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
  645 +#endif
  646 + if (ctx->s == -1)
  647 + {
  648 + close(*s);
  649 + *s = -1;
  650 + }
  651 +
  652 + if (ctx->debug)
  653 + {
  654 + printf("The client connection is accepted.\n");
  655 + }
  656 +
  657 + return ctx->s;
  658 +}
  659 +
  660 +static int _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
  661 +{
  662 + int s_rc;
  663 + while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1)
  664 + {
  665 + if (errno == EINTR)
  666 + {
  667 + if (ctx->debug)
  668 + {
  669 + fprintf(stderr, "A non blocked signal was caught\n");
  670 + }
  671 + /* Necessary after an error */
  672 + FD_ZERO(rset);
  673 + FD_SET(ctx->s, rset);
  674 + }
  675 + else
  676 + {
  677 + return -1;
  678 + }
  679 + }
  680 +
  681 + if ( s_rc == 0 )
  682 + {
  683 + errno = ETIMEDOUT;
  684 + return -1;
  685 + }
  686 +
  687 + return s_rc;
  688 +}
  689 +
  690 +static void _modbus_tcp_free( modbus_t *ctx )
  691 +{
  692 + free(ctx->backend_data);
  693 + free(ctx);
  694 +}
  695 +
  696 +const modbus_backend_t _modbus_tcp_backend =
  697 +{
  698 + _MODBUS_BACKEND_TYPE_TCP,
  699 + _MODBUS_TCP_HEADER_LENGTH,
  700 + _MODBUS_TCP_CHECKSUM_LENGTH,
  701 + MODBUS_TCP_MAX_ADU_LENGTH,
  702 + _modbus_set_slave,
  703 + _modbus_tcp_build_request_basis,
  704 + _modbus_tcp_build_response_basis,
  705 + _modbus_tcp_prepare_response_tid,
  706 + _modbus_tcp_send_msg_pre,
  707 + _modbus_tcp_send,
  708 + _modbus_tcp_receive,
  709 + _modbus_tcp_recv,
  710 + _modbus_tcp_check_integrity,
  711 + _modbus_tcp_pre_check_confirmation,
  712 + _modbus_tcp_connect,
  713 + _modbus_tcp_close,
  714 + _modbus_tcp_flush,
  715 + _modbus_tcp_select,
  716 + _modbus_tcp_free
  717 +};
  718 +
  719 +
  720 +const modbus_backend_t _modbus_tcp_pi_backend =
  721 +{
  722 + _MODBUS_BACKEND_TYPE_TCP,
  723 + _MODBUS_TCP_HEADER_LENGTH,
  724 + _MODBUS_TCP_CHECKSUM_LENGTH,
  725 + MODBUS_TCP_MAX_ADU_LENGTH,
  726 + _modbus_set_slave,
  727 + _modbus_tcp_build_request_basis,
  728 + _modbus_tcp_build_response_basis,
  729 + _modbus_tcp_prepare_response_tid,
  730 + _modbus_tcp_send_msg_pre,
  731 + _modbus_tcp_send,
  732 + _modbus_tcp_receive,
  733 + _modbus_tcp_recv,
  734 + _modbus_tcp_check_integrity,
  735 + _modbus_tcp_pre_check_confirmation,
  736 + _modbus_tcp_pi_connect,
  737 + _modbus_tcp_close,
  738 + _modbus_tcp_flush,
  739 + _modbus_tcp_select,
  740 + _modbus_tcp_free
  741 +};
  742 +
  743 +modbus_t* modbus_new_tcp( const char *ip, int port )
  744 +{
  745 + modbus_t *ctx;
  746 + modbus_tcp_t *ctx_tcp;
  747 + size_t dest_size;
  748 + size_t ret_size;
  749 +
  750 + ctx = (modbus_t *)malloc(sizeof(modbus_t) );
  751 + _modbus_init_common(ctx);
  752 +
  753 + /* Could be changed after to reach a remote serial Modbus device */
  754 + ctx->slave = MODBUS_TCP_SLAVE;
  755 +
  756 + ctx->backend = &_modbus_tcp_backend;
  757 +
  758 + ctx->backend_data = ( modbus_tcp_t *)malloc(sizeof(modbus_tcp_t) );
  759 + ctx_tcp = (modbus_tcp_t *)ctx->backend_data;
  760 +
  761 + if (ip != NULL)
  762 + {
  763 + dest_size = sizeof(char) * 16;
  764 + ret_size = strlcpy(ctx_tcp->ip, ip, dest_size);
  765 + if (ret_size == 0)
  766 + {
  767 + fprintf(stderr, "The IP string is empty\n");
  768 + modbus_free(ctx);
  769 + errno = EINVAL;
  770 + return NULL;
  771 + }
  772 +
  773 + if (ret_size >= dest_size)
  774 + {
  775 + fprintf(stderr, "The IP string has been truncated\n");
  776 + modbus_free(ctx);
  777 + errno = EINVAL;
  778 + return NULL;
  779 + }
  780 + }
  781 + else
  782 + {
  783 + ctx_tcp->ip[0] = '0';
  784 + }
  785 + ctx_tcp->port = port;
  786 + ctx_tcp->t_id = 0;
  787 +
  788 + return ctx;
  789 +}
  790 +
  791 +
  792 +modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
  793 +{
  794 + modbus_t *ctx;
  795 + modbus_tcp_pi_t *ctx_tcp_pi;
  796 + size_t dest_size;
  797 + size_t ret_size;
  798 +
  799 + ctx = (modbus_t *)malloc(sizeof(modbus_t));
  800 + _modbus_init_common(ctx);
  801 +
  802 + /* Could be changed after to reach a remote serial Modbus device */
  803 + ctx->slave = MODBUS_TCP_SLAVE;
  804 +
  805 + ctx->backend = &_modbus_tcp_pi_backend;
  806 +
  807 + ctx->backend_data = (modbus_tcp_pi_t *)malloc(sizeof(modbus_tcp_pi_t));
  808 + ctx_tcp_pi = (modbus_tcp_pi_t *)ctx->backend_data;
  809 +
  810 + if (node == NULL)
  811 + {
  812 + /* The node argument can be empty to indicate any hosts */
  813 + ctx_tcp_pi->node[0] = 0;
  814 + }
  815 + else
  816 + {
  817 + dest_size = sizeof(char) * _MODBUS_TCP_PI_NODE_LENGTH;
  818 + ret_size = strlcpy(ctx_tcp_pi->node, node, dest_size);
  819 + if (ret_size == 0)
  820 + {
  821 + fprintf(stderr, "The node string is empty\n");
  822 + modbus_free(ctx);
  823 + errno = EINVAL;
  824 + return NULL;
  825 + }
  826 +
  827 + if (ret_size >= dest_size)
  828 + {
  829 + fprintf(stderr, "The node string has been truncated\n");
  830 + modbus_free(ctx);
  831 + errno = EINVAL;
  832 + return NULL;
  833 + }
  834 + }
  835 +
  836 + if (service != NULL)
  837 + {
  838 + dest_size = sizeof(char) * _MODBUS_TCP_PI_SERVICE_LENGTH;
  839 + ret_size = strlcpy(ctx_tcp_pi->service, service, dest_size);
  840 + }
  841 + else
  842 + {
  843 + /* Empty service is not allowed, error catched below. */
  844 + ret_size = 0;
  845 + }
  846 +
  847 + if (ret_size == 0)
  848 + {
  849 + fprintf(stderr, "The service string is empty\n");
  850 + modbus_free(ctx);
  851 + errno = EINVAL;
  852 + return NULL;
  853 + }
  854 +
  855 + if (ret_size >= dest_size)
  856 + {
  857 + fprintf(stderr, "The service string has been truncated\n");
  858 + modbus_free(ctx);
  859 + errno = EINVAL;
  860 + return NULL;
  861 + }
  862 +
  863 + ctx_tcp_pi->t_id = 0;
  864 +
  865 + return ctx;
  866 +}
... ...
stack/modbus-tcp.h 0 → 100644
  View file @5735b40
  1 +#pragma once
  2 +
  3 +#include "modbus.h"
  4 +
  5 +MODBUS_BEGIN_DECLS
  6 +
  7 +#define MODBUS_TCP_DEFAULT_PORT 502
  8 +#define MODBUS_TCP_SLAVE 0xFF
  9 +
  10 +/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
  11 + * TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes
  12 + */
  13 +#define MODBUS_TCP_MAX_ADU_LENGTH 260
  14 +
  15 +MODBUS_API modbus_t* modbus_new_tcp(const char *ip_address, int port);
  16 +MODBUS_API int modbus_tcp_listen(modbus_t *ctx, int nb_connection);
  17 +MODBUS_API int modbus_tcp_accept(modbus_t *ctx, int *s);
  18 +
  19 +MODBUS_API modbus_t* modbus_new_tcp_pi(const char *node, const char *service);
  20 +MODBUS_API int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection);
  21 +MODBUS_API int modbus_tcp_pi_accept(modbus_t *ctx, int *s);
  22 +
  23 +MODBUS_END_DECLS
  24 +
... ...
stack/modbus-version.h 0 → 100644
  View file @5735b40
  1 +#pragma once
  2 +
  3 +/* The major version, (1, if %LIBMODBUS_VERSION is 1.2.3) */
  4 +#define LIBMODBUS_VERSION_MAJOR (3)
  5 +
  6 +/* The minor version (2, if %LIBMODBUS_VERSION is 1.2.3) */
  7 +#define LIBMODBUS_VERSION_MINOR (1)
  8 +
  9 +/* The micro version (3, if %LIBMODBUS_VERSION is 1.2.3) */
  10 +#define LIBMODBUS_VERSION_MICRO (4)
  11 +
  12 +/* The full version, like 1.2.3 */
  13 +#define LIBMODBUS_VERSION 3.1.4
  14 +
  15 +/* The full version, in string form (suited for string concatenation)
  16 + */
  17 +#define LIBMODBUS_VERSION_STRING "3.1.4"
  18 +
  19 +/* Numerically encoded version, like 0x010203 */
  20 +#define LIBMODBUS_VERSION_HEX ((LIBMODBUS_VERSION_MAJOR << 24) | \
  21 + (LIBMODBUS_VERSION_MINOR << 16) | \
  22 + (LIBMODBUS_VERSION_MICRO << 8))
  23 +
  24 +/* Evaluates to True if the version is greater than @major, @minor and @micro
  25 + */
  26 +#define LIBMODBUS_VERSION_CHECK(major,minor,micro) \
  27 + (LIBMODBUS_VERSION_MAJOR > (major) || \
  28 + (LIBMODBUS_VERSION_MAJOR == (major) && \
  29 + LIBMODBUS_VERSION_MINOR > (minor)) || \
  30 + (LIBMODBUS_VERSION_MAJOR == (major) && \
  31 + LIBMODBUS_VERSION_MINOR == (minor) && \
  32 + LIBMODBUS_VERSION_MICRO >= (micro)))
  33 +
  34 +
... ...
stack/modbus.c 0 → 100644
  View file @5735b40
  1 +#include <config.h>
  2 +#include <errno.h>
  3 +#include <limits.h>
  4 +#include <stdio.h>
  5 +#include <string.h>
  6 +#include <stdlib.h>
  7 +#include <stdarg.h>
  8 +#include <time.h>
  9 +#include <unistd.h>
  10 +
  11 +#include "modbus.h"
  12 +#include "modbus-private.h"
  13 +
  14 +/* Internal use */
  15 +#define MSG_LENGTH_UNDEFINED -1
  16 +
  17 +/* Max between RTU and TCP max adu length (so TCP) */
  18 +#define MAX_MESSAGE_LENGTH 260
  19 +
  20 +/* 3 steps are used to parse the query */
  21 +typedef enum
  22 +{
  23 + _STEP_FUNCTION,
  24 + _STEP_META,
  25 + _STEP_DATA
  26 +} _step_t;
  27 +
  28 +const char *modbus_strerror( int errnum )
  29 +{
  30 + switch (errnum)
  31 + {
  32 + case EMBXILFUN:
  33 + return "Illegal function";
  34 + case EMBXILADD:
  35 + return "Illegal data address";
  36 + case EMBXILVAL:
  37 + return "Illegal data value";
  38 + case EMBXSFAIL:
  39 + return "Slave device or server failure";
  40 + case EMBXACK:
  41 + return "Acknowledge";
  42 + case EMBXSBUSY:
  43 + return "Slave device or server is busy";
  44 + case EMBXNACK:
  45 + return "Negative acknowledge";
  46 + case EMBXMEMPAR:
  47 + return "Memory parity error";
  48 + case EMBXGPATH:
  49 + return "Gateway path unavailable";
  50 + case EMBXGTAR:
  51 + return "Target device failed to respond";
  52 + case EMBBADCRC:
  53 + return "Invalid CRC";
  54 + case EMBBADDATA:
  55 + return "Invalid data";
  56 + case EMBBADEXC:
  57 + return "Invalid exception code";
  58 + case EMBMDATA:
  59 + return "Too many data";
  60 + case EMBBADSLAVE:
  61 + return "Response not from requested slave";
  62 + default:
  63 + return strerror(errnum);
  64 + }
  65 +}
  66 +
  67 +void _error_print(modbus_t *ctx, const char *context)
  68 +{
  69 + if (ctx->debug)
  70 + {
  71 + fprintf(stderr, "ERROR %s", modbus_strerror(errno));
  72 + if (context != NULL)
  73 + {
  74 + fprintf(stderr, ": %s\n", context);
  75 + }
  76 + else
  77 + {
  78 + fprintf(stderr, "\n");
  79 + }
  80 + }
  81 +}
  82 +
  83 +static void _sleep_response_timeout(modbus_t *ctx)
  84 +{
  85 + /* Response timeout is always positive */
  86 + /* usleep source code */
  87 + struct timespec request, remaining;
  88 + request.tv_sec = ctx->response_timeout.tv_sec;
  89 + request.tv_nsec = ((long int)ctx->response_timeout.tv_usec) * 1000;
  90 + while ( nanosleep( &request, &remaining ) == -1 && errno == EINTR )
  91 + {
  92 + request = remaining;
  93 + }
  94 +}
  95 +
  96 +int modbus_flush(modbus_t *ctx)
  97 +{
  98 + int rc;
  99 +
  100 + if (ctx == NULL)
  101 + {
  102 + errno = EINVAL;
  103 + return -1;
  104 + }
  105 +
  106 + rc = ctx->backend->flush( ctx );
  107 + if (rc != -1 && ctx->debug)
  108 + {
  109 + /* Not all backends are able to return the number of bytes flushed */
  110 + printf("Bytes flushed (%d)\n", rc);
  111 + }
  112 + return rc;
  113 +}
  114 +
  115 +/* Computes the length of the expected response */
  116 +static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t *req)
  117 +{
  118 + int length;
  119 + const int offset = ctx->backend->header_length;
  120 +
  121 + switch (req[offset])
  122 + {
  123 + case MODBUS_FC_READ_COILS:
  124 + case MODBUS_FC_READ_DISCRETE_INPUTS:
  125 + /* Header + nb values (code from write_bits) */
  126 + int nb = (req[offset + 3] << 8) | req[offset + 4];
  127 + length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
  128 + break;
  129 + case MODBUS_FC_WRITE_AND_READ_REGISTERS:
  130 + case MODBUS_FC_READ_HOLDING_REGISTERS:
  131 + case MODBUS_FC_READ_INPUT_REGISTERS:
  132 + /* Header + 2 * nb values */
  133 + length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
  134 + break;
  135 + case MODBUS_FC_READ_EXCEPTION_STATUS:
  136 + length = 3;
  137 + break;
  138 + case MODBUS_FC_REPORT_SLAVE_ID:
  139 + /* The response is device specific (the header provides the
  140 + length) */
  141 + return MSG_LENGTH_UNDEFINED;
  142 + case MODBUS_FC_MASK_WRITE_REGISTER:
  143 + length = 7;
  144 + break;
  145 + default:
  146 + length = 5;
  147 + }
  148 +
  149 + return offset + length + ctx->backend->checksum_length;
  150 +}
  151 +
  152 +/* Sends a request/response */
  153 +static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
  154 +{
  155 + int rc;
  156 + int i;
  157 +
  158 + msg_length = ctx->backend->send_msg_pre(msg, msg_length);
  159 +
  160 + if (ctx->debug)
  161 + {
  162 + for (i = 0; i < msg_length; i++)
  163 + {
  164 + printf("[%.2X]", msg[i]);
  165 + }
  166 + printf("\n");
  167 + }
  168 +
  169 + /* In recovery mode, the write command will be issued until to be
  170 + successful! Disabled by default. */
  171 + do
  172 + {
  173 + rc = ctx->backend->send(ctx, msg, msg_length);
  174 + if (rc == -1)
  175 + {
  176 + _error_print(ctx, NULL);
  177 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK)
  178 + {
  179 + int saved_errno = errno;
  180 +
  181 + if ( (errno == EBADF || errno == ECONNRESET || errno == EPIPE))
  182 + {
  183 + modbus_close(ctx);
  184 + _sleep_response_timeout(ctx);
  185 + modbus_connect(ctx);
  186 + }
  187 + else
  188 + {
  189 + _sleep_response_timeout(ctx);
  190 + modbus_flush(ctx);
  191 + }
  192 + errno = saved_errno;
  193 + }
  194 + }
  195 + } while ( (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && rc == -1);
  196 +
  197 + if (rc > 0 && rc != msg_length) {
  198 + errno = EMBBADDATA;
  199 + return -1;
  200 + }
  201 +
  202 + return rc;
  203 +}
  204 +
  205 +int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length )
  206 +{
  207 + sft_t sft;
  208 + uint8_t req[MAX_MESSAGE_LENGTH];
  209 + int req_length;
  210 +
  211 + if (ctx == NULL)
  212 + {
  213 + errno = EINVAL;
  214 + return -1;
  215 + }
  216 +
  217 + if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1))
  218 + {
  219 + /* The raw request must contain function and slave at least and
  220 + must not be longer than the maximum pdu length plus the slave
  221 + address. */
  222 + errno = EINVAL;
  223 + return -1;
  224 + }
  225 +
  226 + sft.slave = raw_req[0];
  227 + sft.function = raw_req[1];
  228 + /* The t_id is left to zero */
  229 + sft.t_id = 0;
  230 + /* This response function only set the header so it's convenient here */
  231 + req_length = ctx->backend->build_response_basis(&sft, req);
  232 +
  233 + if( raw_req_length > 2 )
  234 + {
  235 + /* Copy data after function code */
  236 + memcpy(req + req_length, raw_req + 2, raw_req_length - 2);
  237 + req_length += raw_req_length - 2;
  238 + }
  239 +
  240 + return send_msg(ctx, req, req_length);
  241 +}
  242 +
  243 +/*
  244 + * ---------- Request Indication ----------
  245 + * | Client | ---------------------->| Server |
  246 + * ---------- Confirmation Response ----------
  247 + */
  248 +
  249 +/* Computes the length to read after the function received */
  250 +static uint8_t compute_meta_length_after_function( int function, msg_type_t msg_type )
  251 +{
  252 + int length;
  253 +
  254 + if (msg_type == MSG_INDICATION)
  255 + {
  256 + if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER)
  257 + {
  258 + length = 4;
  259 + }
  260 + else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS || function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS)
  261 + {
  262 + length = 5;
  263 + }
  264 + else if (function == MODBUS_FC_MASK_WRITE_REGISTER)
  265 + {
  266 + length = 6;
  267 + }
  268 + else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS)
  269 + {
  270 + length = 9;
  271 + }
  272 + else
  273 + {
  274 + /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */
  275 + length = 0;
  276 + }
  277 + }
  278 + else
  279 + {
  280 + /* MSG_CONFIRMATION */
  281 + switch (function)
  282 + {
  283 + case MODBUS_FC_WRITE_SINGLE_COIL:
  284 + case MODBUS_FC_WRITE_SINGLE_REGISTER:
  285 + case MODBUS_FC_WRITE_MULTIPLE_COILS:
  286 + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
  287 + length = 4;
  288 + break;
  289 + case MODBUS_FC_MASK_WRITE_REGISTER:
  290 + length = 6;
  291 + break;
  292 + default:
  293 + length = 1;
  294 + }
  295 + }
  296 +
  297 + return length;
  298 +}
  299 +
  300 +/* Computes the length to read after the meta information (address, count, etc) */
  301 +static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
  302 +{
  303 + int function = msg[ctx->backend->header_length];
  304 + int length;
  305 +
  306 + if (msg_type == MSG_INDICATION)
  307 + {
  308 + switch (function)
  309 + {
  310 + case MODBUS_FC_WRITE_MULTIPLE_COILS:
  311 + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
  312 + length = msg[ctx->backend->header_length + 5];
  313 + break;
  314 + case MODBUS_FC_WRITE_AND_READ_REGISTERS:
  315 + length = msg[ctx->backend->header_length + 9];
  316 + break;
  317 + default:
  318 + length = 0;
  319 + }
  320 + }
  321 + else
  322 + {
  323 + /* MSG_CONFIRMATION */
  324 + if (function <= MODBUS_FC_READ_INPUT_REGISTERS ||
  325 + function == MODBUS_FC_REPORT_SLAVE_ID ||
  326 + function == MODBUS_FC_WRITE_AND_READ_REGISTERS)
  327 + {
  328 + length = msg[ctx->backend->header_length + 1];
  329 + }
  330 + else
  331 + {
  332 + length = 0;
  333 + }
  334 + }
  335 +
  336 + length += ctx->backend->checksum_length;
  337 +
  338 + return length;
  339 +}
  340 +
  341 +
  342 +/* Waits a response from a modbus server or a request from a modbus client.
  343 + This function blocks if there is no replies (3 timeouts).
  344 +
  345 + The function shall return the number of received characters and the received
  346 + message in an array of uint8_t if successful. Otherwise it shall return -1
  347 + and errno is set to one of the values defined below:
  348 + - ECONNRESET
  349 + - EMBBADDATA
  350 + - EMBUNKEXC
  351 + - ETIMEDOUT
  352 + - read() or recv() error codes
  353 +*/
  354 +
  355 +int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
  356 +{
  357 + int rc;
  358 + fd_set rset;
  359 + struct timeval tv;
  360 + struct timeval *p_tv;
  361 + int length_to_read;
  362 + int msg_length = 0;
  363 + _step_t step;
  364 +
  365 + if (ctx->debug)
  366 + {
  367 + if (msg_type == MSG_INDICATION)
  368 + {
  369 + printf("Waiting for a indication...\n");
  370 + }
  371 + else
  372 + {
  373 + printf("Waiting for a confirmation...\n");
  374 + }
  375 + }
  376 +
  377 + /* Add a file descriptor to the set */
  378 + FD_ZERO(&rset);
  379 + FD_SET(ctx->s, &rset);
  380 +
  381 + /* We need to analyse the message step by step. At the first step, we want
  382 + * to reach the function code because all packets contain this
  383 + * information. */
  384 + step = _STEP_FUNCTION;
  385 + length_to_read = ctx->backend->header_length + 1;
  386 +
  387 + if (msg_type == MSG_INDICATION)
  388 + {
  389 + /* Wait for a message, we don't know when the message will be
  390 + * received */
  391 + p_tv = NULL;
  392 + }
  393 + else
  394 + {
  395 + tv.tv_sec = ctx->response_timeout.tv_sec;
  396 + tv.tv_usec = ctx->response_timeout.tv_usec;
  397 + p_tv = &tv;
  398 + }
  399 +
  400 + while (length_to_read != 0)
  401 + {
  402 + rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
  403 + if (rc == -1)
  404 + {
  405 + _error_print(ctx, "select");
  406 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK)
  407 + {
  408 + int saved_errno = errno;
  409 +
  410 + if (errno == ETIMEDOUT)
  411 + {
  412 + _sleep_response_timeout(ctx);
  413 + modbus_flush(ctx);
  414 + }
  415 + else if (errno == EBADF)
  416 + {
  417 + modbus_close(ctx);
  418 + modbus_connect(ctx);
  419 + }
  420 + errno = saved_errno;
  421 + }
  422 + return -1;
  423 + }
  424 +
  425 + rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
  426 + if (rc == 0)
  427 + {
  428 + errno = ECONNRESET;
  429 + rc = -1;
  430 + }
  431 +
  432 + if (rc == -1)
  433 + {
  434 + _error_print(ctx, "read");
  435 + if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
  436 + (errno == ECONNRESET || errno == ECONNREFUSED ||
  437 + errno == EBADF))
  438 + {
  439 + int saved_errno = errno;
  440 + modbus_close(ctx);
  441 + modbus_connect(ctx);
  442 + /* Could be removed by previous calls */
  443 + errno = saved_errno;
  444 + }
  445 + return -1;
  446 + }
  447 +
  448 + /* Display the hex code of each character received */
  449 + if (ctx->debug)
  450 + {
  451 + int i;
  452 + for (i=0; i < rc; i++)
  453 + printf("<%.2X>", msg[msg_length + i]);
  454 + }
  455 +
  456 + /* Sums bytes received */
  457 + msg_length += rc;
  458 + /* Computes remaining bytes */
  459 + length_to_read -= rc;
  460 +
  461 + if (length_to_read == 0)
  462 + {
  463 + switch (step)
  464 + {
  465 + case _STEP_FUNCTION:
  466 + /* Function code position */
  467 + length_to_read = compute_meta_length_after_function( msg[ctx->backend->header_length], msg_type);
  468 + if (length_to_read != 0)
  469 + {
  470 + step = _STEP_META;
  471 + break;
  472 + } /* else switches straight to the next step */
  473 + case _STEP_META:
  474 + length_to_read = compute_data_length_after_meta( ctx, msg, msg_type);
  475 + if ( (msg_length + length_to_read) > (int)ctx->backend->max_adu_length )
  476 + {
  477 + errno = EMBBADDATA;
  478 + _error_print(ctx, "too many data");
  479 + return -1;
  480 + }
  481 + step = _STEP_DATA;
  482 + break;
  483 + default:
  484 + break;
  485 + }
  486 + }
  487 +
  488 + if (length_to_read > 0 &&
  489 + (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0)) {
  490 + /* If there is no character in the buffer, the allowed timeout
  491 + interval between two consecutive bytes is defined by
  492 + byte_timeout */
  493 + tv.tv_sec = ctx->byte_timeout.tv_sec;
  494 + tv.tv_usec = ctx->byte_timeout.tv_usec;
  495 + p_tv = &tv;
  496 + }
  497 + /* else timeout isn't set again, the full response must be read before
  498 + expiration of response timeout (for CONFIRMATION only) */
  499 + }
  500 +
  501 + if (ctx->debug)
  502 + printf("\n");
  503 +
  504 + return ctx->backend->check_integrity(ctx, msg, msg_length);
  505 +}
  506 +
  507 +/* Receive the request from a modbus master */
  508 +int modbus_receive(modbus_t *ctx, uint8_t *req)
  509 +{
  510 + if (ctx == NULL) {
  511 + errno = EINVAL;
  512 + return -1;
  513 + }
  514 +
  515 + return ctx->backend->receive(ctx, req);
  516 +}
  517 +
  518 +/* Receives the confirmation.
  519 +
  520 + The function shall store the read response in rsp and return the number of
  521 + values (bits or words). Otherwise, its shall return -1 and errno is set.
  522 +
  523 + The function doesn't check the confirmation is the expected response to the
  524 + initial request.
  525 +*/
  526 +int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp)
  527 +{
  528 + if (ctx == NULL) {
  529 + errno = EINVAL;
  530 + return -1;
  531 + }
  532 +
  533 + return _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  534 +}
  535 +
  536 +static int check_confirmation(modbus_t *ctx, uint8_t *req,
  537 + uint8_t *rsp, int rsp_length)
  538 +{
  539 + int rc;
  540 + int rsp_length_computed;
  541 + const int offset = ctx->backend->header_length;
  542 + const int function = rsp[offset];
  543 +
  544 + if (ctx->backend->pre_check_confirmation) {
  545 + rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
  546 + if (rc == -1) {
  547 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  548 + _sleep_response_timeout(ctx);
  549 + modbus_flush(ctx);
  550 + }
  551 + return -1;
  552 + }
  553 + }
  554 +
  555 + rsp_length_computed = compute_response_length_from_request(ctx, req);
  556 +
  557 + /* Exception code */
  558 + if (function >= 0x80) {
  559 + if (rsp_length == (offset + 2 + (int)ctx->backend->checksum_length) &&
  560 + req[offset] == (rsp[offset] - 0x80)) {
  561 + /* Valid exception code received */
  562 +
  563 + int exception_code = rsp[offset + 1];
  564 + if (exception_code < MODBUS_EXCEPTION_MAX) {
  565 + errno = MODBUS_ENOBASE + exception_code;
  566 + } else {
  567 + errno = EMBBADEXC;
  568 + }
  569 + _error_print(ctx, NULL);
  570 + return -1;
  571 + } else {
  572 + errno = EMBBADEXC;
  573 + _error_print(ctx, NULL);
  574 + return -1;
  575 + }
  576 + }
  577 +
  578 + /* Check length */
  579 + if ((rsp_length == rsp_length_computed ||
  580 + rsp_length_computed == MSG_LENGTH_UNDEFINED) &&
  581 + function < 0x80) {
  582 + int req_nb_value;
  583 + int rsp_nb_value;
  584 +
  585 + /* Check function code */
  586 + if (function != req[offset]) {
  587 + if (ctx->debug) {
  588 + fprintf(stderr,
  589 + "Received function not corresponding to the request (0x%X != 0x%X)\n",
  590 + function, req[offset]);
  591 + }
  592 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  593 + _sleep_response_timeout(ctx);
  594 + modbus_flush(ctx);
  595 + }
  596 + errno = EMBBADDATA;
  597 + return -1;
  598 + }
  599 +
  600 + /* Check the number of values is corresponding to the request */
  601 + switch (function) {
  602 + case MODBUS_FC_READ_COILS:
  603 + case MODBUS_FC_READ_DISCRETE_INPUTS:
  604 + /* Read functions, 8 values in a byte (nb
  605 + * of values in the request and byte count in
  606 + * the response. */
  607 + req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
  608 + req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
  609 + rsp_nb_value = rsp[offset + 1];
  610 + break;
  611 + case MODBUS_FC_WRITE_AND_READ_REGISTERS:
  612 + case MODBUS_FC_READ_HOLDING_REGISTERS:
  613 + case MODBUS_FC_READ_INPUT_REGISTERS:
  614 + /* Read functions 1 value = 2 bytes */
  615 + req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
  616 + rsp_nb_value = (rsp[offset + 1] / 2);
  617 + break;
  618 + case MODBUS_FC_WRITE_MULTIPLE_COILS:
  619 + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
  620 + /* N Write functions */
  621 + req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
  622 + rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
  623 + break;
  624 + case MODBUS_FC_REPORT_SLAVE_ID:
  625 + /* Report slave ID (bytes received) */
  626 + req_nb_value = rsp_nb_value = rsp[offset + 1];
  627 + break;
  628 + default:
  629 + /* 1 Write functions & others */
  630 + req_nb_value = rsp_nb_value = 1;
  631 + }
  632 +
  633 + if (req_nb_value == rsp_nb_value) {
  634 + rc = rsp_nb_value;
  635 + } else {
  636 + if (ctx->debug) {
  637 + fprintf(stderr,
  638 + "Quantity not corresponding to the request (%d != %d)\n",
  639 + rsp_nb_value, req_nb_value);
  640 + }
  641 +
  642 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  643 + _sleep_response_timeout(ctx);
  644 + modbus_flush(ctx);
  645 + }
  646 +
  647 + errno = EMBBADDATA;
  648 + rc = -1;
  649 + }
  650 + } else {
  651 + if (ctx->debug) {
  652 + fprintf(stderr,
  653 + "Message length not corresponding to the computed length (%d != %d)\n",
  654 + rsp_length, rsp_length_computed);
  655 + }
  656 + if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
  657 + _sleep_response_timeout(ctx);
  658 + modbus_flush(ctx);
  659 + }
  660 + errno = EMBBADDATA;
  661 + rc = -1;
  662 + }
  663 +
  664 + return rc;
  665 +}
  666 +
  667 +static int response_io_status(uint8_t *tab_io_status,
  668 + int address, int nb,
  669 + uint8_t *rsp, int offset)
  670 +{
  671 + int shift = 0;
  672 + /* Instead of byte (not allowed in Win32) */
  673 + int one_byte = 0;
  674 + int i;
  675 +
  676 + for (i = address; i < address + nb; i++) {
  677 + one_byte |= tab_io_status[i] << shift;
  678 + if (shift == 7) {
  679 + /* Byte is full */
  680 + rsp[offset++] = one_byte;
  681 + one_byte = shift = 0;
  682 + } else {
  683 + shift++;
  684 + }
  685 + }
  686 +
  687 + if (shift != 0)
  688 + rsp[offset++] = one_byte;
  689 +
  690 + return offset;
  691 +}
  692 +
  693 +/* Build the exception response */
  694 +static int response_exception(modbus_t *ctx, sft_t *sft,
  695 + int exception_code, uint8_t *rsp,
  696 + unsigned int to_flush,
  697 + const char* template, ...)
  698 +{
  699 + int rsp_length;
  700 +
  701 + /* Print debug message */
  702 + if (ctx->debug) {
  703 + va_list ap;
  704 +
  705 + va_start(ap, template);
  706 + vfprintf(stderr, template, ap);
  707 + va_end(ap);
  708 + }
  709 +
  710 + /* Flush if required */
  711 + if (to_flush) {
  712 + _sleep_response_timeout(ctx);
  713 + modbus_flush(ctx);
  714 + }
  715 +
  716 + /* Build exception response */
  717 + sft->function = sft->function + 0x80;
  718 + rsp_length = ctx->backend->build_response_basis(sft, rsp);
  719 + rsp[rsp_length++] = exception_code;
  720 +
  721 + return rsp_length;
  722 +}
  723 +
  724 +/* Send a response to the received request.
  725 + Analyses the request and constructs a response.
  726 +
  727 + If an error occurs, this function construct the response
  728 + accordingly.
  729 +*/
  730 +int modbus_reply(modbus_t *ctx, const uint8_t *req,
  731 + int req_length, modbus_mapping_t *mb_mapping)
  732 +{
  733 + int offset;
  734 + int slave;
  735 + int function;
  736 + uint16_t address;
  737 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  738 + int rsp_length = 0;
  739 + sft_t sft;
  740 +
  741 + if (ctx == NULL) {
  742 + errno = EINVAL;
  743 + return -1;
  744 + }
  745 +
  746 + offset = ctx->backend->header_length;
  747 + slave = req[offset - 1];
  748 + function = req[offset];
  749 + address = (req[offset + 1] << 8) + req[offset + 2];
  750 +
  751 + sft.slave = slave;
  752 + sft.function = function;
  753 + sft.t_id = ctx->backend->prepare_response_tid(req, &req_length);
  754 +
  755 + /* Data are flushed on illegal number of values errors. */
  756 + switch (function) {
  757 + case MODBUS_FC_READ_COILS:
  758 + case MODBUS_FC_READ_DISCRETE_INPUTS: {
  759 + unsigned int is_input = (function == MODBUS_FC_READ_DISCRETE_INPUTS);
  760 + int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits;
  761 + int nb_bits = is_input ? mb_mapping->nb_input_bits : mb_mapping->nb_bits;
  762 + uint8_t *tab_bits = is_input ? mb_mapping->tab_input_bits : mb_mapping->tab_bits;
  763 + const char * const name = is_input ? "read_input_bits" : "read_bits";
  764 + int nb = (req[offset + 3] << 8) + req[offset + 4];
  765 + /* The mapping can be shifted to reduce memory consumption and it
  766 + doesn't always start at address zero. */
  767 + int mapping_address = address - start_bits;
  768 +
  769 + if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
  770 + rsp_length = response_exception(
  771 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  772 + "Illegal nb of values %d in %s (max %d)\n",
  773 + nb, name, MODBUS_MAX_READ_BITS);
  774 + } else if (mapping_address < 0 || (mapping_address + nb) > nb_bits) {
  775 + rsp_length = response_exception(
  776 + ctx, &sft,
  777 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  778 + "Illegal data address 0x%0X in %s\n",
  779 + mapping_address < 0 ? address : address + nb, name);
  780 + } else {
  781 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  782 + rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
  783 + rsp_length = response_io_status(tab_bits, mapping_address, nb,
  784 + rsp, rsp_length);
  785 + }
  786 + }
  787 + break;
  788 + case MODBUS_FC_READ_HOLDING_REGISTERS:
  789 + case MODBUS_FC_READ_INPUT_REGISTERS: {
  790 + unsigned int is_input = (function == MODBUS_FC_READ_INPUT_REGISTERS);
  791 + int start_registers = is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers;
  792 + int nb_registers = is_input ? mb_mapping->nb_input_registers : mb_mapping->nb_registers;
  793 + uint16_t *tab_registers = is_input ? mb_mapping->tab_input_registers : mb_mapping->tab_registers;
  794 + const char * const name = is_input ? "read_input_registers" : "read_registers";
  795 + int nb = (req[offset + 3] << 8) + req[offset + 4];
  796 + /* The mapping can be shifted to reduce memory consumption and it
  797 + doesn't always start at address zero. */
  798 + int mapping_address = address - start_registers;
  799 +
  800 + if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
  801 + rsp_length = response_exception(
  802 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  803 + "Illegal nb of values %d in %s (max %d)\n",
  804 + nb, name, MODBUS_MAX_READ_REGISTERS);
  805 + } else if (mapping_address < 0 || (mapping_address + nb) > nb_registers) {
  806 + rsp_length = response_exception(
  807 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  808 + "Illegal data address 0x%0X in %s\n",
  809 + mapping_address < 0 ? address : address + nb, name);
  810 + } else {
  811 + int i;
  812 +
  813 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  814 + rsp[rsp_length++] = nb << 1;
  815 + for (i = mapping_address; i < mapping_address + nb; i++) {
  816 + rsp[rsp_length++] = tab_registers[i] >> 8;
  817 + rsp[rsp_length++] = tab_registers[i] & 0xFF;
  818 + }
  819 + }
  820 + }
  821 + break;
  822 + case MODBUS_FC_WRITE_SINGLE_COIL: {
  823 + int mapping_address = address - mb_mapping->start_bits;
  824 +
  825 + if (mapping_address < 0 || mapping_address >= mb_mapping->nb_bits) {
  826 + rsp_length = response_exception(
  827 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  828 + "Illegal data address 0x%0X in write_bit\n",
  829 + address);
  830 + } else {
  831 + int data = (req[offset + 3] << 8) + req[offset + 4];
  832 +
  833 + if (data == 0xFF00 || data == 0x0) {
  834 + mb_mapping->tab_bits[mapping_address] = data ? ON : OFF;
  835 + memcpy(rsp, req, req_length);
  836 + rsp_length = req_length;
  837 + } else {
  838 + rsp_length = response_exception(
  839 + ctx, &sft,
  840 + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, FALSE,
  841 + "Illegal data value 0x%0X in write_bit request at address %0X\n",
  842 + data, address);
  843 + }
  844 + }
  845 + }
  846 + break;
  847 + case MODBUS_FC_WRITE_SINGLE_REGISTER: {
  848 + int mapping_address = address - mb_mapping->start_registers;
  849 +
  850 + if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
  851 + rsp_length = response_exception(
  852 + ctx, &sft,
  853 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  854 + "Illegal data address 0x%0X in write_register\n",
  855 + address);
  856 + } else {
  857 + int data = (req[offset + 3] << 8) + req[offset + 4];
  858 +
  859 + mb_mapping->tab_registers[mapping_address] = data;
  860 + memcpy(rsp, req, req_length);
  861 + rsp_length = req_length;
  862 + }
  863 + }
  864 + break;
  865 + case MODBUS_FC_WRITE_MULTIPLE_COILS: {
  866 + int nb = (req[offset + 3] << 8) + req[offset + 4];
  867 + int mapping_address = address - mb_mapping->start_bits;
  868 +
  869 + if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) {
  870 + /* May be the indication has been truncated on reading because of
  871 + * invalid address (eg. nb is 0 but the request contains values to
  872 + * write) so it's necessary to flush. */
  873 + rsp_length = response_exception(
  874 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  875 + "Illegal number of values %d in write_bits (max %d)\n",
  876 + nb, MODBUS_MAX_WRITE_BITS);
  877 + } else if (mapping_address < 0 ||
  878 + (mapping_address + nb) > mb_mapping->nb_bits) {
  879 + rsp_length = response_exception(
  880 + ctx, &sft,
  881 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  882 + "Illegal data address 0x%0X in write_bits\n",
  883 + mapping_address < 0 ? address : address + nb);
  884 + } else {
  885 + /* 6 = byte count */
  886 + modbus_set_bits_from_bytes(mb_mapping->tab_bits, mapping_address, nb,
  887 + &req[offset + 6]);
  888 +
  889 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  890 + /* 4 to copy the bit address (2) and the quantity of bits */
  891 + memcpy(rsp + rsp_length, req + rsp_length, 4);
  892 + rsp_length += 4;
  893 + }
  894 + }
  895 + break;
  896 + case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: {
  897 + int nb = (req[offset + 3] << 8) + req[offset + 4];
  898 + int mapping_address = address - mb_mapping->start_registers;
  899 +
  900 + if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb) {
  901 + rsp_length = response_exception(
  902 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  903 + "Illegal number of values %d in write_registers (max %d)\n",
  904 + nb, MODBUS_MAX_WRITE_REGISTERS);
  905 + } else if (mapping_address < 0 ||
  906 + (mapping_address + nb) > mb_mapping->nb_registers) {
  907 + rsp_length = response_exception(
  908 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  909 + "Illegal data address 0x%0X in write_registers\n",
  910 + mapping_address < 0 ? address : address + nb);
  911 + } else {
  912 + int i, j;
  913 + for (i = mapping_address, j = 6; i < mapping_address + nb; i++, j += 2) {
  914 + /* 6 and 7 = first value */
  915 + mb_mapping->tab_registers[i] =
  916 + (req[offset + j] << 8) + req[offset + j + 1];
  917 + }
  918 +
  919 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  920 + /* 4 to copy the address (2) and the no. of registers */
  921 + memcpy(rsp + rsp_length, req + rsp_length, 4);
  922 + rsp_length += 4;
  923 + }
  924 + }
  925 + break;
  926 + case MODBUS_FC_REPORT_SLAVE_ID:
  927 + {
  928 + int str_len;
  929 + int byte_count_pos;
  930 +
  931 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  932 + /* Skip byte count for now */
  933 + byte_count_pos = rsp_length++;
  934 + rsp[rsp_length++] = _REPORT_SLAVE_ID;
  935 + /* Run indicator status to ON */
  936 + rsp[rsp_length++] = 0xFF;
  937 + str_len = 3 + strlen(MODBUS_VERSION_STRING);
  938 + memcpy(rsp + rsp_length, "LMB" MODBUS_VERSION_STRING, str_len);
  939 + rsp_length += str_len;
  940 + rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
  941 + }
  942 + break;
  943 + case MODBUS_FC_READ_EXCEPTION_STATUS:
  944 + if (ctx->debug) {
  945 + fprintf(stderr, "FIXME Not implemented\n");
  946 + }
  947 + errno = ENOPROTOOPT;
  948 + return -1;
  949 + break;
  950 + case MODBUS_FC_MASK_WRITE_REGISTER: {
  951 + int mapping_address = address - mb_mapping->start_registers;
  952 +
  953 + if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
  954 + rsp_length = response_exception(
  955 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  956 + "Illegal data address 0x%0X in write_register\n",
  957 + address);
  958 + } else {
  959 + uint16_t data = mb_mapping->tab_registers[mapping_address];
  960 + uint16_t and = (req[offset + 3] << 8) + req[offset + 4];
  961 + uint16_t or = (req[offset + 5] << 8) + req[offset + 6];
  962 +
  963 + data = (data & and) | (or & (~and));
  964 + mb_mapping->tab_registers[mapping_address] = data;
  965 + memcpy(rsp, req, req_length);
  966 + rsp_length = req_length;
  967 + }
  968 + }
  969 + break;
  970 + case MODBUS_FC_WRITE_AND_READ_REGISTERS: {
  971 + int nb = (req[offset + 3] << 8) + req[offset + 4];
  972 + uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6];
  973 + int nb_write = (req[offset + 7] << 8) + req[offset + 8];
  974 + int nb_write_bytes = req[offset + 9];
  975 + int mapping_address = address - mb_mapping->start_registers;
  976 + int mapping_address_write = address_write - mb_mapping->start_registers;
  977 +
  978 + if (nb_write < 1 || MODBUS_MAX_WR_WRITE_REGISTERS < nb_write ||
  979 + nb < 1 || MODBUS_MAX_WR_READ_REGISTERS < nb ||
  980 + nb_write_bytes != nb_write * 2) {
  981 + rsp_length = response_exception(
  982 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
  983 + "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n",
  984 + nb_write, nb, MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS);
  985 + } else if (mapping_address < 0 ||
  986 + (mapping_address + nb) > mb_mapping->nb_registers ||
  987 + mapping_address < 0 ||
  988 + (mapping_address_write + nb_write) > mb_mapping->nb_registers) {
  989 + rsp_length = response_exception(
  990 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
  991 + "Illegal data read address 0x%0X or write address 0x%0X write_and_read_registers\n",
  992 + mapping_address < 0 ? address : address + nb,
  993 + mapping_address_write < 0 ? address_write : address_write + nb_write);
  994 + } else {
  995 + int i, j;
  996 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  997 + rsp[rsp_length++] = nb << 1;
  998 +
  999 + /* Write first.
  1000 + 10 and 11 are the offset of the first values to write */
  1001 + for (i = mapping_address_write, j = 10;
  1002 + i < mapping_address_write + nb_write; i++, j += 2) {
  1003 + mb_mapping->tab_registers[i] =
  1004 + (req[offset + j] << 8) + req[offset + j + 1];
  1005 + }
  1006 +
  1007 + /* and read the data for the response */
  1008 + for (i = mapping_address; i < mapping_address + nb; i++) {
  1009 + rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
  1010 + rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
  1011 + }
  1012 + }
  1013 + }
  1014 + break;
  1015 +
  1016 + default:
  1017 + rsp_length = response_exception(
  1018 + ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, TRUE,
  1019 + "Unknown Modbus function code: 0x%0X\n", function);
  1020 + break;
  1021 + }
  1022 +
  1023 + /* Suppress any responses when the request was a broadcast */
  1024 + return (slave == MODBUS_BROADCAST_ADDRESS) ? 0 : send_msg(ctx, rsp, rsp_length);
  1025 +}
  1026 +
  1027 +int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
  1028 + unsigned int exception_code)
  1029 +{
  1030 + int offset;
  1031 + int slave;
  1032 + int function;
  1033 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1034 + int rsp_length;
  1035 + int dummy_length = 99;
  1036 + sft_t sft;
  1037 +
  1038 + if (ctx == NULL) {
  1039 + errno = EINVAL;
  1040 + return -1;
  1041 + }
  1042 +
  1043 + offset = ctx->backend->header_length;
  1044 + slave = req[offset - 1];
  1045 + function = req[offset];
  1046 +
  1047 + sft.slave = slave;
  1048 + sft.function = function + 0x80;;
  1049 + sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length);
  1050 + rsp_length = ctx->backend->build_response_basis(&sft, rsp);
  1051 +
  1052 + /* Positive exception code */
  1053 + if (exception_code < MODBUS_EXCEPTION_MAX) {
  1054 + rsp[rsp_length++] = exception_code;
  1055 + return send_msg(ctx, rsp, rsp_length);
  1056 + } else {
  1057 + errno = EINVAL;
  1058 + return -1;
  1059 + }
  1060 +}
  1061 +
  1062 +/* Reads IO status */
  1063 +static int read_io_status(modbus_t *ctx, int function,
  1064 + int addr, int nb, uint8_t *dest)
  1065 +{
  1066 + int rc;
  1067 + int req_length;
  1068 +
  1069 + uint8_t req[_MIN_REQ_LENGTH];
  1070 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1071 +
  1072 + req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
  1073 +
  1074 + rc = send_msg(ctx, req, req_length);
  1075 + if (rc > 0) {
  1076 + int i, temp, bit;
  1077 + int pos = 0;
  1078 + int offset;
  1079 + int offset_end;
  1080 +
  1081 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1082 + if (rc == -1)
  1083 + return -1;
  1084 +
  1085 + rc = check_confirmation(ctx, req, rsp, rc);
  1086 + if (rc == -1)
  1087 + return -1;
  1088 +
  1089 + offset = ctx->backend->header_length + 2;
  1090 + offset_end = offset + rc;
  1091 + for (i = offset; i < offset_end; i++) {
  1092 + /* Shift reg hi_byte to temp */
  1093 + temp = rsp[i];
  1094 +
  1095 + for (bit = 0x01; (bit & 0xff) && (pos < nb);) {
  1096 + dest[pos++] = (temp & bit) ? TRUE : FALSE;
  1097 + bit = bit << 1;
  1098 + }
  1099 +
  1100 + }
  1101 + }
  1102 +
  1103 + return rc;
  1104 +}
  1105 +
  1106 +/* Reads the boolean status of bits and sets the array elements
  1107 + in the destination to TRUE or FALSE (single bits). */
  1108 +int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
  1109 +{
  1110 + int rc;
  1111 +
  1112 + if (ctx == NULL) {
  1113 + errno = EINVAL;
  1114 + return -1;
  1115 + }
  1116 +
  1117 + if (nb > MODBUS_MAX_READ_BITS) {
  1118 + if (ctx->debug) {
  1119 + fprintf(stderr,
  1120 + "ERROR Too many bits requested (%d > %d)\n",
  1121 + nb, MODBUS_MAX_READ_BITS);
  1122 + }
  1123 + errno = EMBMDATA;
  1124 + return -1;
  1125 + }
  1126 +
  1127 + rc = read_io_status(ctx, MODBUS_FC_READ_COILS, addr, nb, dest);
  1128 +
  1129 + if (rc == -1)
  1130 + return -1;
  1131 + else
  1132 + return nb;
  1133 +}
  1134 +
  1135 +
  1136 +/* Same as modbus_read_bits but reads the remote device input table */
  1137 +int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
  1138 +{
  1139 + int rc;
  1140 +
  1141 + if (ctx == NULL) {
  1142 + errno = EINVAL;
  1143 + return -1;
  1144 + }
  1145 +
  1146 + if (nb > MODBUS_MAX_READ_BITS) {
  1147 + if (ctx->debug) {
  1148 + fprintf(stderr,
  1149 + "ERROR Too many discrete inputs requested (%d > %d)\n",
  1150 + nb, MODBUS_MAX_READ_BITS);
  1151 + }
  1152 + errno = EMBMDATA;
  1153 + return -1;
  1154 + }
  1155 +
  1156 + rc = read_io_status(ctx, MODBUS_FC_READ_DISCRETE_INPUTS, addr, nb, dest);
  1157 +
  1158 + if (rc == -1)
  1159 + return -1;
  1160 + else
  1161 + return nb;
  1162 +}
  1163 +
  1164 +/* Reads the data from a remove device and put that data into an array */
  1165 +static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_t *dest)
  1166 +{
  1167 + int rc;
  1168 + int req_length;
  1169 + uint8_t req[_MIN_REQ_LENGTH];
  1170 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1171 +
  1172 + if (nb > MODBUS_MAX_READ_REGISTERS)
  1173 + {
  1174 + if (ctx->debug)
  1175 + {
  1176 + fprintf(stderr,
  1177 + "ERROR Too many registers requested (%d > %d)\n",
  1178 + nb, MODBUS_MAX_READ_REGISTERS);
  1179 + }
  1180 + errno = EMBMDATA;
  1181 + return -1;
  1182 + }
  1183 +
  1184 + req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
  1185 +
  1186 + rc = send_msg(ctx, req, req_length);
  1187 + if (rc > 0)
  1188 + {
  1189 + int offset;
  1190 + int i;
  1191 +
  1192 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1193 + if (rc == -1)
  1194 + return -1;
  1195 +
  1196 + rc = check_confirmation(ctx, req, rsp, rc);
  1197 + if (rc == -1)
  1198 + return -1;
  1199 +
  1200 + offset = ctx->backend->header_length;
  1201 +
  1202 + for (i = 0; i < rc; i++) {
  1203 + /* shift reg hi_byte to temp OR with lo_byte */
  1204 + dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
  1205 + rsp[offset + 3 + (i << 1)];
  1206 + }
  1207 + }
  1208 +
  1209 + return rc;
  1210 +}
  1211 +
  1212 +/* Reads the holding registers of remote device and put the data into an
  1213 + array */
  1214 +int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
  1215 +{
  1216 + int status;
  1217 +
  1218 + if (ctx == NULL) {
  1219 + errno = EINVAL;
  1220 + return -1;
  1221 + }
  1222 +
  1223 + if (nb > MODBUS_MAX_READ_REGISTERS)
  1224 + {
  1225 + if (ctx->debug)
  1226 + {
  1227 + fprintf(stderr,
  1228 + "ERROR Too many registers requested (%d > %d)\n",
  1229 + nb, MODBUS_MAX_READ_REGISTERS);
  1230 + }
  1231 + errno = EMBMDATA;
  1232 + return -1;
  1233 + }
  1234 +
  1235 + status = read_registers(ctx, MODBUS_FC_READ_HOLDING_REGISTERS,
  1236 + addr, nb, dest);
  1237 + return status;
  1238 +}
  1239 +
  1240 +/* Reads the input registers of remote device and put the data into an array */
  1241 +int modbus_read_input_registers(modbus_t *ctx, int addr, int nb,
  1242 + uint16_t *dest)
  1243 +{
  1244 + int status;
  1245 +
  1246 + if (ctx == NULL) {
  1247 + errno = EINVAL;
  1248 + return -1;
  1249 + }
  1250 +
  1251 + if (nb > MODBUS_MAX_READ_REGISTERS) {
  1252 + fprintf(stderr,
  1253 + "ERROR Too many input registers requested (%d > %d)\n",
  1254 + nb, MODBUS_MAX_READ_REGISTERS);
  1255 + errno = EMBMDATA;
  1256 + return -1;
  1257 + }
  1258 +
  1259 + status = read_registers(ctx, MODBUS_FC_READ_INPUT_REGISTERS,
  1260 + addr, nb, dest);
  1261 +
  1262 + return status;
  1263 +}
  1264 +
  1265 +/* Write a value to the specified register of the remote device.
  1266 + Used by write_bit and write_register */
  1267 +static int write_single(modbus_t *ctx, int function, int addr, int value)
  1268 +{
  1269 + int rc;
  1270 + int req_length;
  1271 + uint8_t req[_MIN_REQ_LENGTH];
  1272 +
  1273 + if (ctx == NULL) {
  1274 + errno = EINVAL;
  1275 + return -1;
  1276 + }
  1277 +
  1278 + req_length = ctx->backend->build_request_basis(ctx, function, addr, value, req);
  1279 +
  1280 + rc = send_msg(ctx, req, req_length);
  1281 + if (rc > 0) {
  1282 + /* Used by write_bit and write_register */
  1283 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1284 +
  1285 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1286 + if (rc == -1)
  1287 + return -1;
  1288 +
  1289 + rc = check_confirmation(ctx, req, rsp, rc);
  1290 + }
  1291 +
  1292 + return rc;
  1293 +}
  1294 +
  1295 +/* Turns ON or OFF a single bit of the remote device */
  1296 +int modbus_write_bit(modbus_t *ctx, int addr, int status)
  1297 +{
  1298 + if (ctx == NULL) {
  1299 + errno = EINVAL;
  1300 + return -1;
  1301 + }
  1302 +
  1303 + return write_single(ctx, MODBUS_FC_WRITE_SINGLE_COIL, addr,
  1304 + status ? 0xFF00 : 0);
  1305 +}
  1306 +
  1307 +/* Writes a value in one register of the remote device */
  1308 +int modbus_write_register(modbus_t *ctx, int addr, int value)
  1309 +{
  1310 + if (ctx == NULL) {
  1311 + errno = EINVAL;
  1312 + return -1;
  1313 + }
  1314 +
  1315 + return write_single(ctx, MODBUS_FC_WRITE_SINGLE_REGISTER, addr, value);
  1316 +}
  1317 +
  1318 +/* Write the bits of the array in the remote device */
  1319 +int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *src)
  1320 +{
  1321 + int rc;
  1322 + int i;
  1323 + int byte_count;
  1324 + int req_length;
  1325 + int bit_check = 0;
  1326 + int pos = 0;
  1327 + uint8_t req[MAX_MESSAGE_LENGTH];
  1328 +
  1329 + if (ctx == NULL) {
  1330 + errno = EINVAL;
  1331 + return -1;
  1332 + }
  1333 +
  1334 + if (nb > MODBUS_MAX_WRITE_BITS) {
  1335 + if (ctx->debug) {
  1336 + fprintf(stderr, "ERROR Writing too many bits (%d > %d)\n",
  1337 + nb, MODBUS_MAX_WRITE_BITS);
  1338 + }
  1339 + errno = EMBMDATA;
  1340 + return -1;
  1341 + }
  1342 +
  1343 + req_length = ctx->backend->build_request_basis(ctx,
  1344 + MODBUS_FC_WRITE_MULTIPLE_COILS,
  1345 + addr, nb, req);
  1346 + byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
  1347 + req[req_length++] = byte_count;
  1348 +
  1349 + for (i = 0; i < byte_count; i++) {
  1350 + int bit;
  1351 +
  1352 + bit = 0x01;
  1353 + req[req_length] = 0;
  1354 +
  1355 + while ((bit & 0xFF) && (bit_check++ < nb)) {
  1356 + if (src[pos++])
  1357 + req[req_length] |= bit;
  1358 + else
  1359 + req[req_length] &=~ bit;
  1360 +
  1361 + bit = bit << 1;
  1362 + }
  1363 + req_length++;
  1364 + }
  1365 +
  1366 + rc = send_msg(ctx, req, req_length);
  1367 + if (rc > 0) {
  1368 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1369 +
  1370 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1371 + if (rc == -1)
  1372 + return -1;
  1373 +
  1374 + rc = check_confirmation(ctx, req, rsp, rc);
  1375 + }
  1376 +
  1377 +
  1378 + return rc;
  1379 +}
  1380 +
  1381 +/* Write the values from the array to the registers of the remote device */
  1382 +int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src)
  1383 +{
  1384 + int rc;
  1385 + int i;
  1386 + int req_length;
  1387 + int byte_count;
  1388 + uint8_t req[MAX_MESSAGE_LENGTH];
  1389 +
  1390 + if (ctx == NULL) {
  1391 + errno = EINVAL;
  1392 + return -1;
  1393 + }
  1394 +
  1395 + if (nb > MODBUS_MAX_WRITE_REGISTERS) {
  1396 + if (ctx->debug) {
  1397 + fprintf(stderr,
  1398 + "ERROR Trying to write to too many registers (%d > %d)\n",
  1399 + nb, MODBUS_MAX_WRITE_REGISTERS);
  1400 + }
  1401 + errno = EMBMDATA;
  1402 + return -1;
  1403 + }
  1404 +
  1405 + req_length = ctx->backend->build_request_basis(ctx,
  1406 + MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
  1407 + addr, nb, req);
  1408 + byte_count = nb * 2;
  1409 + req[req_length++] = byte_count;
  1410 +
  1411 + for (i = 0; i < nb; i++) {
  1412 + req[req_length++] = src[i] >> 8;
  1413 + req[req_length++] = src[i] & 0x00FF;
  1414 + }
  1415 +
  1416 + rc = send_msg(ctx, req, req_length);
  1417 + if (rc > 0) {
  1418 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1419 +
  1420 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1421 + if (rc == -1)
  1422 + return -1;
  1423 +
  1424 + rc = check_confirmation(ctx, req, rsp, rc);
  1425 + }
  1426 +
  1427 + return rc;
  1428 +}
  1429 +
  1430 +int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask)
  1431 +{
  1432 + int rc;
  1433 + int req_length;
  1434 + /* The request length can not exceed _MIN_REQ_LENGTH - 2 and 4 bytes to
  1435 + * store the masks. The ugly substraction is there to remove the 'nb' value
  1436 + * (2 bytes) which is not used. */
  1437 + uint8_t req[_MIN_REQ_LENGTH + 2];
  1438 +
  1439 + req_length = ctx->backend->build_request_basis(ctx,
  1440 + MODBUS_FC_MASK_WRITE_REGISTER,
  1441 + addr, 0, req);
  1442 +
  1443 + /* HACKISH, count is not used */
  1444 + req_length -= 2;
  1445 +
  1446 + req[req_length++] = and_mask >> 8;
  1447 + req[req_length++] = and_mask & 0x00ff;
  1448 + req[req_length++] = or_mask >> 8;
  1449 + req[req_length++] = or_mask & 0x00ff;
  1450 +
  1451 + rc = send_msg(ctx, req, req_length);
  1452 + if (rc > 0) {
  1453 + /* Used by write_bit and write_register */
  1454 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1455 +
  1456 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1457 + if (rc == -1)
  1458 + return -1;
  1459 +
  1460 + rc = check_confirmation(ctx, req, rsp, rc);
  1461 + }
  1462 +
  1463 + return rc;
  1464 +}
  1465 +
  1466 +/* Write multiple registers from src array to remote device and read multiple
  1467 + registers from remote device to dest array. */
  1468 +int modbus_write_and_read_registers(modbus_t *ctx,
  1469 + int write_addr, int write_nb,
  1470 + const uint16_t *src,
  1471 + int read_addr, int read_nb,
  1472 + uint16_t *dest)
  1473 +
  1474 +{
  1475 + int rc;
  1476 + int req_length;
  1477 + int i;
  1478 + int byte_count;
  1479 + uint8_t req[MAX_MESSAGE_LENGTH];
  1480 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1481 +
  1482 + if (ctx == NULL) {
  1483 + errno = EINVAL;
  1484 + return -1;
  1485 + }
  1486 +
  1487 + if (write_nb > MODBUS_MAX_WR_WRITE_REGISTERS) {
  1488 + if (ctx->debug) {
  1489 + fprintf(stderr,
  1490 + "ERROR Too many registers to write (%d > %d)\n",
  1491 + write_nb, MODBUS_MAX_WR_WRITE_REGISTERS);
  1492 + }
  1493 + errno = EMBMDATA;
  1494 + return -1;
  1495 + }
  1496 +
  1497 + if (read_nb > MODBUS_MAX_WR_READ_REGISTERS) {
  1498 + if (ctx->debug) {
  1499 + fprintf(stderr,
  1500 + "ERROR Too many registers requested (%d > %d)\n",
  1501 + read_nb, MODBUS_MAX_WR_READ_REGISTERS);
  1502 + }
  1503 + errno = EMBMDATA;
  1504 + return -1;
  1505 + }
  1506 + req_length = ctx->backend->build_request_basis(ctx,
  1507 + MODBUS_FC_WRITE_AND_READ_REGISTERS,
  1508 + read_addr, read_nb, req);
  1509 +
  1510 + req[req_length++] = write_addr >> 8;
  1511 + req[req_length++] = write_addr & 0x00ff;
  1512 + req[req_length++] = write_nb >> 8;
  1513 + req[req_length++] = write_nb & 0x00ff;
  1514 + byte_count = write_nb * 2;
  1515 + req[req_length++] = byte_count;
  1516 +
  1517 + for (i = 0; i < write_nb; i++) {
  1518 + req[req_length++] = src[i] >> 8;
  1519 + req[req_length++] = src[i] & 0x00FF;
  1520 + }
  1521 +
  1522 + rc = send_msg(ctx, req, req_length);
  1523 + if (rc > 0) {
  1524 + int offset;
  1525 +
  1526 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1527 + if (rc == -1)
  1528 + return -1;
  1529 +
  1530 + rc = check_confirmation(ctx, req, rsp, rc);
  1531 + if (rc == -1)
  1532 + return -1;
  1533 +
  1534 + offset = ctx->backend->header_length;
  1535 + for (i = 0; i < rc; i++) {
  1536 + /* shift reg hi_byte to temp OR with lo_byte */
  1537 + dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
  1538 + rsp[offset + 3 + (i << 1)];
  1539 + }
  1540 + }
  1541 +
  1542 + return rc;
  1543 +}
  1544 +
  1545 +/* Send a request to get the slave ID of the device (only available in serial
  1546 + communication). */
  1547 +int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest)
  1548 +{
  1549 + int rc;
  1550 + int req_length;
  1551 + uint8_t req[_MIN_REQ_LENGTH];
  1552 +
  1553 + if (ctx == NULL || max_dest <= 0) {
  1554 + errno = EINVAL;
  1555 + return -1;
  1556 + }
  1557 +
  1558 + req_length = ctx->backend->build_request_basis(ctx, MODBUS_FC_REPORT_SLAVE_ID,
  1559 + 0, 0, req);
  1560 +
  1561 + /* HACKISH, addr and count are not used */
  1562 + req_length -= 4;
  1563 +
  1564 + rc = send_msg(ctx, req, req_length);
  1565 + if (rc > 0) {
  1566 + int i;
  1567 + int offset;
  1568 + uint8_t rsp[MAX_MESSAGE_LENGTH];
  1569 +
  1570 + rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
  1571 + if (rc == -1)
  1572 + return -1;
  1573 +
  1574 + rc = check_confirmation(ctx, req, rsp, rc);
  1575 + if (rc == -1)
  1576 + return -1;
  1577 +
  1578 + offset = ctx->backend->header_length + 2;
  1579 +
  1580 + /* Byte count, slave id, run indicator status and
  1581 + additional data. Truncate copy to max_dest. */
  1582 + for (i=0; i < rc && i < max_dest; i++) {
  1583 + dest[i] = rsp[offset + i];
  1584 + }
  1585 + }
  1586 +
  1587 + return rc;
  1588 +}
  1589 +
  1590 +void _modbus_init_common(modbus_t *ctx)
  1591 +{
  1592 + /* Slave and socket are initialized to -1 */
  1593 + ctx->slave = -1;
  1594 + ctx->s = -1;
  1595 +
  1596 + ctx->debug = FALSE;
  1597 + ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE;
  1598 +
  1599 + ctx->response_timeout.tv_sec = 0;
  1600 + ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT;
  1601 +
  1602 + ctx->byte_timeout.tv_sec = 0;
  1603 + ctx->byte_timeout.tv_usec = _BYTE_TIMEOUT;
  1604 +}
  1605 +
  1606 +/* Define the slave number */
  1607 +int modbus_set_slave(modbus_t *ctx, int slave)
  1608 +{
  1609 + if (ctx == NULL) {
  1610 + errno = EINVAL;
  1611 + return -1;
  1612 + }
  1613 +
  1614 + return ctx->backend->set_slave(ctx, slave);
  1615 +}
  1616 +
  1617 +int modbus_set_error_recovery(modbus_t *ctx,
  1618 + modbus_error_recovery_mode error_recovery)
  1619 +{
  1620 + if (ctx == NULL) {
  1621 + errno = EINVAL;
  1622 + return -1;
  1623 + }
  1624 +
  1625 + /* The type of modbus_error_recovery_mode is unsigned enum */
  1626 + ctx->error_recovery = (uint8_t) error_recovery;
  1627 + return 0;
  1628 +}
  1629 +
  1630 +int modbus_set_socket(modbus_t *ctx, int s)
  1631 +{
  1632 + if (ctx == NULL) {
  1633 + errno = EINVAL;
  1634 + return -1;
  1635 + }
  1636 +
  1637 + ctx->s = s;
  1638 + return 0;
  1639 +}
  1640 +
  1641 +int modbus_get_socket(modbus_t *ctx)
  1642 +{
  1643 + if (ctx == NULL) {
  1644 + errno = EINVAL;
  1645 + return -1;
  1646 + }
  1647 +
  1648 + return ctx->s;
  1649 +}
  1650 +
  1651 +/* Get the timeout interval used to wait for a response */
  1652 +int modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
  1653 +{
  1654 + if (ctx == NULL) {
  1655 + errno = EINVAL;
  1656 + return -1;
  1657 + }
  1658 +
  1659 + *to_sec = ctx->response_timeout.tv_sec;
  1660 + *to_usec = ctx->response_timeout.tv_usec;
  1661 + return 0;
  1662 +}
  1663 +
  1664 +int modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
  1665 +{
  1666 + if (ctx == NULL ||
  1667 + (to_sec == 0 && to_usec == 0) || to_usec > 999999) {
  1668 + errno = EINVAL;
  1669 + return -1;
  1670 + }
  1671 +
  1672 + ctx->response_timeout.tv_sec = to_sec;
  1673 + ctx->response_timeout.tv_usec = to_usec;
  1674 + return 0;
  1675 +}
  1676 +
  1677 +/* Get the timeout interval between two consecutive bytes of a message */
  1678 +int modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
  1679 +{
  1680 + if (ctx == NULL) {
  1681 + errno = EINVAL;
  1682 + return -1;
  1683 + }
  1684 +
  1685 + *to_sec = ctx->byte_timeout.tv_sec;
  1686 + *to_usec = ctx->byte_timeout.tv_usec;
  1687 + return 0;
  1688 +}
  1689 +
  1690 +int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
  1691 +{
  1692 + /* Byte timeout can be disabled when both values are zero */
  1693 + if (ctx == NULL || to_usec > 999999) {
  1694 + errno = EINVAL;
  1695 + return -1;
  1696 + }
  1697 +
  1698 + ctx->byte_timeout.tv_sec = to_sec;
  1699 + ctx->byte_timeout.tv_usec = to_usec;
  1700 + return 0;
  1701 +}
  1702 +
  1703 +int modbus_get_header_length(modbus_t *ctx)
  1704 +{
  1705 + if (ctx == NULL) {
  1706 + errno = EINVAL;
  1707 + return -1;
  1708 + }
  1709 +
  1710 + return ctx->backend->header_length;
  1711 +}
  1712 +
  1713 +int modbus_connect(modbus_t *ctx)
  1714 +{
  1715 + if (ctx == NULL) {
  1716 + errno = EINVAL;
  1717 + return -1;
  1718 + }
  1719 +
  1720 + return ctx->backend->connect(ctx);
  1721 +}
  1722 +
  1723 +void modbus_close(modbus_t *ctx)
  1724 +{
  1725 + if (ctx == NULL)
  1726 + return;
  1727 +
  1728 + ctx->backend->close(ctx);
  1729 +}
  1730 +
  1731 +void modbus_free(modbus_t *ctx)
  1732 +{
  1733 + if (ctx == NULL)
  1734 + return;
  1735 +
  1736 + ctx->backend->free(ctx);
  1737 +}
  1738 +
  1739 +int modbus_set_debug(modbus_t *ctx, int flag)
  1740 +{
  1741 + if (ctx == NULL) {
  1742 + errno = EINVAL;
  1743 + return -1;
  1744 + }
  1745 +
  1746 + ctx->debug = flag;
  1747 + return 0;
  1748 +}
  1749 +
  1750 +/* Allocates 4 arrays to store bits, input bits, registers and inputs
  1751 + registers. The pointers are stored in modbus_mapping structure.
  1752 +
  1753 + The modbus_mapping_new_ranges() function shall return the new allocated
  1754 + structure if successful. Otherwise it shall return NULL and set errno to
  1755 + ENOMEM. */
  1756 +modbus_mapping_t* modbus_mapping_new_start_address(
  1757 + unsigned int start_bits, unsigned int nb_bits,
  1758 + unsigned int start_input_bits, unsigned int nb_input_bits,
  1759 + unsigned int start_registers, unsigned int nb_registers,
  1760 + unsigned int start_input_registers, unsigned int nb_input_registers)
  1761 +{
  1762 + modbus_mapping_t *mb_mapping;
  1763 +
  1764 + mb_mapping = (modbus_mapping_t *)malloc(sizeof(modbus_mapping_t));
  1765 + if (mb_mapping == NULL) {
  1766 + return NULL;
  1767 + }
  1768 +
  1769 + /* 0X */
  1770 + mb_mapping->nb_bits = nb_bits;
  1771 + mb_mapping->start_bits = start_bits;
  1772 + if (nb_bits == 0) {
  1773 + mb_mapping->tab_bits = NULL;
  1774 + } else {
  1775 + /* Negative number raises a POSIX error */
  1776 + mb_mapping->tab_bits =
  1777 + (uint8_t *) malloc(nb_bits * sizeof(uint8_t));
  1778 + if (mb_mapping->tab_bits == NULL) {
  1779 + free(mb_mapping);
  1780 + return NULL;
  1781 + }
  1782 + memset(mb_mapping->tab_bits, 0, nb_bits * sizeof(uint8_t));
  1783 + }
  1784 +
  1785 + /* 1X */
  1786 + mb_mapping->nb_input_bits = nb_input_bits;
  1787 + mb_mapping->start_input_bits = start_input_bits;
  1788 + if (nb_input_bits == 0) {
  1789 + mb_mapping->tab_input_bits = NULL;
  1790 + } else {
  1791 + mb_mapping->tab_input_bits =
  1792 + (uint8_t *) malloc(nb_input_bits * sizeof(uint8_t));
  1793 + if (mb_mapping->tab_input_bits == NULL) {
  1794 + free(mb_mapping->tab_bits);
  1795 + free(mb_mapping);
  1796 + return NULL;
  1797 + }
  1798 + memset(mb_mapping->tab_input_bits, 0, nb_input_bits * sizeof(uint8_t));
  1799 + }
  1800 +
  1801 + /* 4X */
  1802 + mb_mapping->nb_registers = nb_registers;
  1803 + mb_mapping->start_registers = start_registers;
  1804 + if (nb_registers == 0) {
  1805 + mb_mapping->tab_registers = NULL;
  1806 + } else {
  1807 + mb_mapping->tab_registers =
  1808 + (uint16_t *) malloc(nb_registers * sizeof(uint16_t));
  1809 + if (mb_mapping->tab_registers == NULL) {
  1810 + free(mb_mapping->tab_input_bits);
  1811 + free(mb_mapping->tab_bits);
  1812 + free(mb_mapping);
  1813 + return NULL;
  1814 + }
  1815 + memset(mb_mapping->tab_registers, 0, nb_registers * sizeof(uint16_t));
  1816 + }
  1817 +
  1818 + /* 3X */
  1819 + mb_mapping->nb_input_registers = nb_input_registers;
  1820 + mb_mapping->start_input_registers = start_input_registers;
  1821 + if (nb_input_registers == 0) {
  1822 + mb_mapping->tab_input_registers = NULL;
  1823 + } else {
  1824 + mb_mapping->tab_input_registers =
  1825 + (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
  1826 + if (mb_mapping->tab_input_registers == NULL) {
  1827 + free(mb_mapping->tab_registers);
  1828 + free(mb_mapping->tab_input_bits);
  1829 + free(mb_mapping->tab_bits);
  1830 + free(mb_mapping);
  1831 + return NULL;
  1832 + }
  1833 + memset(mb_mapping->tab_input_registers, 0,
  1834 + nb_input_registers * sizeof(uint16_t));
  1835 + }
  1836 +
  1837 + return mb_mapping;
  1838 +}
  1839 +
  1840 +modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits,
  1841 + int nb_registers, int nb_input_registers)
  1842 +{
  1843 + return modbus_mapping_new_start_address(
  1844 + 0, nb_bits, 0, nb_input_bits, 0, nb_registers, 0, nb_input_registers);
  1845 +}
  1846 +
  1847 +/* Frees the 4 arrays */
  1848 +void modbus_mapping_free(modbus_mapping_t *mb_mapping)
  1849 +{
  1850 + if (mb_mapping == NULL) {
  1851 + return;
  1852 + }
  1853 +
  1854 + free(mb_mapping->tab_input_registers);
  1855 + free(mb_mapping->tab_registers);
  1856 + free(mb_mapping->tab_input_bits);
  1857 + free(mb_mapping->tab_bits);
  1858 + free(mb_mapping);
  1859 +}
  1860 +
  1861 +#ifndef HAVE_STRLCPY
  1862 +/*
  1863 + * Function strlcpy was originally developed by
  1864 + * Todd C. Miller <Todd.Miller@courtesan.com> to simplify writing secure code.
  1865 + * See ftp://ftp.openbsd.org/pub/OpenBSD/src/lib/libc/string/strlcpy.3
  1866 + * for more information.
  1867 + *
  1868 + * Thank you Ulrich Drepper... not!
  1869 + *
  1870 + * Copy src to string dest of size dest_size. At most dest_size-1 characters
  1871 + * will be copied. Always NUL terminates (unless dest_size == 0). Returns
  1872 + * strlen(src); if retval >= dest_size, truncation occurred.
  1873 + */
  1874 +size_t strlcpy(char *dest, const char *src, size_t dest_size)
  1875 +{
  1876 + register char *d = dest;
  1877 + register const char *s = src;
  1878 + register size_t n = dest_size;
  1879 +
  1880 + /* Copy as many bytes as will fit */
  1881 + if (n != 0 && --n != 0) {
  1882 + do {
  1883 + if ((*d++ = *s++) == 0)
  1884 + break;
  1885 + } while (--n != 0);
  1886 + }
  1887 +
  1888 + /* Not enough room in dest, add NUL and traverse rest of src */
  1889 + if (n == 0) {
  1890 + if (dest_size != 0)
  1891 + *d = '\0'; /* NUL-terminate dest */
  1892 + while (*s++)
  1893 + ;
  1894 + }
  1895 +
  1896 + return (s - src - 1); /* count does not include NUL */
  1897 +}
  1898 +#endif
... ...
stack/modbus.h 0 → 100644
  View file @5735b40
  1 +
  2 +#pragma once
  3 +
  4 +/* Add this for macros that defined unix flavor */
  5 +#if (defined(__unix__) || defined(unix)) && !defined(USG)
  6 +#include <sys/param.h>
  7 +#endif
  8 +
  9 +#include <stdint.h>
  10 +
  11 +#define MODBUS_API
  12 +
  13 +#define MODBUS_VERSION_STRING "1.0.0"
  14 +
  15 +#ifdef __cplusplus
  16 +#define MODBUS_BEGIN_DECLS extern "C" {
  17 +#define MODBUS_END_DECLS }
  18 +#else
  19 +# define MODBUS_BEGIN_DECLS
  20 +# define MODBUS_END_DECLS
  21 +#endif
  22 +
  23 +MODBUS_BEGIN_DECLS
  24 +
  25 +#ifndef FALSE
  26 +#define FALSE 0
  27 +#endif
  28 +
  29 +#ifndef TRUE
  30 +#define TRUE 1
  31 +#endif
  32 +
  33 +#ifndef OFF
  34 +#define OFF 0
  35 +#endif
  36 +
  37 +#ifndef ON
  38 +#define ON 1
  39 +#endif
  40 +
  41 +/* Modbus function codes */
  42 +#define MODBUS_FC_READ_COILS 0x01
  43 +#define MODBUS_FC_READ_DISCRETE_INPUTS 0x02
  44 +#define MODBUS_FC_READ_HOLDING_REGISTERS 0x03
  45 +#define MODBUS_FC_READ_INPUT_REGISTERS 0x04
  46 +#define MODBUS_FC_WRITE_SINGLE_COIL 0x05
  47 +#define MODBUS_FC_WRITE_SINGLE_REGISTER 0x06
  48 +#define MODBUS_FC_READ_EXCEPTION_STATUS 0x07
  49 +#define MODBUS_FC_WRITE_MULTIPLE_COILS 0x0F
  50 +#define MODBUS_FC_WRITE_MULTIPLE_REGISTERS 0x10
  51 +#define MODBUS_FC_REPORT_SLAVE_ID 0x11
  52 +#define MODBUS_FC_MASK_WRITE_REGISTER 0x16
  53 +#define MODBUS_FC_WRITE_AND_READ_REGISTERS 0x17
  54 +
  55 +#define MODBUS_BROADCAST_ADDRESS 0
  56 +
  57 +/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 1 page 12)
  58 + * Quantity of Coils to read (2 bytes): 1 to 2000 (0x7D0)
  59 + * (chapter 6 section 11 page 29)
  60 + * Quantity of Coils to write (2 bytes): 1 to 1968 (0x7B0)
  61 + */
  62 +#define MODBUS_MAX_READ_BITS 2000
  63 +#define MODBUS_MAX_WRITE_BITS 1968
  64 +
  65 +/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 3 page 15)
  66 + * Quantity of Registers to read (2 bytes): 1 to 125 (0x7D)
  67 + * (chapter 6 section 12 page 31)
  68 + * Quantity of Registers to write (2 bytes) 1 to 123 (0x7B)
  69 + * (chapter 6 section 17 page 38)
  70 + * Quantity of Registers to write in R/W registers (2 bytes) 1 to 121 (0x79)
  71 + */
  72 +#define MODBUS_MAX_READ_REGISTERS 125
  73 +#define MODBUS_MAX_WRITE_REGISTERS 123
  74 +#define MODBUS_MAX_WR_WRITE_REGISTERS 121
  75 +#define MODBUS_MAX_WR_READ_REGISTERS 125
  76 +
  77 +/* The size of the MODBUS PDU is limited by the size constraint inherited from
  78 + * the first MODBUS implementation on Serial Line network (max. RS485 ADU = 256
  79 + * bytes). Therefore, MODBUS PDU for serial line communication = 256 - Server
  80 + * address (1 byte) - CRC (2 bytes) = 253 bytes.
  81 + */
  82 +#define MODBUS_MAX_PDU_LENGTH 253
  83 +
  84 +/* Consequently:
  85 + * - RTU MODBUS ADU = 253 bytes + Server address (1 byte) + CRC (2 bytes) = 256
  86 + * bytes.
  87 + * - TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes.
  88 + * so the maximum of both backend in 260 bytes. This size can used to allocate
  89 + * an array of bytes to store responses and it will be compatible with the two
  90 + * backends.
  91 + */
  92 +#define MODBUS_MAX_ADU_LENGTH 260
  93 +
  94 +/* Random number to avoid errno conflicts */
  95 +#define MODBUS_ENOBASE 112345678
  96 +
  97 +/* Protocol exceptions */
  98 +enum {
  99 + MODBUS_EXCEPTION_ILLEGAL_FUNCTION = 0x01,
  100 + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
  101 + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
  102 + MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE,
  103 + MODBUS_EXCEPTION_ACKNOWLEDGE,
  104 + MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY,
  105 + MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE,
  106 + MODBUS_EXCEPTION_MEMORY_PARITY,
  107 + MODBUS_EXCEPTION_NOT_DEFINED,
  108 + MODBUS_EXCEPTION_GATEWAY_PATH,
  109 + MODBUS_EXCEPTION_GATEWAY_TARGET,
  110 + MODBUS_EXCEPTION_MAX
  111 +};
  112 +
  113 +#define EMBXILFUN (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_FUNCTION)
  114 +#define EMBXILADD (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS)
  115 +#define EMBXILVAL (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE)
  116 +#define EMBXSFAIL (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE)
  117 +#define EMBXACK (MODBUS_ENOBASE + MODBUS_EXCEPTION_ACKNOWLEDGE)
  118 +#define EMBXSBUSY (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY)
  119 +#define EMBXNACK (MODBUS_ENOBASE + MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE)
  120 +#define EMBXMEMPAR (MODBUS_ENOBASE + MODBUS_EXCEPTION_MEMORY_PARITY)
  121 +#define EMBXGPATH (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_PATH)
  122 +#define EMBXGTAR (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_TARGET)
  123 +
  124 +/* Native libmodbus error codes */
  125 +#define EMBBADCRC (EMBXGTAR + 1)
  126 +#define EMBBADDATA (EMBXGTAR + 2)
  127 +#define EMBBADEXC (EMBXGTAR + 3)
  128 +#define EMBUNKEXC (EMBXGTAR + 4)
  129 +#define EMBMDATA (EMBXGTAR + 5)
  130 +#define EMBBADSLAVE (EMBXGTAR + 6)
  131 +
  132 +typedef struct _modbus modbus_t;
  133 +
  134 +typedef struct {
  135 + int nb_bits;
  136 + int start_bits;
  137 + int nb_input_bits;
  138 + int start_input_bits;
  139 + int nb_input_registers;
  140 + int start_input_registers;
  141 + int nb_registers;
  142 + int start_registers;
  143 + uint8_t *tab_bits;
  144 + uint8_t *tab_input_bits;
  145 + uint16_t *tab_input_registers;
  146 + uint16_t *tab_registers;
  147 +} modbus_mapping_t;
  148 +
  149 +typedef enum
  150 +{
  151 + MODBUS_ERROR_RECOVERY_NONE = 0,
  152 + MODBUS_ERROR_RECOVERY_LINK = (1<<1),
  153 + MODBUS_ERROR_RECOVERY_PROTOCOL = (1<<2)
  154 +} modbus_error_recovery_mode;
  155 +
  156 +MODBUS_API int modbus_set_slave(modbus_t* ctx, int slave);
  157 +MODBUS_API int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_mode error_recovery);
  158 +MODBUS_API int modbus_set_socket(modbus_t *ctx, int s);
  159 +MODBUS_API int modbus_get_socket(modbus_t *ctx);
  160 +
  161 +MODBUS_API int modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
  162 +MODBUS_API int modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
  163 +
  164 +MODBUS_API int modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
  165 +MODBUS_API int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
  166 +
  167 +MODBUS_API int modbus_get_header_length(modbus_t *ctx);
  168 +
  169 +MODBUS_API int modbus_connect(modbus_t *ctx);
  170 +MODBUS_API void modbus_close(modbus_t *ctx);
  171 +
  172 +MODBUS_API void modbus_free(modbus_t *ctx);
  173 +
  174 +MODBUS_API int modbus_flush(modbus_t *ctx);
  175 +MODBUS_API int modbus_set_debug(modbus_t *ctx, int flag);
  176 +
  177 +MODBUS_API const char *modbus_strerror(int errnum);
  178 +
  179 +MODBUS_API int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
  180 +MODBUS_API int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
  181 +MODBUS_API int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
  182 +MODBUS_API int modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
  183 +MODBUS_API int modbus_write_bit(modbus_t *ctx, int coil_addr, int status);
  184 +MODBUS_API int modbus_write_register(modbus_t *ctx, int reg_addr, int value);
  185 +MODBUS_API int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
  186 +MODBUS_API int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
  187 +MODBUS_API int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask);
  188 +MODBUS_API int modbus_write_and_read_registers(modbus_t *ctx, int write_addr, int write_nb,const uint16_t *src, int read_addr, int read_nb,uint16_t *dest);
  189 +MODBUS_API int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest);
  190 +
  191 +MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address
  192 +(
  193 + unsigned int start_bits, unsigned int nb_bits,
  194 + unsigned int start_input_bits, unsigned int nb_input_bits,
  195 + unsigned int start_registers, unsigned int nb_registers,
  196 + unsigned int start_input_registers, unsigned int nb_input_registers
  197 +);
  198 +
  199 +MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits, int nb_registers, int nb_input_registers);
  200 +MODBUS_API void modbus_mapping_free(modbus_mapping_t *mb_mapping);
  201 +
  202 +MODBUS_API int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length);
  203 +
  204 +MODBUS_API int modbus_receive(modbus_t *ctx, uint8_t *req);
  205 +
  206 +MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp);
  207 +
  208 +MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping);
  209 +MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code);
  210 +
  211 +/**
  212 + * UTILS FUNCTIONS
  213 + **/
  214 +
  215 +#define MODBUS_GET_HIGH_BYTE(data) (((data) >> 8) & 0xFF)
  216 +#define MODBUS_GET_LOW_BYTE(data) ((data) & 0xFF)
  217 +#define MODBUS_GET_INT64_FROM_INT16(tab_int16, index) \
  218 + (((int64_t)tab_int16[(index) ] << 48) + \
  219 + ((int64_t)tab_int16[(index) + 1] << 32) + \
  220 + ((int64_t)tab_int16[(index) + 2] << 16) + \
  221 + (int64_t)tab_int16[(index) + 3])
  222 +#define MODBUS_GET_INT32_FROM_INT16(tab_int16, index) ((tab_int16[(index)] << 16) + tab_int16[(index) + 1])
  223 +#define MODBUS_GET_INT16_FROM_INT8(tab_int8, index) ((tab_int8[(index)] << 8) + tab_int8[(index) + 1])
  224 +#define MODBUS_SET_INT16_TO_INT8(tab_int8, index, value) \
  225 + do { \
  226 + tab_int8[(index)] = (value) >> 8; \
  227 + tab_int8[(index) + 1] = (value) & 0xFF; \
  228 + } while (0)
  229 +#define MODBUS_SET_INT32_TO_INT16(tab_int16, index, value) \
  230 + do { \
  231 + tab_int16[(index) ] = (value) >> 16; \
  232 + tab_int16[(index) + 1] = (value); \
  233 + } while (0)
  234 +#define MODBUS_SET_INT64_TO_INT16(tab_int16, index, value) \
  235 + do { \
  236 + tab_int16[(index) ] = (value) >> 48; \
  237 + tab_int16[(index) + 1] = (value) >> 32; \
  238 + tab_int16[(index) + 2] = (value) >> 16; \
  239 + tab_int16[(index) + 3] = (value); \
  240 + } while (0)
  241 +
  242 +MODBUS_API void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value);
  243 +MODBUS_API void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
  244 + const uint8_t *tab_byte);
  245 +MODBUS_API uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx, unsigned int nb_bits);
  246 +MODBUS_API float modbus_get_float(const uint16_t *src);
  247 +MODBUS_API float modbus_get_float_abcd(const uint16_t *src);
  248 +MODBUS_API float modbus_get_float_dcba(const uint16_t *src);
  249 +MODBUS_API float modbus_get_float_badc(const uint16_t *src);
  250 +MODBUS_API float modbus_get_float_cdab(const uint16_t *src);
  251 +
  252 +MODBUS_API void modbus_set_float(float f, uint16_t *dest);
  253 +MODBUS_API void modbus_set_float_abcd(float f, uint16_t *dest);
  254 +MODBUS_API void modbus_set_float_dcba(float f, uint16_t *dest);
  255 +MODBUS_API void modbus_set_float_badc(float f, uint16_t *dest);
  256 +MODBUS_API void modbus_set_float_cdab(float f, uint16_t *dest);
  257 +
  258 +#include "modbus-tcp.h"
  259 +#include "modbus-rtu.h"
  260 +
  261 +MODBUS_END_DECLS
  262 +
... ...