Revert "Setting up working version"

This reverts commit 783ce3c583f4d6094ed7e9a68c41534fc2bceffd.

Revert "Setting up working version"
This reverts commit 783ce3c583f4d6094ed7e9a68c41534fc2bceffd.
Peter M. Groen
1 parent 783ce3c5
Showing 29 changed files with 6101 additions and 648 deletions
3rdparty/libmodbus/config.h
3rdparty/libmodbus/modbus-data.c
3rdparty/libmodbus/modbus-private.h
3rdparty/libmodbus/modbus-rtu-private.h
3rdparty/libmodbus/modbus-rtu.c
3rdparty/libmodbus/modbus-rtu.h
3rdparty/libmodbus/modbus-tcp-private.h
3rdparty/libmodbus/modbus-tcp.c
3rdparty/libmodbus/modbus-tcp.h
3rdparty/libmodbus/modbus-version.h
3rdparty/libmodbus/modbus.c
3rdparty/libmodbus/modbus.h
src/ModbusAdapter.cpp
src/ModbusAdapter.h
src/ModbusConnections.cpp
src/ModbusConnections.h
src/main.cpp
stack/config.h
stack/modbus-data.c
stack/modbus-private.h
+/* config.h.  Generated from config.h.in by configure.  */
+/* config.h.in.  Generated from configure.ac by autoheader.  */
+
 /* Define to 1 if you have the <arpa/inet.h> header file. */
 #define HAVE_ARPA_INET_H 1
-/* Define to 1 if you have the declaration of `TIOCSRS485', and to 0 if you don't. */
+/* Define to 1 if you have the declaration of `TIOCSRS485', and to 0 if you
+   don't. */
 #define HAVE_DECL_TIOCSRS485 0
-/* Define to 1 if you have the declaration of `TIOCM_RTS', and to 0 if you don't. */
+/* Define to 1 if you have the declaration of `TIOCM_RTS', and to 0 if you
+   don't. */
 #define HAVE_DECL_TIOCM_RTS 0
-/* Define to 1 if you have the declaration of `__CYGWIN__', and to 0 if you don't. */
+/* Define to 1 if you have the declaration of `__CYGWIN__', and to 0 if you
+   don't. */
 #define HAVE_DECL___CYGWIN__ 0
 /* Define to 1 if you have the <dlfcn.h> header file. */
@@ -135,13 +141,13 @@
 #define PACKAGE_STRING "libmodbus 3.1.0-1"
 /* Define to the one symbol short name of this package. */
-#define PACKAGE_TARNAME "modbus"
+#define PACKAGE_TARNAME "libmodbus"
 /* Define to the home page for this package. */
 #define PACKAGE_URL ""
 /* Define to the version of this package. */
-#define PACKAGE_VERSION "1.0.0"
+#define PACKAGE_VERSION "3.1.0-1"
 /* Define to 1 if you have the ANSI C header files. */
 #define STDC_HEADERS 1
@@ -150,7 +156,7 @@
 #define TIME_WITH_SYS_TIME 1
 /* Version number of package */
-#define VERSION "1.0.0"
+#define VERSION "3.1.0-1"
 /* Define to empty if `const' does not conform to ANSI C. */
 /* #undef const */
+/*
+ * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
 #include <stdlib.h>
-#include "stdint.h"
+#ifndef _MSC_VER
+#  include <stdint.h>
+#else
+#  include "stdint.h"
+#endif
 #include <string.h>
 #include <assert.h>
-#include <arpa/inet.h>
+
+#if defined(_WIN32)
+#  include <winsock2.h>
+#else
+#  include <arpa/inet.h>
+#endif
+
 #include <config.h>
 #include "modbus.h"
@@ -14,6 +29,13 @@
 #  include <byteswap.h>
 #endif
+#if defined(__APPLE__)
+#  include <libkern/OSByteOrder.h>
+#  define bswap_16 OSSwapInt16
+#  define bswap_32 OSSwapInt32
+#  define bswap_64 OSSwapInt64
+#endif
+
 #if defined(__GNUC__)
 #  define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
 #  if GCC_VERSION >= 430
@@ -23,8 +45,13 @@
 #  endif
 #endif
+#if defined(_MSC_VER) && (_MSC_VER >= 1400)
+#  define bswap_32 _byteswap_ulong
+#  define bswap_16 _byteswap_ushort
+#endif
+
 #if !defined(__CYGWIN__) && !defined(bswap_16)
-#pragma  message "Fallback on C functions for bswap_16"
+#  warning "Fallback on C functions for bswap_16"
 static inline uint16_t bswap_16(uint16_t x)
 {
     return (x >> 8) | (x << 8);
@@ -32,7 +59,7 @@ static inline uint16_t bswap_16(uint16_t x)
 #endif
 #if !defined(bswap_32)
-#pragma  message "Fallback on C functions for bswap_32"
+#  warning "Fallback on C functions for bswap_32"
 static inline uint32_t bswap_32(uint32_t x)
 {
     return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
@@ -41,25 +68,24 @@ static inline uint32_t bswap_32(uint32_t x)
 /* Sets many bits from a single byte value (all 8 bits of the byte value are
    set) */
-void modbus_set_bits_from_byte( uint8_t *dest, int idx, const uint8_t value )
+void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value)
 {
     int i;
-    for (i=0; i < 8; i++) 
-    {
+    for (i=0; i < 8; i++) {
         dest[idx+i] = (value & (1 << i)) ? 1 : 0;
     }
 }
 /* Sets many bits from a table of bytes (only the bits between idx and
    idx + nb_bits are set) */
-void modbus_set_bits_from_bytes( uint8_t *dest, int idx, unsigned int nb_bits, const uint8_t *tab_byte )
+void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
+                                const uint8_t *tab_byte)
 {
     unsigned int i;
     int shift = 0;
-    for ( i = idx; i < idx + nb_bits; i++ ) 
-    {
+    for (i = idx; i < idx + nb_bits; i++) {
         dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
         /* gcc doesn't like: shift = (++shift) % 8; */
         shift++;
@@ -69,20 +95,19 @@ void modbus_set_bits_from_bytes( uint8_t *dest, int idx, unsigned int nb_bits, c
 /* Gets the byte value from many bits.
    To obtain a full byte, set nb_bits to 8. */
-uint8_t modbus_get_byte_from_bits( const uint8_t *src, int idx, unsigned int nb_bits )
+uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx,
+                                  unsigned int nb_bits)
 {
     unsigned int i;
     uint8_t value = 0;
-    if (nb_bits > 8) 
-    {
+    if (nb_bits > 8) {
         /* Assert is ignored if NDEBUG is set */
         assert(nb_bits < 8);
         nb_bits = 8;
     }
-    for (i=0; i < nb_bits; i++) 
-    {
+    for (i=0; i < nb_bits; i++) {
         value |= (src[idx+i] << i);
     }
+/*
+ * Copyright © 2010-2012 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
-#pragma once
+#ifndef MODBUS_PRIVATE_H
+#define MODBUS_PRIVATE_H
+#ifndef _MSC_VER
 # include <stdint.h>
 # include <sys/time.h>
+#else
+# include "stdint.h"
+# include <time.h>
+typedef int ssize_t;
+#endif
 #include <sys/types.h>
 #include <config.h>
@@ -18,15 +30,16 @@ MODBUS_BEGIN_DECLS
  * - HEADER_LENGTH_RTU (1) + function (1) + address (2) + number (2) + CRC (2)
  */
 #define _MIN_REQ_LENGTH 12
+
 #define _REPORT_SLAVE_ID 180
+
 #define _MODBUS_EXCEPTION_RSP_LENGTH 5
 /* Timeouts in microsecond (0.5 s) */
 #define _RESPONSE_TIMEOUT    500000
 #define _BYTE_TIMEOUT        500000
-typedef enum 
-{
+typedef enum {
     _MODBUS_BACKEND_TYPE_RTU=0,
     _MODBUS_BACKEND_TYPE_TCP
 } modbus_backend_type_t;
@@ -36,8 +49,7 @@ typedef enum
  *  | Client | ---------------------->| Server |
  *  ---------- Confirmation  Response ----------
  */
-typedef enum 
-{
+typedef enum {
     /* Request message on the server side */
     MSG_INDICATION,
     /* Request message on the client side */
@@ -46,29 +58,30 @@ typedef enum
 /* This structure reduces the number of params in functions and so
  * optimizes the speed of execution (~ 37%). */
-typedef struct _sft 
-{
+typedef struct _sft {
     int slave;
     int function;
     int t_id;
 } sft_t;
-typedef struct _modbus_backend 
-{
+typedef struct _modbus_backend {
     unsigned int backend_type;
     unsigned int header_length;
     unsigned int checksum_length;
     unsigned int max_adu_length;
     int (*set_slave) (modbus_t *ctx, int slave);
-    int (*build_request_basis) (modbus_t *ctx, int function, int addr, int nb, uint8_t *req);
+    int (*build_request_basis) (modbus_t *ctx, int function, int addr,
+                                int nb, uint8_t *req);
     int (*build_response_basis) (sft_t *sft, uint8_t *rsp);
     int (*prepare_response_tid) (const uint8_t *req, int *req_length);
     int (*send_msg_pre) (uint8_t *req, int req_length);
     ssize_t (*send) (modbus_t *ctx, const uint8_t *req, int req_length);
     int (*receive) (modbus_t *ctx, uint8_t *req);
     ssize_t (*recv) (modbus_t *ctx, uint8_t *rsp, int rsp_length);
-    int (*check_integrity) (modbus_t *ctx, uint8_t *msg, const int msg_length);
-    int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length);
+    int (*check_integrity) (modbus_t *ctx, uint8_t *msg,
+                            const int msg_length);
+    int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req,
+                                   const uint8_t *rsp, int rsp_length);
     int (*connect) (modbus_t *ctx);
     void (*close) (modbus_t *ctx);
     int (*flush) (modbus_t *ctx);
@@ -76,8 +89,7 @@ typedef struct _modbus_backend
     void (*free) (modbus_t *ctx);
 } modbus_backend_t;
-struct _modbus 
-{
+struct _modbus {
     /* Slave address */
     int slave;
     /* Socket or file descriptor */
@@ -99,3 +111,5 @@ size_t strlcpy(char *dest, const char *src, size_t dest_size);
 #endif
 MODBUS_END_DECLS
+
+#endif  /* MODBUS_PRIVATE_H */
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
-#pragma once
+#ifndef MODBUS_RTU_PRIVATE_H
+#define MODBUS_RTU_PRIVATE_H
+#ifndef _MSC_VER
 #include <stdint.h>
+#else
+#include "stdint.h"
+#endif
+
+#if defined(_WIN32)
+#include <windows.h>
+#else
 #include <termios.h>
+#endif
 #define _MODBUS_RTU_HEADER_LENGTH      1
 #define _MODBUS_RTU_PRESET_REQ_LENGTH  6
@@ -10,8 +25,24 @@
 #define _MODBUS_RTU_CHECKSUM_LENGTH    2
-typedef struct _modbus_rtu
-{
+#if defined(_WIN32)
+#if !defined(ENOTSUP)
+#define ENOTSUP WSAEOPNOTSUPP
+#endif
+
+/* WIN32: struct containing serial handle and a receive buffer */
+#define PY_BUF_SIZE 512
+struct win32_ser {
+    /* File handle */
+    HANDLE fd;
+    /* Receive buffer */
+    uint8_t buf[PY_BUF_SIZE];
+    /* Received chars */
+    DWORD n_bytes;
+};
+#endif /* _WIN32 */
+
+typedef struct _modbus_rtu {
     /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
     char *device;
     /* Bauds: 9600, 19200, 57600, 115200, etc */
@@ -22,13 +53,25 @@ typedef struct _modbus_rtu
     uint8_t stop_bit;
     /* Parity: 'N', 'O', 'E' */
     char parity;
+#if defined(_WIN32)
+    struct win32_ser w_ser;
+    DCB old_dcb;
+#else
     /* Save old termios settings */
     struct termios old_tios;
-
+#endif
 #if HAVE_DECL_TIOCSRS485
     int serial_mode;
 #endif
-
+//***Not part of libmodbus - added for QModMaster***//
+#if defined(_WIN32) || HAVE_DECL_TIOCM_RTS
+    int rts;
+    int rts_delay;
+    int onebyte_time;
+    void (*set_rts) (modbus_t *ctx, int on);
+#endif
     /* To handle many slaves on the same link */
     int confirmation_to_ignore;
 } modbus_rtu_t;
+
+#endif /* MODBUS_RTU_PRIVATE_H */
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
 #include <stdio.h>
 #include <stdlib.h>
 #include <errno.h>
 #include <fcntl.h>
 #include <string.h>
+#ifndef _MSC_VER
 #include <unistd.h>
+#endif
 #include <assert.h>
 #include "modbus-private.h"
@@ -82,15 +89,12 @@ static const uint8_t table_crc_lo[] = {
 /* Define the slave ID of the remote device to talk in master mode or set the
  * internal slave ID in slave mode */
-static int _modbus_set_slave( modbus_t *ctx, int slave )
+static int _modbus_set_slave(modbus_t *ctx, int slave)
 {
     /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
-    if (slave >= 0 && slave <= 247) 
-    {
+    if (slave >= 0 && slave <= 247) {
         ctx->slave = slave;
-    } 
-    else 
-    {
+    } else {
         errno = EINVAL;
         return -1;
     }
@@ -99,7 +103,9 @@ static int _modbus_set_slave( modbus_t *ctx, int slave )
 }
 /* Builds a RTU request header */
-static int _modbus_rtu_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
+static int _modbus_rtu_build_request_basis(modbus_t *ctx, int function,
+                                           int addr, int nb,
+                                           uint8_t *req)
 {
     assert(ctx->slave != -1);
     req[0] = ctx->slave;
@@ -113,7 +119,7 @@ static int _modbus_rtu_build_request_basis( modbus_t *ctx, int function, int add
 }
 /* Builds a RTU response header */
-static int _modbus_rtu_build_response_basis( sft_t *sft, uint8_t *rsp )
+static int _modbus_rtu_build_response_basis(sft_t *sft, uint8_t *rsp)
 {
     /* In this case, the slave is certainly valid because a check is already
      * done in _modbus_rtu_listen */
@@ -123,11 +129,11 @@ static int _modbus_rtu_build_response_basis( sft_t *sft, uint8_t *rsp )
     return _MODBUS_RTU_PRESET_RSP_LENGTH;
 }
-static uint16_t crc16( uint8_t *buffer, uint16_t buffer_length )
+static uint16_t crc16(uint8_t *buffer, uint16_t buffer_length)
 {
-    uint8_t crc_hi = 0xFF;  /* high CRC byte initialized */
-    uint8_t crc_lo = 0xFF;  /* low CRC byte initialized */
-    unsigned int i;         /* will index into CRC lookup */
+    uint8_t crc_hi = 0xFF; /* high CRC byte initialized */
+    uint8_t crc_lo = 0xFF; /* low CRC byte initialized */
+    unsigned int i; /* will index into CRC lookup */
     /* pass through message buffer */
     while (buffer_length--) {
@@ -139,47 +145,138 @@ static uint16_t crc16( uint8_t *buffer, uint16_t buffer_length )
     return (crc_hi << 8 | crc_lo);
 }
-static int _modbus_rtu_prepare_response_tid( const uint8_t *req, int *req_length )
+static int _modbus_rtu_prepare_response_tid(const uint8_t *req, int *req_length)
 {
     (*req_length) -= _MODBUS_RTU_CHECKSUM_LENGTH;
     /* No TID */
     return 0;
 }
-static int _modbus_rtu_send_msg_pre( uint8_t *req, int req_length )
+static int _modbus_rtu_send_msg_pre(uint8_t *req, int req_length)
 {
-    uint16_t crc = crc16( req, req_length );
+    uint16_t crc = crc16(req, req_length);
     req[req_length++] = crc >> 8;
     req[req_length++] = crc & 0x00FF;
     return req_length;
 }
+#if defined(_WIN32)
+
+/* This simple implementation is sort of a substitute of the select() call,
+ * working this way: the win32_ser_select() call tries to read some data from
+ * the serial port, setting the timeout as the select() call would. Data read is
+ * stored into the receive buffer, that is then consumed by the win32_ser_read()
+ * call.  So win32_ser_select() does both the event waiting and the reading,
+ * while win32_ser_read() only consumes the receive buffer.
+ */
+
+static void win32_ser_init(struct win32_ser *ws)
+{
+    /* Clear everything */
+    memset(ws, 0x00, sizeof(struct win32_ser));
+
+    /* Set file handle to invalid */
+    ws->fd = INVALID_HANDLE_VALUE;
+}
+
+/* FIXME Try to remove length_to_read -> max_len argument, only used by win32 */
+static int win32_ser_select(struct win32_ser *ws, int max_len,
+                            const struct timeval *tv)
+{
+    COMMTIMEOUTS comm_to;
+    unsigned int msec = 0;
+
+    /* Check if some data still in the buffer to be consumed */
+    if (ws->n_bytes > 0) {
+        return 1;
+    }
+
+    /* Setup timeouts like select() would do.
+       FIXME Please someone on Windows can look at this?
+       Does it possible to use WaitCommEvent?
+       When tv is NULL, MAXDWORD isn't infinite!
+     */
+    if (tv == NULL) {
+        msec = MAXDWORD;
+    } else {
+        msec = tv->tv_sec * 1000 + tv->tv_usec / 1000;
+        if (msec < 1)
+            msec = 1;
+    }
+
+    comm_to.ReadIntervalTimeout = msec;
+    comm_to.ReadTotalTimeoutMultiplier = 0;
+    comm_to.ReadTotalTimeoutConstant = msec;
+    comm_to.WriteTotalTimeoutMultiplier = 0;
+    comm_to.WriteTotalTimeoutConstant = 1000;
+    SetCommTimeouts(ws->fd, &comm_to);
+
+    /* Read some bytes */
+    if ((max_len > PY_BUF_SIZE) || (max_len < 0)) {
+        max_len = PY_BUF_SIZE;
+    }
+
+    if (ReadFile(ws->fd, &ws->buf, max_len, &ws->n_bytes, NULL)) {
+        /* Check if some bytes available */
+        if (ws->n_bytes > 0) {
+            /* Some bytes read */
+            return 1;
+        } else {
+            /* Just timed out */
+            return 0;
+        }
+    } else {
+        /* Some kind of error */
+        return -1;
+    }
+}
+
+static int win32_ser_read(struct win32_ser *ws, uint8_t *p_msg,
+                          unsigned int max_len)
+{
+    unsigned int n = ws->n_bytes;
+
+    if (max_len < n) {
+        n = max_len;
+    }
+
+    if (n > 0) {
+        memcpy(p_msg, ws->buf, n);
+    }
+
+    ws->n_bytes -= n;
+
+    return n;
+}
+#endif
+
 #if HAVE_DECL_TIOCM_RTS
-static void _modbus_rtu_ioctl_rts( modbus_t *ctx, int on )
+static void _modbus_rtu_ioctl_rts(modbus_t *ctx, int on)
 {
     int fd = ctx->s;
     int flags;
     ioctl(fd, TIOCMGET, &flags);
-    if (on) 
-    {
+    if (on) {
         flags |= TIOCM_RTS;
-    } 
-    else 
-    {
+    } else {
         flags &= ~TIOCM_RTS;
     }
     ioctl(fd, TIOCMSET, &flags);
 }
 #endif
-static ssize_t _modbus_rtu_send( modbus_t *ctx, const uint8_t *req, int req_length )
+static ssize_t _modbus_rtu_send(modbus_t *ctx, const uint8_t *req, int req_length)
 {
+#if defined(_WIN32)
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+    DWORD n_bytes = 0;
+    return (WriteFile(ctx_rtu->w_ser.fd, req, req_length, &n_bytes, NULL)) ? (ssize_t)n_bytes : -1;
+#else
 #if HAVE_DECL_TIOCM_RTS
     modbus_rtu_t *ctx_rtu = ctx->backend_data;
-    if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE) 
-    {
+    if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE) {
         ssize_t size;
         if (ctx->debug) {
@@ -195,37 +292,31 @@ static ssize_t _modbus_rtu_send( modbus_t *ctx, const uint8_t *req, int req_leng
         ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
         return size;
-    } 
-    else 
-    {
+    } else {
 #endif
         return write(ctx->s, req, req_length);
 #if HAVE_DECL_TIOCM_RTS
     }
 #endif
+#endif
 }
-static int _modbus_rtu_receive( modbus_t *ctx, uint8_t *req )
+static int _modbus_rtu_receive(modbus_t *ctx, uint8_t *req)
 {
     int rc;
     modbus_rtu_t *ctx_rtu = ctx->backend_data;
-    if( ctx_rtu->confirmation_to_ignore ) 
-    {
+    if (ctx_rtu->confirmation_to_ignore) {
         _modbus_receive_msg(ctx, req, MSG_CONFIRMATION);
         /* Ignore errors and reset the flag */
         ctx_rtu->confirmation_to_ignore = FALSE;
         rc = 0;
-        if( ctx->debug ) 
-        {
+        if (ctx->debug) {
             printf("Confirmation to ignore\n");
         }
-    } 
-    else 
-    {
-        rc = _modbus_receive_msg( ctx, req, MSG_INDICATION );
-        if( rc == 0 ) 
-        {
+    } else {
+        rc = _modbus_receive_msg(ctx, req, MSG_INDICATION);
+        if (rc == 0) {
             /* The next expected message is a confirmation to ignore */
             ctx_rtu->confirmation_to_ignore = TRUE;
         }
@@ -233,33 +324,40 @@ static int _modbus_rtu_receive( modbus_t *ctx, uint8_t *req )
     return rc;
 }
-static ssize_t _modbus_rtu_recv( modbus_t *ctx, uint8_t *rsp, int rsp_length )
+static ssize_t _modbus_rtu_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length)
 {
+#if defined(_WIN32)
+    return win32_ser_read(&((modbus_rtu_t *)ctx->backend_data)->w_ser, rsp, rsp_length);
+#else
     return read(ctx->s, rsp, rsp_length);
+#endif
 }
 static int _modbus_rtu_flush(modbus_t *);
-static int _modbus_rtu_pre_check_confirmation(modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length)
+static int _modbus_rtu_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
+                                              const uint8_t *rsp, int rsp_length)
 {
-    /* Check responding slave is the slave we requested (except for broacast request) */
-    if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS) 
-    {
-        if (ctx->debug) 
-        {
-            fprintf(stderr, "The responding slave %d isn't the requested slave %d\n", rsp[0], req[0]);
+    /* Check responding slave is the slave we requested (except for broacast
+     * request) */
+    if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "The responding slave %d isn't the requested slave %d\n",
+                    rsp[0], req[0]);
         }
         errno = EMBBADSLAVE;
         return -1;
-    } 
-
-    return 0;
+    } else {
+        return 0;
+    }
 }
 /* The check_crc16 function shall return 0 is the message is ignored and the
    message length if the CRC is valid. Otherwise it shall return -1 and set
    errno to EMBADCRC. */
-static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
+static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg,
+                                       const int msg_length)
 {
     uint16_t crc_calculated;
     uint16_t crc_received;
@@ -267,10 +365,8 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int ms
     /* Filter on the Modbus unit identifier (slave) in RTU mode to avoid useless
      * CRC computing. */
-    if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) 
-    {
-        if (ctx->debug) 
-        {
+    if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) {
+        if (ctx->debug) {
             printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave);
         }
         /* 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
     crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
     /* Check CRC of msg */
-    if (crc_calculated == crc_received) 
-    {
+    if (crc_calculated == crc_received) {
         return msg_length;
-    } 
-    else 
-    {
-        if (ctx->debug) 
-        {
+    } else {
+        if (ctx->debug) {
             fprintf(stderr, "ERROR CRC received 0x%0X != CRC calculated 0x%0X\n",
                     crc_received, crc_calculated);
         }
-        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL)
-        {
+        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
             _modbus_rtu_flush(ctx);
         }
         errno = EMBBADCRC;
@@ -305,18 +396,189 @@ static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int ms
 /* Sets up a serial port for RTU communications */
 static int _modbus_rtu_connect(modbus_t *ctx)
 {
+#if defined(_WIN32)
+    DCB dcb;
+#else
     struct termios tios;
     speed_t speed;
     int flags;
-
+#endif
     modbus_rtu_t *ctx_rtu = ctx->backend_data;
-    if (ctx->debug) 
-    {
+    if (ctx->debug) {
         printf("Opening %s at %d bauds (%c, %d, %d)\n",
-               ctx_rtu->device, ctx_rtu->baud, ctx_rtu->parity, ctx_rtu->data_bit, ctx_rtu->stop_bit);
+               ctx_rtu->device, ctx_rtu->baud, ctx_rtu->parity,
+               ctx_rtu->data_bit, ctx_rtu->stop_bit);
+    }
+
+#if defined(_WIN32)
+    /* Some references here:
+     * http://msdn.microsoft.com/en-us/library/aa450602.aspx
+     */
+    win32_ser_init(&ctx_rtu->w_ser);
+
+    /* ctx_rtu->device should contain a string like "COMxx:" xx being a decimal
+     * number */
+    ctx_rtu->w_ser.fd = CreateFileA(ctx_rtu->device,
+                                    GENERIC_READ | GENERIC_WRITE,
+                                    0,
+                                    NULL,
+                                    OPEN_EXISTING,
+                                    0,
+                                    NULL);
+
+    /* Error checking */
+    if (ctx_rtu->w_ser.fd == INVALID_HANDLE_VALUE) {
+        if (ctx->debug) {
+            fprintf(stderr, "ERROR Can't open the device %s (LastError %d)\n",
+                    ctx_rtu->device, (int)GetLastError());
+        }
+        return -1;
+    }
+
+    /* Save params */
+    ctx_rtu->old_dcb.DCBlength = sizeof(DCB);
+    if (!GetCommState(ctx_rtu->w_ser.fd, &ctx_rtu->old_dcb)) {
+        if (ctx->debug) {
+            fprintf(stderr, "ERROR Error getting configuration (LastError %d)\n",
+                    (int)GetLastError());
+        }
+        CloseHandle(ctx_rtu->w_ser.fd);
+        ctx_rtu->w_ser.fd = INVALID_HANDLE_VALUE;
+        return -1;
     }
+    /* Build new configuration (starting from current settings) */
+    dcb = ctx_rtu->old_dcb;
+
+    /* Speed setting */
+    switch (ctx_rtu->baud) {
+    case 110:
+        dcb.BaudRate = CBR_110;
+        break;
+    case 300:
+        dcb.BaudRate = CBR_300;
+        break;
+    case 600:
+        dcb.BaudRate = CBR_600;
+        break;
+    case 1200:
+        dcb.BaudRate = CBR_1200;
+        break;
+    case 2400:
+        dcb.BaudRate = CBR_2400;
+        break;
+    case 4800:
+        dcb.BaudRate = CBR_4800;
+        break;
+    case 9600:
+        dcb.BaudRate = CBR_9600;
+        break;
+    case 14400:
+        dcb.BaudRate = CBR_14400;
+        break;
+    case 19200:
+        dcb.BaudRate = CBR_19200;
+        break;
+    case 38400:
+        dcb.BaudRate = CBR_38400;
+        break;
+    case 57600:
+        dcb.BaudRate = CBR_57600;
+        break;
+    case 115200:
+        dcb.BaudRate = CBR_115200;
+        break;
+    case 230400:
+        /* CBR_230400 - not defined */
+        dcb.BaudRate = 230400;
+        break;
+    case 250000:
+        dcb.BaudRate = 250000;
+        break;
+    case 460800:
+        dcb.BaudRate = 460800;
+        break;
+    case 500000:
+        dcb.BaudRate = 500000;
+        break;
+    case 921600:
+        dcb.BaudRate = 921600;
+        break;
+    case 1000000:
+        dcb.BaudRate = 1000000;
+        break;
+    default:
+        dcb.BaudRate = CBR_9600;
+        if (ctx->debug) {
+            fprintf(stderr, "WARNING Unknown baud rate %d for %s (B9600 used)\n",
+                    ctx_rtu->baud, ctx_rtu->device);
+        }
+    }
+
+    /* Data bits */
+    switch (ctx_rtu->data_bit) {
+    case 5:
+        dcb.ByteSize = 5;
+        break;
+    case 6:
+        dcb.ByteSize = 6;
+        break;
+    case 7:
+        dcb.ByteSize = 7;
+        break;
+    case 8:
+    default:
+        dcb.ByteSize = 8;
+        break;
+    }
+
+    /* Stop bits */
+    if (ctx_rtu->stop_bit == 1)
+        dcb.StopBits = ONESTOPBIT;
+    else /* 2 */
+        dcb.StopBits = TWOSTOPBITS;
+
+    /* Parity */
+    if (ctx_rtu->parity == 'N') {
+        dcb.Parity = NOPARITY;
+        dcb.fParity = FALSE;
+    } else if (ctx_rtu->parity == 'E') {
+        dcb.Parity = EVENPARITY;
+        dcb.fParity = TRUE;
+    } else {
+        /* odd */
+        dcb.Parity = ODDPARITY;
+        dcb.fParity = TRUE;
+    }
+
+    /* Hardware handshaking left as default settings retrieved */
+
+    /* No software handshaking */
+    dcb.fTXContinueOnXoff = TRUE;
+    dcb.fOutX = FALSE;
+    dcb.fInX = FALSE;
+
+    /* Binary mode (it's the only supported on Windows anyway) */
+    dcb.fBinary = TRUE;
+
+    /* Don't want errors to be blocking */
+    dcb.fAbortOnError = FALSE;
+
+	//***Not part of libmodbus - added for QModMaster***//
+    dcb.fRtsControl = ctx_rtu->rts;
+	
+    /* Setup port */
+    if (!SetCommState(ctx_rtu->w_ser.fd, &dcb)) {
+        if (ctx->debug) {
+            fprintf(stderr, "ERROR Error setting new configuration (LastError %d)\n",
+                    (int)GetLastError());
+        }
+        CloseHandle(ctx_rtu->w_ser.fd);
+        ctx_rtu->w_ser.fd = INVALID_HANDLE_VALUE;
+        return -1;
+    }
+#else
     /* The O_NOCTTY flag tells UNIX that this program doesn't want
        to be the "controlling terminal" for that port. If you
        don't specify this then any input (such as keyboard abort
@@ -346,8 +608,7 @@ static int _modbus_rtu_connect(modbus_t *ctx)
     /* C_ISPEED     Input baud (new interface)
        C_OSPEED     Output baud (new interface)
     */
-    switch (ctx_rtu->baud) 
-    {
+    switch (ctx_rtu->baud) {
     case 110:
         speed = B110;
         break;
@@ -628,13 +889,15 @@ static int _modbus_rtu_connect(modbus_t *ctx)
     */
     /* Unused because we use open with the NDELAY option */
     tios.c_cc[VMIN] = 0;
-    tios.c_cc[VTIME] = 10;  // Set per default to 1 second
+    tios.c_cc[VTIME] = 0;
     if (tcsetattr(ctx->s, TCSANOW, &tios) < 0) {
         close(ctx->s);
         ctx->s = -1;
         return -1;
     }
+#endif
+
     return 0;
 }
@@ -849,35 +1112,62 @@ static void _modbus_rtu_close(modbus_t *ctx)
     /* Restore line settings and close file descriptor in RTU mode */
     modbus_rtu_t *ctx_rtu = ctx->backend_data;
+#if defined(_WIN32)
+    /* Revert settings */
+    if (!SetCommState(ctx_rtu->w_ser.fd, &ctx_rtu->old_dcb) && ctx->debug) {
+        fprintf(stderr, "ERROR Couldn't revert to configuration (LastError %d)\n",
+                (int)GetLastError());
+    }
+
+    if (!CloseHandle(ctx_rtu->w_ser.fd) && ctx->debug) {
+        fprintf(stderr, "ERROR Error while closing handle (LastError %d)\n",
+                (int)GetLastError());
+    }
+#else
     if (ctx->s != -1) {
         tcsetattr(ctx->s, TCSANOW, &ctx_rtu->old_tios);
         close(ctx->s);
         ctx->s = -1;
     }
+#endif
 }
 static int _modbus_rtu_flush(modbus_t *ctx)
 {
+#if defined(_WIN32)
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+    ctx_rtu->w_ser.n_bytes = 0;
+    return (PurgeComm(ctx_rtu->w_ser.fd, PURGE_RXCLEAR) == FALSE);
+#else
     return tcflush(ctx->s, TCIOFLUSH);
+#endif
 }
-static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
+static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset,
+                              struct timeval *tv, int length_to_read)
 {
     int s_rc;
-    while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1)
-    {
-        if (errno == EINTR)
-        {
-            if (ctx->debug)
-            {
+#if defined(_WIN32)
+    s_rc = win32_ser_select(&((modbus_rtu_t *)ctx->backend_data)->w_ser,
+                            length_to_read, tv);
+    if (s_rc == 0) {
+        errno = ETIMEDOUT;
+        return -1;
+    }
+
+    if (s_rc < 0) {
+        return -1;
+    }
+#else
+    while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1) {
+        if (errno == EINTR) {
+            if (ctx->debug) {
                 fprintf(stderr, "A non blocked signal was caught\n");
             }
             /* Necessary after an error */
             FD_ZERO(rset);
             FD_SET(ctx->s, rset);
-        }
-        else
-        {
+        } else {
             return -1;
         }
     }
@@ -887,6 +1177,7 @@ static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, i
         errno = ETIMEDOUT;
         return -1;
     }
+#endif
     return s_rc;
 }
@@ -919,9 +1210,10 @@ const modbus_backend_t _modbus_rtu_backend = {
     _modbus_rtu_free
 };
+//***Not part of libmodbus - rts param added for QModMaster***//
 modbus_t* modbus_new_rtu(const char *device,
                          int baud, char parity, int data_bit,
-                         int stop_bit)
+                         int stop_bit, int rts)
 {
     modbus_t *ctx;
     modbus_rtu_t *ctx_rtu;
@@ -981,6 +1273,11 @@ modbus_t* modbus_new_rtu(const char *device,
     ctx_rtu->rts_delay = ctx_rtu->onebyte_time;
 #endif
+//***Not part of libmodbus - added for QModMaster***//
+#if defined(_WIN32)
+    ctx_rtu->rts = rts;
+#endif
+
     ctx_rtu->confirmation_to_ignore = FALSE;
     return ctx;
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
-#pragma once
+#ifndef MODBUS_RTU_H
+#define MODBUS_RTU_H
 #include "modbus.h"
@@ -10,25 +16,28 @@ MODBUS_BEGIN_DECLS
  */
 #define MODBUS_RTU_MAX_ADU_LENGTH  256
-MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity, int data_bit, int stop_bit);
+//***Not part of libmodbus - rts param added for QModMaster***//
+MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity,
+                                    int data_bit, int stop_bit, int rts);
 #define MODBUS_RTU_RS232 0
 #define MODBUS_RTU_RS485 1
-MODBUS_API int modbus_rtu_set_serial_mode( modbus_t *ctx, int mode );
-MODBUS_API int modbus_rtu_get_serial_mode( modbus_t *ctx );
+MODBUS_API int modbus_rtu_set_serial_mode(modbus_t *ctx, int mode);
+MODBUS_API int modbus_rtu_get_serial_mode(modbus_t *ctx);
 #define MODBUS_RTU_RTS_NONE   0
 #define MODBUS_RTU_RTS_UP     1
 #define MODBUS_RTU_RTS_DOWN   2
-MODBUS_API int modbus_rtu_set_rts( modbus_t *ctx, int mode );
-MODBUS_API int modbus_rtu_get_rts( modbus_t *ctx );
+MODBUS_API int modbus_rtu_set_rts(modbus_t *ctx, int mode);
+MODBUS_API int modbus_rtu_get_rts(modbus_t *ctx);
-MODBUS_API int modbus_rtu_set_custom_rts( modbus_t *ctx, void ( *set_rts ) ( modbus_t *ctx, int on ) );
+MODBUS_API int modbus_rtu_set_custom_rts(modbus_t *ctx, void (*set_rts) (modbus_t *ctx, int on));
-MODBUS_API int modbus_rtu_set_rts_delay( modbus_t *ctx, int us );
-MODBUS_API int modbus_rtu_get_rts_delay( modbus_t *ctx );
+MODBUS_API int modbus_rtu_set_rts_delay(modbus_t *ctx, int us);
+MODBUS_API int modbus_rtu_get_rts_delay(modbus_t *ctx);
 MODBUS_END_DECLS
+#endif /* MODBUS_RTU_H */
-#pragma once
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+
+#ifndef MODBUS_TCP_PRIVATE_H
+#define MODBUS_TCP_PRIVATE_H
 #define _MODBUS_TCP_HEADER_LENGTH      7
 #define _MODBUS_TCP_PRESET_REQ_LENGTH 12
@@ -8,8 +15,7 @@
 /* In both structures, the transaction ID must be placed on first position
    to have a quick access not dependant of the TCP backend */
-typedef struct _modbus_tcp 
-{
+typedef struct _modbus_tcp {
     /* Extract from MODBUS Messaging on TCP/IP Implementation Guide V1.0b
        (page 23/46):
        The transaction identifier is used to associate the future response
@@ -24,8 +30,7 @@ typedef struct _modbus_tcp
 #define _MODBUS_TCP_PI_NODE_LENGTH    1025
 #define _MODBUS_TCP_PI_SERVICE_LENGTH   32
-typedef struct _modbus_tcp_pi 
-{
+typedef struct _modbus_tcp_pi {
     /* Transaction ID */
     uint16_t t_id;
     /* TCP port */
@@ -36,4 +41,4 @@ typedef struct _modbus_tcp_pi
     char service[_MODBUS_TCP_PI_SERVICE_LENGTH];
 } modbus_tcp_pi_t;
-
+#endif /* MODBUS_TCP_PRIVATE_H */
-
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -21,30 +20,46 @@
 # include <arpa/inet.h>
 # include <netdb.h>
+
 #if !defined(MSG_NOSIGNAL)
 #define MSG_NOSIGNAL 0
 #endif
+#if defined(_AIX) && !defined(MSG_DONTWAIT)
+#define MSG_DONTWAIT MSG_NONBLOCK
+#endif
+
 #include "modbus-private.h"
 #include "modbus-tcp.h"
 #include "modbus-tcp-private.h"
+#ifdef OS_WIN32
+static int _modbus_tcp_init_win32(void)
+{
+    /* Initialise Windows Socket API */
+    WSADATA wsaData;
+
+    if (WSAStartup(MAKEWORD(2, 2), &wsaData) != 0) {
+        fprintf(stderr, "WSAStartup() returned error code %d\n",
+                (unsigned int)GetLastError());
+        errno = EIO;
+        return -1;
+    }
+    return 0;
+}
+#endif
+
 static int _modbus_set_slave(modbus_t *ctx, int slave)
 {
     /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
-    if ( slave >= 0 && slave <= 247) 
-    {
+    if (slave >= 0 && slave <= 247) {
         ctx->slave = slave;
-    } 
-    else if ( slave == MODBUS_TCP_SLAVE ) 
-    {
+    } else if (slave == MODBUS_TCP_SLAVE) {
         /* The special value MODBUS_TCP_SLAVE (0xFF) can be used in TCP mode to
          * restore the default value. */
         ctx->slave = slave;
-    } 
-    else 
-    {
+    } else {
         errno = EINVAL;
         return -1;
     }
@@ -53,7 +68,9 @@ static int _modbus_set_slave(modbus_t *ctx, int slave)
 }
 /* Builds a TCP request header */
-static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
+static int _modbus_tcp_build_request_basis(modbus_t *ctx, int function,
+                                           int addr, int nb,
+                                           uint8_t *req)
 {
     modbus_tcp_t *ctx_tcp = ctx->backend_data;
@@ -62,7 +79,6 @@ static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int add
         ctx_tcp->t_id++;
     else
         ctx_tcp->t_id = 0;
-
     req[0] = ctx_tcp->t_id >> 8;
     req[1] = ctx_tcp->t_id & 0x00ff;
@@ -84,7 +100,7 @@ static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int add
 }
 /* Builds a TCP response header */
-static int _modbus_tcp_build_response_basis( sft_t *sft, uint8_t *rsp )
+static int _modbus_tcp_build_response_basis(sft_t *sft, uint8_t *rsp)
 {
     /* Extract from MODBUS Messaging on TCP/IP Implementation
        Guide V1.0b (page 23/46):
@@ -107,12 +123,12 @@ static int _modbus_tcp_build_response_basis( sft_t *sft, uint8_t *rsp )
 }
-static int _modbus_tcp_prepare_response_tid( const uint8_t *req, int *req_length )
+static int _modbus_tcp_prepare_response_tid(const uint8_t *req, int *req_length)
 {
     return (req[0] << 8) + req[1];
 }
-static int _modbus_tcp_send_msg_pre( uint8_t *req, int req_length )
+static int _modbus_tcp_send_msg_pre(uint8_t *req, int req_length)
 {
     /* Substract the header length to the message length */
     int mbap_length = req_length - 6;
@@ -123,7 +139,7 @@ static int _modbus_tcp_send_msg_pre( uint8_t *req, int req_length )
     return req_length;
 }
-static ssize_t _modbus_tcp_send( modbus_t *ctx, const uint8_t *req, int req_length )
+static ssize_t _modbus_tcp_send(modbus_t *ctx, const uint8_t *req, int req_length)
 {
     /* MSG_NOSIGNAL
        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
     return send(ctx->s, (const char *)req, req_length, MSG_NOSIGNAL);
 }
-static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req) 
-{
+static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req) {
     return _modbus_receive_msg(ctx, req, MSG_INDICATION);
 }
-static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length) 
-{
-    return recv( ctx->s, (char *)rsp, rsp_length, 0 );
+static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length) {
+    return recv(ctx->s, (char *)rsp, rsp_length, 0);
 }
-static int _modbus_tcp_check_integrity( modbus_t *ctx, uint8_t *msg, const int msg_length )
+static int _modbus_tcp_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
 {
     return msg_length;
 }
-static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length )
+static int _modbus_tcp_pre_check_confirmation(modbus_t *ctx, const uint8_t *req,
+                                              const uint8_t *rsp, int rsp_length)
 {
     /* Check transaction ID */
     if (req[0] != rsp[0] || req[1] != rsp[1]) {
-        if (ctx->debug) 
-        {
+        if (ctx->debug) {
             fprintf(stderr, "Invalid transaction ID received 0x%X (not 0x%X)\n",
                     (rsp[0] << 8) + rsp[1], (req[0] << 8) + req[1]);
         }
@@ -161,10 +175,8 @@ static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req
     }
     /* Check protocol ID */
-    if (rsp[2] != 0x0 && rsp[3] != 0x0) 
-    {
-        if (ctx->debug) 
-        {
+    if (rsp[2] != 0x0 && rsp[3] != 0x0) {
+        if (ctx->debug) {
             fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n",
                     (rsp[2] << 8) + rsp[3]);
         }
@@ -175,7 +187,7 @@ static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req
     return 0;
 }
-static int _modbus_tcp_set_ipv4_options( int s )
+static int _modbus_tcp_set_ipv4_options(int s)
 {
     int rc;
     int option;
@@ -183,9 +195,9 @@ static int _modbus_tcp_set_ipv4_options( int s )
     /* Set the TCP no delay flag */
     /* SOL_TCP = IPPROTO_TCP */
     option = 1;
-    rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (const void *)&option, sizeof(int) );
-    if (rc == -1) 
-    {
+    rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY,
+                    (const void *)&option, sizeof(int));
+    if (rc == -1) {
         return -1;
     }
@@ -193,10 +205,19 @@ static int _modbus_tcp_set_ipv4_options( int s )
      * make sockets non-blocking */
     /* Do not care about the return value, this is optional */
 #if !defined(SOCK_NONBLOCK) && defined(FIONBIO)
+#ifdef OS_WIN32
+    {
+        /* Setting FIONBIO expects an unsigned long according to MSDN */
+        u_long loption = 1;
+        ioctlsocket(s, FIONBIO, &loption);
+    }
+#else
     option = 1;
     ioctl(s, FIONBIO, &option);
 #endif
+#endif
+#ifndef OS_WIN32
     /**
      * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's
      * necessary to workaround that problem.
@@ -208,16 +229,26 @@ static int _modbus_tcp_set_ipv4_options( int s )
     if (rc == -1) {
         return -1;
     }
+#endif
     return 0;
 }
-static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen, const struct timeval *ro_tv )
+static int _connect(int sockfd, const struct sockaddr *addr, socklen_t addrlen,
+                    const struct timeval *ro_tv)
 {
     int rc = connect(sockfd, addr, addrlen);
-    if (rc == -1 && errno == EINPROGRESS) 
-    {
+#ifdef OS_WIN32
+    int wsaError = 0;
+    if (rc == -1) {
+        wsaError = WSAGetLastError();
+    }
+
+    if (wsaError == WSAEWOULDBLOCK || wsaError == WSAEINPROGRESS) {
+#else
+    if (rc == -1 && errno == EINPROGRESS) {
+#endif
         fd_set wset;
         int optval;
         socklen_t optlen = sizeof(optval);
@@ -227,20 +258,16 @@ static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen,
         FD_ZERO(&wset);
         FD_SET(sockfd, &wset);
         rc = select(sockfd + 1, NULL, &wset, NULL, &tv);
-        if (rc <= 0) 
-        {
+        if (rc <= 0) {
             /* Timeout or fail */
             return -1;
         }
         /* The connection is established if SO_ERROR and optval are set to 0 */
         rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *)&optval, &optlen);
-        if ( rc == 0 && optval == 0 ) 
-        {
+        if (rc == 0 && optval == 0) {
             return 0;
-        } 
-        else 
-        {
+        } else {
             errno = ECONNREFUSED;
             return -1;
         }
@@ -249,7 +276,7 @@ static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen,
 }
 /* Establishes a modbus TCP connection with a Modbus server. */
-static int _modbus_tcp_connect( modbus_t *ctx )
+static int _modbus_tcp_connect(modbus_t *ctx)
 {
     int rc;
     /* Specialized version of sockaddr for Internet socket address (same size) */
@@ -257,6 +284,12 @@ static int _modbus_tcp_connect( modbus_t *ctx )
     modbus_tcp_t *ctx_tcp = ctx->backend_data;
     int flags = SOCK_STREAM;
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
 #ifdef SOCK_CLOEXEC
     flags |= SOCK_CLOEXEC;
 #endif
@@ -266,21 +299,18 @@ static int _modbus_tcp_connect( modbus_t *ctx )
 #endif
     ctx->s = socket(PF_INET, flags, 0);
-    if (ctx->s == -1) 
-    {
+    if (ctx->s == -1) {
         return -1;
     }
     rc = _modbus_tcp_set_ipv4_options(ctx->s);
-    if (rc == -1) 
-    {
+    if (rc == -1) {
         close(ctx->s);
         ctx->s = -1;
         return -1;
     }
-    if (ctx->debug) 
-    {
+    if (ctx->debug) {
         printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port);
     }
@@ -288,9 +318,8 @@ static int _modbus_tcp_connect( modbus_t *ctx )
     addr.sin_port = htons(ctx_tcp->port);
     addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
     rc = _connect(ctx->s, (struct sockaddr *)&addr, sizeof(addr), &ctx->response_timeout);
-    if( rc == -1 ) 
-    {
-        close( ctx->s );
+    if (rc == -1) {
+        close(ctx->s);
         ctx->s = -1;
         return -1;
     }
@@ -299,7 +328,7 @@ static int _modbus_tcp_connect( modbus_t *ctx )
 }
 /* Establishes a modbus TCP PI connection with a Modbus server. */
-static int _modbus_tcp_pi_connect( modbus_t *ctx )
+static int _modbus_tcp_pi_connect(modbus_t *ctx)
 {
     int rc;
     struct addrinfo *ai_list;
@@ -307,6 +336,12 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
     struct addrinfo ai_hints;
     modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
     memset(&ai_hints, 0, sizeof(ai_hints));
 #ifdef AI_ADDRCONFIG
     ai_hints.ai_flags |= AI_ADDRCONFIG;
@@ -318,19 +353,17 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
     ai_hints.ai_next = NULL;
     ai_list = NULL;
-    rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service, &ai_hints, &ai_list);
-    if (rc != 0) 
-    {
-        if (ctx->debug) 
-        {
+    rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service,
+                     &ai_hints, &ai_list);
+    if (rc != 0) {
+        if (ctx->debug) {
             fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
         }
         errno = ECONNREFUSED;
         return -1;
     }
-    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) 
-    {
+    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
         int flags = ai_ptr->ai_socktype;
         int s;
@@ -349,14 +382,12 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
         if (ai_ptr->ai_family == AF_INET)
             _modbus_tcp_set_ipv4_options(s);
-        if (ctx->debug) 
-        {
+        if (ctx->debug) {
             printf("Connecting to [%s]:%s\n", ctx_tcp_pi->node, ctx_tcp_pi->service);
         }
         rc = _connect(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen, &ctx->response_timeout);
-        if (rc == -1) 
-        {
+        if (rc == -1) {
             close(s);
             continue;
         }
@@ -367,8 +398,7 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
     freeaddrinfo(ai_list);
-    if (ctx->s < 0) 
-    {
+    if (ctx->s < 0) {
         return -1;
     }
@@ -376,88 +406,102 @@ static int _modbus_tcp_pi_connect( modbus_t *ctx )
 }
 /* Closes the network connection and socket in TCP mode */
-static void _modbus_tcp_close( modbus_t *ctx )
+static void _modbus_tcp_close(modbus_t *ctx)
 {
-    if (ctx->s != -1) 
-    {
+    if (ctx->s != -1) {
         shutdown(ctx->s, SHUT_RDWR);
         close(ctx->s);
         ctx->s = -1;
     }
 }
-static int _modbus_tcp_flush( modbus_t *ctx )
+static int _modbus_tcp_flush(modbus_t *ctx)
 {
     int rc;
     int rc_sum = 0;
-    do 
-    {
+    do {
         /* Extract the garbage from the socket */
         char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
-
+#ifndef OS_WIN32
         rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
-        if ( rc > 0 ) 
-        {
+#else
+        /* On Win32, it's a bit more complicated to not wait */
+        fd_set rset;
+        struct timeval tv;
+
+        tv.tv_sec = 0;
+        tv.tv_usec = 0;
+        FD_ZERO(&rset);
+        FD_SET(ctx->s, &rset);
+        rc = select(ctx->s+1, &rset, NULL, NULL, &tv);
+        if (rc == -1) {
+            return -1;
+        }
+
+        if (rc == 1) {
+            /* There is data to flush */
+            rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, 0);
+        }
+#endif
+        if (rc > 0) {
             rc_sum += rc;
         }
-    } while ( rc == MODBUS_TCP_MAX_ADU_LENGTH );
+    } while (rc == MODBUS_TCP_MAX_ADU_LENGTH);
     return rc_sum;
 }
 /* Listens for any request from one or many modbus masters in TCP */
-int modbus_tcp_listen( modbus_t *ctx, int nb_connection )
+int modbus_tcp_listen(modbus_t *ctx, int nb_connection)
 {
     int new_s;
     int enable;
     struct sockaddr_in addr;
     modbus_tcp_t *ctx_tcp;
-    if (ctx == NULL) 
-    {
+    if (ctx == NULL) {
         errno = EINVAL;
         return -1;
     }
     ctx_tcp = ctx->backend_data;
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
     new_s = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
-    if ( new_s == -1 ) 
-    {
+    if (new_s == -1) {
         return -1;
     }
     enable = 1;
-    if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR, (char *)&enable, sizeof(enable)) == -1) 
-    {
+    if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR,
+                   (char *)&enable, sizeof(enable)) == -1) {
         close(new_s);
         return -1;
     }
     memset(&addr, 0, sizeof(addr));
     addr.sin_family = AF_INET;
-
     /* If the modbus port is < to 1024, we need the setuid root. */
     addr.sin_port = htons(ctx_tcp->port);
-    if (ctx_tcp->ip[0] == '0') 
-    {
+    if (ctx_tcp->ip[0] == '0') {
         /* Listen any addresses */
         addr.sin_addr.s_addr = htonl(INADDR_ANY);
-    } 
-    else 
-    {
+    } else {
         /* Listen only specified IP address */
         addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
     }
-    if (bind( new_s, (struct sockaddr *)&addr, sizeof(addr)) == -1) 
-    {
+    if (bind(new_s, (struct sockaddr *)&addr, sizeof(addr)) == -1) {
         close(new_s);
         return -1;
     }
-    if (listen(new_s, nb_connection) == -1) 
-    {
+    if (listen(new_s, nb_connection) == -1) {
         close(new_s);
         return -1;
     }
@@ -465,7 +509,7 @@ int modbus_tcp_listen( modbus_t *ctx, int nb_connection )
     return new_s;
 }
-int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
+int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection)
 {
     int rc;
     struct addrinfo *ai_list;
@@ -476,33 +520,32 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
     int new_s;
     modbus_tcp_pi_t *ctx_tcp_pi;
-    if (ctx == NULL) 
-    {
+    if (ctx == NULL) {
         errno = EINVAL;
         return -1;
     }
     ctx_tcp_pi = ctx->backend_data;
-    if ( ctx_tcp_pi->node[0] == 0) 
-    {
+#ifdef OS_WIN32
+    if (_modbus_tcp_init_win32() == -1) {
+        return -1;
+    }
+#endif
+
+    if (ctx_tcp_pi->node[0] == 0) {
         node = NULL; /* == any */
-    } 
-    else 
-    {
+    } else {
         node = ctx_tcp_pi->node;
     }
-    if ( ctx_tcp_pi->service[0] == 0 ) 
-    {
+    if (ctx_tcp_pi->service[0] == 0) {
         service = "502";
-    } 
-    else 
-    {
+    } else {
         service = ctx_tcp_pi->service;
     }
-    memset( &ai_hints, 0, sizeof (ai_hints) );
+    memset(&ai_hints, 0, sizeof (ai_hints));
     /* If node is not NULL, than the AI_PASSIVE flag is ignored. */
     ai_hints.ai_flags |= AI_PASSIVE;
 #ifdef AI_ADDRCONFIG
@@ -516,10 +559,8 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
     ai_list = NULL;
     rc = getaddrinfo(node, service, &ai_hints, &ai_list);
-    if (rc != 0) 
-    {
-        if (ctx->debug) 
-        {
+    if (rc != 0) {
+        if (ctx->debug) {
             fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
         }
         errno = ECONNREFUSED;
@@ -527,28 +568,23 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
     }
     new_s = -1;
-    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) 
-    {
+    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) {
         int s;
-        s = socket( ai_ptr->ai_family, ai_ptr->ai_socktype, ai_ptr->ai_protocol );
-        if (s < 0) 
-        {
-            if (ctx->debug) 
-            {
+        s = socket(ai_ptr->ai_family, ai_ptr->ai_socktype,
+                   ai_ptr->ai_protocol);
+        if (s < 0) {
+            if (ctx->debug) {
                 perror("socket");
             }
             continue;
-        } 
-        else 
-        {
+        } else {
             int enable = 1;
-            rc = setsockopt( s, SOL_SOCKET, SO_REUSEADDR, (void *)&enable, sizeof (enable) );
-            if (rc != 0) 
-            {
+            rc = setsockopt(s, SOL_SOCKET, SO_REUSEADDR,
+                            (void *)&enable, sizeof (enable));
+            if (rc != 0) {
                 close(s);
-                if (ctx->debug) 
-                {
+                if (ctx->debug) {
                     perror("setsockopt");
                 }
                 continue;
@@ -556,22 +592,18 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
         }
         rc = bind(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen);
-        if (rc != 0) 
-        {
+        if (rc != 0) {
             close(s);
-            if (ctx->debug) 
-            {
+            if (ctx->debug) {
                 perror("bind");
             }
             continue;
         }
         rc = listen(s, nb_connection);
-        if (rc != 0) 
-        {
+        if (rc != 0) {
             close(s);
-            if (ctx->debug) 
-            {
+            if (ctx->debug) {
                 perror("listen");
             }
             continue;
@@ -582,8 +614,7 @@ int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
     }
     freeaddrinfo(ai_list);
-    if (new_s < 0) 
-    {
+    if (new_s < 0) {
         return -1;
     }
@@ -595,8 +626,7 @@ int modbus_tcp_accept(modbus_t *ctx, int *s)
     struct sockaddr_in addr;
     socklen_t addrlen;
-    if (ctx == NULL) 
-    {
+    if (ctx == NULL) {
         errno = EINVAL;
         return -1;
     }
@@ -609,15 +639,13 @@ int modbus_tcp_accept(modbus_t *ctx, int *s)
     ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
 #endif
-    if (ctx->s == -1) 
-    {
+    if (ctx->s == -1) {
         close(*s);
         *s = -1;
         return -1;
     }
-    if (ctx->debug) 
-    {
+    if (ctx->debug) {
         printf("The client connection from %s is accepted\n",
                inet_ntoa(addr.sin_addr));
     }
@@ -625,13 +653,12 @@ int modbus_tcp_accept(modbus_t *ctx, int *s)
     return ctx->s;
 }
-int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
+int modbus_tcp_pi_accept(modbus_t *ctx, int *s)
 {
     struct sockaddr_storage addr;
     socklen_t addrlen;
-    if (ctx == NULL) 
-    {
+    if (ctx == NULL) {
         errno = EINVAL;
         return -1;
     }
@@ -643,14 +670,12 @@ int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
 #else
     ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
 #endif
-    if (ctx->s == -1) 
-    {
+    if (ctx->s == -1) {
         close(*s);
         *s = -1;
     }
-    if (ctx->debug) 
-    {
+    if (ctx->debug) {
         printf("The client connection is accepted.\n");
     }
@@ -660,26 +685,20 @@ int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
 static int _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
 {
     int s_rc;
-    while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1) 
-    {
-        if (errno == EINTR) 
-        {
-            if (ctx->debug) 
-            {
+    while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1) {
+        if (errno == EINTR) {
+            if (ctx->debug) {
                 fprintf(stderr, "A non blocked signal was caught\n");
             }
             /* Necessary after an error */
             FD_ZERO(rset);
             FD_SET(ctx->s, rset);
-        } 
-        else 
-        {
+        } else {
             return -1;
         }
     }
-    if ( s_rc == 0 ) 
-    {
+    if (s_rc == 0) {
         errno = ETIMEDOUT;
         return -1;
     }
@@ -687,14 +706,12 @@ static int _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, i
     return s_rc;
 }
-static void _modbus_tcp_free( modbus_t *ctx ) 
-{
+static void _modbus_tcp_free(modbus_t *ctx) {
     free(ctx->backend_data);
     free(ctx);
 }
-const modbus_backend_t _modbus_tcp_backend = 
-{
+const modbus_backend_t _modbus_tcp_backend = {
     _MODBUS_BACKEND_TYPE_TCP,
     _MODBUS_TCP_HEADER_LENGTH,
     _MODBUS_TCP_CHECKSUM_LENGTH,
@@ -717,8 +734,7 @@ const modbus_backend_t _modbus_tcp_backend =
 };
-const modbus_backend_t _modbus_tcp_pi_backend = 
-{
+const modbus_backend_t _modbus_tcp_pi_backend = {
     _MODBUS_BACKEND_TYPE_TCP,
     _MODBUS_TCP_HEADER_LENGTH,
     _MODBUS_TCP_CHECKSUM_LENGTH,
@@ -740,14 +756,27 @@ const modbus_backend_t _modbus_tcp_pi_backend =
     _modbus_tcp_free
 };
-modbus_t* modbus_new_tcp( const char *ip, int port )
+modbus_t* modbus_new_tcp(const char *ip, int port)
 {
     modbus_t *ctx;
     modbus_tcp_t *ctx_tcp;
     size_t dest_size;
     size_t ret_size;
-    ctx = (modbus_t *)malloc(sizeof(modbus_t) );
+#if defined(OS_BSD)
+    /* MSG_NOSIGNAL is unsupported on *BSD so we install an ignore
+       handler for SIGPIPE. */
+    struct sigaction sa;
+
+    sa.sa_handler = SIG_IGN;
+    if (sigaction(SIGPIPE, &sa, NULL) < 0) {
+        /* The debug flag can't be set here... */
+        fprintf(stderr, "Coud not install SIGPIPE handler.\n");
+        return NULL;
+    }
+#endif
+
+    ctx = (modbus_t *)malloc(sizeof(modbus_t));
     _modbus_init_common(ctx);
     /* 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 )
     ctx->backend = &_modbus_tcp_backend;
-    ctx->backend_data = ( modbus_tcp_t *)malloc(sizeof(modbus_tcp_t) );
+    ctx->backend_data = (modbus_tcp_t *)malloc(sizeof(modbus_tcp_t));
     ctx_tcp = (modbus_tcp_t *)ctx->backend_data;
-    if (ip != NULL) 
-    {
+    if (ip != NULL) {
         dest_size = sizeof(char) * 16;
         ret_size = strlcpy(ctx_tcp->ip, ip, dest_size);
-        if (ret_size == 0) 
-        {
+        if (ret_size == 0) {
             fprintf(stderr, "The IP string is empty\n");
             modbus_free(ctx);
             errno = EINVAL;
             return NULL;
         }
-        if (ret_size >= dest_size) 
-        {
+        if (ret_size >= dest_size) {
             fprintf(stderr, "The IP string has been truncated\n");
             modbus_free(ctx);
             errno = EINVAL;
             return NULL;
         }
-    } 
-    else 
-    {
+    } else {
         ctx_tcp->ip[0] = '0';
     }
     ctx_tcp->port = port;
@@ -789,12 +813,12 @@ modbus_t* modbus_new_tcp( const char *ip, int port )
 }
-modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
+modbus_t* modbus_new_tcp_pi(const char *node, const char *service)
 {
-    modbus_t        *ctx;
+    modbus_t *ctx;
     modbus_tcp_pi_t *ctx_tcp_pi;
-    size_t           dest_size;
-    size_t           ret_size;
+    size_t dest_size;
+    size_t ret_size;
     ctx = (modbus_t *)malloc(sizeof(modbus_t));
     _modbus_init_common(ctx);
@@ -807,25 +831,20 @@ modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
     ctx->backend_data = (modbus_tcp_pi_t *)malloc(sizeof(modbus_tcp_pi_t));
     ctx_tcp_pi = (modbus_tcp_pi_t *)ctx->backend_data;
-    if (node == NULL) 
-    {
+    if (node == NULL) {
         /* The node argument can be empty to indicate any hosts */
         ctx_tcp_pi->node[0] = 0;
-    } 
-    else 
-    {
+    } else {
         dest_size = sizeof(char) * _MODBUS_TCP_PI_NODE_LENGTH;
         ret_size = strlcpy(ctx_tcp_pi->node, node, dest_size);
-        if (ret_size == 0) 
-        {
+        if (ret_size == 0) {
             fprintf(stderr, "The node string is empty\n");
             modbus_free(ctx);
             errno = EINVAL;
             return NULL;
         }
-        if (ret_size >= dest_size) 
-        {
+        if (ret_size >= dest_size) {
             fprintf(stderr, "The node string has been truncated\n");
             modbus_free(ctx);
             errno = EINVAL;
@@ -833,27 +852,22 @@ modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
         }
     }
-    if (service != NULL) 
-    {
+    if (service != NULL) {
         dest_size = sizeof(char) * _MODBUS_TCP_PI_SERVICE_LENGTH;
         ret_size = strlcpy(ctx_tcp_pi->service, service, dest_size);
-    } 
-    else 
-    {
+    } else {
         /* Empty service is not allowed, error catched below. */
         ret_size = 0;
     }
-    if (ret_size == 0) 
-    {
+    if (ret_size == 0) {
         fprintf(stderr, "The service string is empty\n");
         modbus_free(ctx);
         errno = EINVAL;
         return NULL;
     }
-    if (ret_size >= dest_size) 
-    {
+    if (ret_size >= dest_size) {
         fprintf(stderr, "The service string has been truncated\n");
         modbus_free(ctx);
         errno = EINVAL;
-#pragma once
+/*
+ * Copyright © 2001-2010 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+
+#ifndef MODBUS_TCP_H
+#define MODBUS_TCP_H
 #include "modbus.h"
 MODBUS_BEGIN_DECLS
+#if defined(_WIN32) && !defined(__CYGWIN__)
+/* Win32 with MinGW, supplement to <errno.h> */
+#include <winsock2.h>
+#if !defined(ECONNRESET)
+#define ECONNRESET   WSAECONNRESET
+#endif
+#if !defined(ECONNREFUSED)
+#define ECONNREFUSED WSAECONNREFUSED
+#endif
+#if !defined(ETIMEDOUT)
+#define ETIMEDOUT    WSAETIMEDOUT
+#endif
+#if !defined(ENOPROTOOPT)
+#define ENOPROTOOPT  WSAENOPROTOOPT
+#endif
+#if !defined(EINPROGRESS)
+#define EINPROGRESS  WSAEINPROGRESS
+#endif
+#endif
+
 #define MODBUS_TCP_DEFAULT_PORT   502
 #define MODBUS_TCP_SLAVE         0xFF
@@ -22,3 +49,4 @@ MODBUS_API int modbus_tcp_pi_accept(modbus_t *ctx, int *s);
 MODBUS_END_DECLS
+#endif /* MODBUS_TCP_H */
-#pragma once 
+/*
+ * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * This library is free software; you can redistribute it and/or
+ * modify it under the terms of the GNU Lesser General Public
+ * License as published by the Free Software Foundation; either
+ * version 2.1 of the License, or (at your option) any later version.
+ *
+ * This library is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
+ * Lesser General Public License for more details.
+ *
+ * You should have received a copy of the GNU Lesser General Public
+ * License along with this library; if not, write to the Free Software
+ * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ */
+
+#ifndef MODBUS_VERSION_H
+#define MODBUS_VERSION_H
 /* The major version, (1, if %LIBMODBUS_VERSION is 1.2.3) */
 #define LIBMODBUS_VERSION_MAJOR (3)
@@ -31,4 +50,4 @@
       LIBMODBUS_VERSION_MINOR == (minor) &&             \
       LIBMODBUS_VERSION_MICRO >= (micro)))
-
+#endif /* MODBUS_VERSION_H */
-#include <config.h>
-#include <errno.h>
-#include <limits.h>
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ *
+ * This library implements the Modbus protocol.
+ * http://libmodbus.org/
+ */
+
 #include <stdio.h>
 #include <string.h>
 #include <stdlib.h>
 #include <stdarg.h>
+#include <errno.h>
+#include <limits.h>
 #include <time.h>
+#ifndef _MSC_VER
 #include <unistd.h>
+#endif
+
+#include <config.h>
 #include "modbus.h"
 #include "modbus-private.h"
@@ -14,67 +26,65 @@
 /* Internal use */
 #define MSG_LENGTH_UNDEFINED -1
+/* Exported version */
+const unsigned int libmodbus_version_major = LIBMODBUS_VERSION_MAJOR;
+const unsigned int libmodbus_version_minor = LIBMODBUS_VERSION_MINOR;
+const unsigned int libmodbus_version_micro = LIBMODBUS_VERSION_MICRO;
+
 /* Max between RTU and TCP max adu length (so TCP) */
 #define MAX_MESSAGE_LENGTH 260
 /* 3 steps are used to parse the query */
-typedef enum
-{
+typedef enum {
     _STEP_FUNCTION,
     _STEP_META,
     _STEP_DATA
 } _step_t;
-const char *modbus_strerror( int errnum )
-{
-    switch (errnum)
-    {
-        case EMBXILFUN:
-            return "Illegal function";
-        case EMBXILADD:
-            return "Illegal data address";
-        case EMBXILVAL:
-            return "Illegal data value";
-        case EMBXSFAIL:
-            return "Slave device or server failure";
-        case EMBXACK:
-            return "Acknowledge";
-        case EMBXSBUSY:
-            return "Slave device or server is busy";
-        case EMBXNACK:
-            return "Negative acknowledge";
-        case EMBXMEMPAR:
-            return "Memory parity error";
-        case EMBXGPATH:
-            return "Gateway path unavailable";
-        case EMBXGTAR:
-            return "Target device failed to respond";
-        case EMBBADCRC:
-            return "Invalid CRC";
-        case EMBBADDATA:
-            return "Invalid data";
-        case EMBBADEXC:
-            return "Invalid exception code";
-        case EMBMDATA:
-            return "Too many data";
-        case EMBBADSLAVE:
-            return "Response not from requested slave";
-        default:
-            return strerror(errnum);
+const char *modbus_strerror(int errnum) {
+    switch (errnum) {
+    case EMBXILFUN:
+        return "Illegal function";
+    case EMBXILADD:
+        return "Illegal data address";
+    case EMBXILVAL:
+        return "Illegal data value";
+    case EMBXSFAIL:
+        return "Slave device or server failure";
+    case EMBXACK:
+        return "Acknowledge";
+    case EMBXSBUSY:
+        return "Slave device or server is busy";
+    case EMBXNACK:
+        return "Negative acknowledge";
+    case EMBXMEMPAR:
+        return "Memory parity error";
+    case EMBXGPATH:
+        return "Gateway path unavailable";
+    case EMBXGTAR:
+        return "Target device failed to respond";
+    case EMBBADCRC:
+        return "Invalid CRC";
+    case EMBBADDATA:
+        return "Invalid data";
+    case EMBBADEXC:
+        return "Invalid exception code";
+    case EMBMDATA:
+        return "Too many data";
+    case EMBBADSLAVE:
+        return "Response not from requested slave";
+    default:
+        return strerror(errnum);
     }
 }
 void _error_print(modbus_t *ctx, const char *context)
 {
-    if (ctx->debug)
-    {
+    if (ctx->debug) {
         fprintf(stderr, "ERROR %s", modbus_strerror(errno));
-        if (context != NULL)
-        {
+        if (context != NULL) {
             fprintf(stderr, ": %s\n", context);
-        }
-        else
-        {
+        } else {
             fprintf(stderr, "\n");
         }
     }
@@ -83,29 +93,32 @@ void _error_print(modbus_t *ctx, const char *context)
 static void _sleep_response_timeout(modbus_t *ctx)
 {
     /* Response timeout is always positive */
+#ifdef _WIN32
+    /* usleep doesn't exist on Windows */
+    Sleep((ctx->response_timeout.tv_sec * 1000) +
+          (ctx->response_timeout.tv_usec / 1000));
+#else
     /* usleep source code */
     struct timespec request, remaining;
     request.tv_sec = ctx->response_timeout.tv_sec;
     request.tv_nsec = ((long int)ctx->response_timeout.tv_usec) * 1000;
-    while ( nanosleep( &request, &remaining ) == -1 && errno == EINTR )
-    {
+    while (nanosleep(&request, &remaining) == -1 && errno == EINTR) {
         request = remaining;
     }
+#endif
 }
 int modbus_flush(modbus_t *ctx)
 {
     int rc;
-    if (ctx == NULL)
-    {
+    if (ctx == NULL) {
         errno = EINVAL;
         return -1;
     }
-    rc = ctx->backend->flush( ctx );
-    if (rc != -1 && ctx->debug)
-    {
+    rc = ctx->backend->flush(ctx);
+    if (rc != -1 && ctx->debug) {
         /* Not all backends are able to return the number of bytes flushed */
         printf("Bytes flushed (%d)\n", rc);
     }
@@ -118,44 +131,32 @@ static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t 
     int length;
     const int offset = ctx->backend->header_length;
-    switch (req[offset])
-    {
-        case MODBUS_FC_READ_COILS:
-        case MODBUS_FC_READ_DISCRETE_INPUTS:
-        {
-            /* Header + nb values (code from write_bits) */
-            int nb = (req[offset + 3] << 8) | req[offset + 4];
-            length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
-            break;
-        }
-        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
-        case MODBUS_FC_READ_HOLDING_REGISTERS:
-        case MODBUS_FC_READ_INPUT_REGISTERS:
-        {
-            /* Header + 2 * nb values */
-            length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
-            break;
-        }
-        case MODBUS_FC_READ_EXCEPTION_STATUS:
-        {
-            length = 3;
-            break;
-        }
-        case MODBUS_FC_REPORT_SLAVE_ID:
-        {
-            /* The response is device specific (the header provides the
-               length) */
-            return MSG_LENGTH_UNDEFINED;
-        }
-        case MODBUS_FC_MASK_WRITE_REGISTER:
-        {
-            length = 7;
-            break;
-        }
-        default:
-        {
-            length = 5;
-        }
+    switch (req[offset]) {
+    case MODBUS_FC_READ_COILS:
+    case MODBUS_FC_READ_DISCRETE_INPUTS: {
+        /* Header + nb values (code from write_bits) */
+        int nb = (req[offset + 3] << 8) | req[offset + 4];
+        length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
+    }
+        break;
+    case MODBUS_FC_WRITE_AND_READ_REGISTERS:
+    case MODBUS_FC_READ_HOLDING_REGISTERS:
+    case MODBUS_FC_READ_INPUT_REGISTERS:
+        /* Header + 2 * nb values */
+        length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
+        break;
+    case MODBUS_FC_READ_EXCEPTION_STATUS:
+        length = 3;
+        break;
+    case MODBUS_FC_REPORT_SLAVE_ID:
+        /* The response is device specific (the header provides the
+           length) */
+        return MSG_LENGTH_UNDEFINED;
+    case MODBUS_FC_MASK_WRITE_REGISTER:
+        length = 7;
+        break;
+    default:
+        length = 5;
     }
     return offset + length + ctx->backend->checksum_length;
@@ -169,42 +170,34 @@ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
     msg_length = ctx->backend->send_msg_pre(msg, msg_length);
-    if (ctx->debug)
-    {
+    if (ctx->debug) {
         for (i = 0; i < msg_length; i++)
-        {
             printf("[%.2X]", msg[i]);
-        }
         printf("\n");
     }
-
+    
 	/* In recovery mode, the write command will be issued until to be
        successful! Disabled by default. */
-    do
-    {
+    do {
         rc = ctx->backend->send(ctx, msg, msg_length);
-        if (rc == -1)
-        {
+        if (rc == -1) {
             _error_print(ctx, NULL);
-            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK)
-            {
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
                 int saved_errno = errno;
-                if ( (errno == EBADF || errno == ECONNRESET || errno == EPIPE))
-                {
+                if ((errno == EBADF || errno == ECONNRESET || errno == EPIPE)) {
                     modbus_close(ctx);
                     _sleep_response_timeout(ctx);
                     modbus_connect(ctx);
-                }
-                else
-                {
+                } else {
                     _sleep_response_timeout(ctx);
                     modbus_flush(ctx);
                 }
                 errno = saved_errno;
             }
         }
-    } while ( (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && rc == -1);
+    } while ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
+             rc == -1);
     if (rc > 0 && rc != msg_length) {
         errno = EMBBADDATA;
@@ -214,20 +207,18 @@ static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
     return rc;
 }
-int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length )
+int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length)
 {
     sft_t sft;
     uint8_t req[MAX_MESSAGE_LENGTH];
     int req_length;
-    if (ctx == NULL)
-    {
+    if (ctx == NULL) {
         errno = EINVAL;
         return -1;
     }
-    if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1))
-    {
+    if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1)) {
         /* The raw request must contain function and slave at least and
            must not be longer than the maximum pdu length plus the slave
            address. */
@@ -242,8 +233,7 @@ int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length
     /* This response function only set the header so it's convenient here */
     req_length = ctx->backend->build_response_basis(&sft, req);
-    if( raw_req_length > 2 )
-    {
+    if (raw_req_length > 2) {
         /* Copy data after function code */
         memcpy(req + req_length, raw_req + 2, raw_req_length - 2);
         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
  */
 /* Computes the length to read after the function received */
-static uint8_t compute_meta_length_after_function( int function, msg_type_t msg_type )
+static uint8_t compute_meta_length_after_function(int function,
+                                                  msg_type_t msg_type)
 {
     int length;
-    if (msg_type == MSG_INDICATION)
-    {
-        if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER)
-        {
+    if (msg_type == MSG_INDICATION) {
+        if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER) {
             length = 4;
-        }
-        else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS || function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS)
-        {
+        } else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS ||
+                   function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS) {
             length = 5;
-        }
-        else if (function == MODBUS_FC_MASK_WRITE_REGISTER)
-        {
+        } else if (function == MODBUS_FC_MASK_WRITE_REGISTER) {
             length = 6;
-        }
-        else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS)
-        {
+        } else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
             length = 9;
-        }
-        else
-        {
+        } else {
             /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */
             length = 0;
         }
-    }
-    else
-    {
+    } else {
         /* MSG_CONFIRMATION */
-        switch (function)
-        {
-            case MODBUS_FC_WRITE_SINGLE_COIL:
-            case MODBUS_FC_WRITE_SINGLE_REGISTER:
-            case MODBUS_FC_WRITE_MULTIPLE_COILS:
-            case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
-                length = 4;
-                break;
-            case MODBUS_FC_MASK_WRITE_REGISTER:
-                length = 6;
-                break;
-            default:
-                length = 1;
+        switch (function) {
+        case MODBUS_FC_WRITE_SINGLE_COIL:
+        case MODBUS_FC_WRITE_SINGLE_REGISTER:
+        case MODBUS_FC_WRITE_MULTIPLE_COILS:
+        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
+            length = 4;
+            break;
+        case MODBUS_FC_MASK_WRITE_REGISTER:
+            length = 6;
+            break;
+        default:
+            length = 1;
         }
     }
@@ -310,37 +289,31 @@ static uint8_t compute_meta_length_after_function( int function, msg_type_t msg_
 }
 /* Computes the length to read after the meta information (address, count, etc) */
-static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
+static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg,
+                                          msg_type_t msg_type)
 {
     int function = msg[ctx->backend->header_length];
     int length;
-    if (msg_type == MSG_INDICATION)
-    {
-        switch (function)
-        {
-            case MODBUS_FC_WRITE_MULTIPLE_COILS:
-            case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
-                length = msg[ctx->backend->header_length + 5];
-                break;
-            case MODBUS_FC_WRITE_AND_READ_REGISTERS:
-                length = msg[ctx->backend->header_length + 9];
-                break;
-            default:
-                length = 0;
+    if (msg_type == MSG_INDICATION) {
+        switch (function) {
+        case MODBUS_FC_WRITE_MULTIPLE_COILS:
+        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
+            length = msg[ctx->backend->header_length + 5];
+            break;
+        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
+            length = msg[ctx->backend->header_length + 9];
+            break;
+        default:
+            length = 0;
         }
-    }
-    else
-    {
+    } else {
         /* MSG_CONFIRMATION */
         if (function <= MODBUS_FC_READ_INPUT_REGISTERS ||
             function == MODBUS_FC_REPORT_SLAVE_ID ||
-            function == MODBUS_FC_WRITE_AND_READ_REGISTERS)
-        {
+            function == MODBUS_FC_WRITE_AND_READ_REGISTERS) {
             length = msg[ctx->backend->header_length + 1];
-        }
-        else
-        {
+        } else {
             length = 0;
         }
     }
@@ -364,7 +337,7 @@ static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_
    - read() or recv() error codes
 */
-int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
+int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
 {
     int rc;
     fd_set rset;
@@ -374,14 +347,10 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
     int msg_length = 0;
     _step_t step;
-    if (ctx->debug)
-    {
-        if (msg_type == MSG_INDICATION)
-        {
+    if (ctx->debug) {
+        if (msg_type == MSG_INDICATION) {
             printf("Waiting for a indication...\n");
-        }
-        else
-        {
+        } else {
             printf("Waiting for a confirmation...\n");
         }
     }
@@ -396,36 +365,27 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
     step = _STEP_FUNCTION;
     length_to_read = ctx->backend->header_length + 1;
-    if (msg_type == MSG_INDICATION)
-    {
+    if (msg_type == MSG_INDICATION) {
         /* Wait for a message, we don't know when the message will be
          * received */
         p_tv = NULL;
-    }
-    else
-    {
+    } else {
         tv.tv_sec = ctx->response_timeout.tv_sec;
         tv.tv_usec = ctx->response_timeout.tv_usec;
         p_tv = &tv;
     }
-    while (length_to_read != 0)
-    {
+    while (length_to_read != 0) {
         rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
-        if (rc == -1)
-        {
+        if (rc == -1) {
             _error_print(ctx, "select");
-            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK)
-            {
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) {
                 int saved_errno = errno;
-                if (errno == ETIMEDOUT)
-                {
+                if (errno == ETIMEDOUT) {
                     _sleep_response_timeout(ctx);
                     modbus_flush(ctx);
-                }
-                else if (errno == EBADF)
-                {
+                } else if (errno == EBADF) {
                     modbus_close(ctx);
                     modbus_connect(ctx);
                 }
@@ -435,19 +395,16 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
         }
         rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
-        if (rc == 0)
-        {
+        if (rc == 0) {
             errno = ECONNRESET;
             rc = -1;
         }
-        if (rc == -1)
-        {
+        if (rc == -1) {
             _error_print(ctx, "read");
             if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
                 (errno == ECONNRESET || errno == ECONNREFUSED ||
-                 errno == EBADF))
-            {
+                 errno == EBADF)) {
                 int saved_errno = errno;
                 modbus_close(ctx);
                 modbus_connect(ctx);
@@ -458,8 +415,7 @@ int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
         }
         /* Display the hex code of each character received */
-        if (ctx->debug)
-        {
+        if (ctx->debug) {
             int i;
             for (i=0; i < rc; i++)
                 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 )
         /* Computes remaining bytes */
         length_to_read -= rc;
-        if (length_to_read == 0)
-        {
-            switch( step )
-            {
-                case _STEP_FUNCTION:
-                    /* Function code position */
-                    length_to_read = compute_meta_length_after_function( msg[ctx->backend->header_length], msg_type);
-                    if (length_to_read != 0)
-                    {
-                        step = _STEP_META;
-                        break;
-                    } /* else switches straight to the next step */
-                    __attribute__((fallthrough));               ///< Let the compiler know we intentionallly created a fallthrough.
-                case _STEP_META:
-                    length_to_read = compute_data_length_after_meta( ctx, msg, msg_type);
-                    if ( (msg_length + length_to_read) > (int)ctx->backend->max_adu_length )
-                    {
-                        errno = EMBBADDATA;
-                        _error_print(ctx, "too many data");
-                        return -1;
-                    }
-                    step = _STEP_DATA;
-                    break;
-                default:
+        if (length_to_read == 0) {
+            switch (step) {
+            case _STEP_FUNCTION:
+                /* Function code position */
+                length_to_read = compute_meta_length_after_function(
+                    msg[ctx->backend->header_length],
+                    msg_type);
+                if (length_to_read != 0) {
+                    step = _STEP_META;
                     break;
+                } /* else switches straight to the next step */
+            case _STEP_META:
+                length_to_read = compute_data_length_after_meta(
+                    ctx, msg, msg_type);
+                if ((msg_length + length_to_read) > (int)ctx->backend->max_adu_length) {
+                    errno = EMBBADDATA;
+                    _error_print(ctx, "too many data");
+                    return -1;
+                }
+                step = _STEP_DATA;
+                break;
+            default:
+                break;
             }
         }
         if (length_to_read > 0 &&
-            (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0))
-        {
+            (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0)) {
             /* If there is no character in the buffer, the allowed timeout
                interval between two consecutive bytes is defined by
                byte_timeout */
@@ -937,8 +890,7 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
         }
     }
         break;
-    case MODBUS_FC_REPORT_SLAVE_ID:
-    {
+    case MODBUS_FC_REPORT_SLAVE_ID: {
         int str_len;
         int byte_count_pos;
@@ -948,8 +900,9 @@ int modbus_reply(modbus_t *ctx, const uint8_t *req,
         rsp[rsp_length++] = _REPORT_SLAVE_ID;
         /* Run indicator status to ON */
         rsp[rsp_length++] = 0xFF;
-        str_len = 3 + strlen(MODBUS_VERSION_STRING);
-        memcpy(rsp + rsp_length, "LMB" MODBUS_VERSION_STRING, str_len);
+        /* LMB + length of LIBMODBUS_VERSION_STRING */
+        str_len = 3 + strlen(LIBMODBUS_VERSION_STRING);
+        memcpy(rsp + rsp_length, "LMB" LIBMODBUS_VERSION_STRING, str_len);
         rsp_length += str_len;
         rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
     }
@@ -1188,8 +1141,7 @@ static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_
         if (ctx->debug)
         {
             fprintf(stderr,
-                    "ERROR Too many registers requested (%d > %d)\n",
-                    nb, MODBUS_MAX_READ_REGISTERS);
+                    "ERROR Too many registers requested (%d > %d)\n", nb, MODBUS_MAX_READ_REGISTERS);
         }
         errno = EMBMDATA;
         return -1;
@@ -1213,10 +1165,10 @@ static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_
         offset = ctx->backend->header_length;
-        for (i = 0; i < rc; i++) {
+        for (i = 0; i < rc; i++)
+        {
             /* shift reg hi_byte to temp OR with lo_byte */
-            dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
-                rsp[offset + 3 + (i << 1)];
+            dest[i] = (rsp[offset + 2 + (i << 1)] << 8) | rsp[offset + 3 + (i << 1)];
         }
     }
@@ -1234,10 +1186,8 @@ int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
         return -1;
     }
-    if (nb > MODBUS_MAX_READ_REGISTERS)
-    {
-        if (ctx->debug)
-        {
+    if (nb > MODBUS_MAX_READ_REGISTERS) {
+        if (ctx->debug) {
             fprintf(stderr,
                     "ERROR Too many registers requested (%d > %d)\n",
                     nb, MODBUS_MAX_READ_REGISTERS);
+/*
+ * Copyright © 2001-2013 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
-#pragma once
+#ifndef MODBUS_H
+#define MODBUS_H
 /* Add this for macros that defined unix flavor */
 #if (defined(__unix__) || defined(unix)) && !defined(USG)
 #include <sys/param.h>
 #endif
+#ifndef _MSC_VER
 #include <stdint.h>
+#else
+#include "stdint.h"
+#endif
-#define MODBUS_API
+#include "modbus-version.h"
-#define MODBUS_VERSION_STRING "1.0.0"
+#if defined(_MSC_VER)
+# if defined(DLLBUILD)
+/* define DLLBUILD when building the DLL */
+#  define MODBUS_API __declspec(dllexport)
+# else
+#  define MODBUS_API __declspec(dllimport)
+# endif
+#else
+# define MODBUS_API
+#endif
 #ifdef  __cplusplus
-#define MODBUS_BEGIN_DECLS  extern "C" {
-#define MODBUS_END_DECLS }
+# define MODBUS_BEGIN_DECLS  extern "C" {
+# define MODBUS_END_DECLS    }
 #else
 # define MODBUS_BEGIN_DECLS
 # define MODBUS_END_DECLS
@@ -129,6 +148,10 @@ enum {
 #define EMBMDATA   (EMBXGTAR + 5)
 #define EMBBADSLAVE (EMBXGTAR + 6)
+extern const unsigned int libmodbus_version_major;
+extern const unsigned int libmodbus_version_minor;
+extern const unsigned int libmodbus_version_micro;
+
 typedef struct _modbus modbus_t;
 typedef struct {
@@ -185,18 +208,19 @@ MODBUS_API int modbus_write_register(modbus_t *ctx, int reg_addr, int value);
 MODBUS_API int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
 MODBUS_API int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
 MODBUS_API int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask);
-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);
+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);
 MODBUS_API int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest);
-MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address
-(
+MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address(
     unsigned int start_bits, unsigned int nb_bits,
     unsigned int start_input_bits, unsigned int nb_input_bits,
     unsigned int start_registers, unsigned int nb_registers,
-    unsigned int start_input_registers, unsigned int nb_input_registers
-);
+    unsigned int start_input_registers, unsigned int nb_input_registers);
-MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits, int nb_registers, int nb_input_registers);
+MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits,
+                                                int nb_registers, int nb_input_registers);
 MODBUS_API void modbus_mapping_free(modbus_mapping_t *mb_mapping);
 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);
 MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp);
-MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping);
-MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code);
+MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req,
+                            int req_length, modbus_mapping_t *mb_mapping);
+MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
+                                      unsigned int exception_code);
 /**
  * UTILS FUNCTIONS
@@ -260,3 +286,4 @@ MODBUS_API void modbus_set_float_cdab(float f, uint16_t *dest);
 MODBUS_END_DECLS
+#endif  /* MODBUS_H */
-/*****************************************************************************
- * Copyright (c) 2022 Priva b.v.
- *****************************************************************************/
 #include "ConnectionConfig.h"
-#include "modbus.h"
 #include "ModbusAdapter.h"
-#include "privautils/PrivaLogger.h"
 #include <cstring>  /// Added for memset functionality
-
-
 ModbusAdapter::ModbusAdapter()
     : m_modbus( nullptr )
 {
+    this->InitBuffers();
 }
 ModbusAdapter::~ModbusAdapter()
@@ -21,25 +15,23 @@ ModbusAdapter::~ModbusAdapter()
 }
-bool ModbusAdapter::ModbusConnect( const ConnectionConfig &config )
+bool ModbusAdapter::ModbusConnect( const ConnectionConfig &conncfg )
 {
     if( m_connected )
     {
         this->ModbusDisconnect();   // Will already set m_connected
     }
-    m_connType = config.getType();
+    m_connType = conncfg.getType();
     switch( m_connType )
     {
         case ConnectionType::CT_SERIAL:
-            m_connected = this->ModbusConnectRTU( config.getPort(), config.getBaudRate(),
-                                                  config.getParity(), config.getDataBits(),
-                                                  config.getStopBits(), config.getTimeOut());
+            m_connected = this->ModbusConnectRTU( conncfg.getPort(), conncfg.getBaudRate(), conncfg.getParity(), conncfg.getDataBits(), conncfg.getStopBits(), conncfg.getTimeOut(), MODBUS_RTU_RTS_NONE );
             break;
         case ConnectionType::CT_TCP:
-            m_connected = this->ModbusConnectTCP( config.getIpAddress(), config.getTcpPort(), 10 );
+            m_connected = this->ModbusConnectTCP( conncfg.getIpAddress(), conncfg.getTcpPort(), 10 );
             break;
         default:
@@ -146,6 +138,7 @@ modbusData ModbusAdapter::ModbusReadHoldReg( int slaveId, int startAddress, int 
     }
     modbus_flush( m_modbus );
+    this->ClearBuffers();
     // -- End Critical Section --- ?? //
@@ -200,6 +193,8 @@ void ModbusAdapter::ModBusWriteData( int slaveId, int functionCode, int startAdd
             break;
     }
+    delete[]  data8;
+    delete[] data16;
     modbus_flush(m_modbus); // Flush data.
     if( resultValue != noOfItems )
@@ -238,18 +233,9 @@ std::string ModbusAdapter::ErrorString( int errnum ) const
 }
 /* ============= PRIVATE METHODS ============= */
-bool ModbusAdapter::ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int timeOut )
+bool ModbusAdapter::ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int RTS, int timeOut )
 {
-    {   // Keep logging scope as short as possible
-        std::string logLine( "Connecting : " + serialport + " ( "
-                            + std::to_string( baud ) + " : "
-                            + std::to_string( dataBits ) + ","
-                            + std::string( parity ) + " "
-                            + std::to_string( stopBits ) + " ) with timeout : "
-                            + std::to_string( timeout * 0.1 ) + " seconds." );
-        PRIVALOG_INFO(this->logZone, logLine );
-    }
-    m_modbus = modbus_new_rtu( serialport.c_str(), baud, parity, dataBits, stopBits );
+    m_modbus = modbus_new_rtu( serialport.c_str(), baud, parity, dataBits, stopBits, RTS );
 #ifdef LIB_MODBUS_DEBUG_OUTPUT
     // Do sensible logging through PRIVA_LOG
@@ -274,7 +260,7 @@ bool ModbusAdapter::ModbusConnectRTU( const std::string &amp;serialport, int baud, c
     m_connected = true;
     // Set recovery mode
-    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL | MODBUS_ERROR_RECOVERY_LINK );
+    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL );
     // Set the response timeout
     modbus_set_response_timeout( m_modbus, timeOut, 0 );
@@ -315,10 +301,25 @@ bool ModbusAdapter::ModbusConnectTCP( const std::string &amp;ip, int port, int timeO
     m_connected = true;
     // Set recovery mode
-    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL | MODBUS_ERROR_RECOVERY_LINK );
+    modbus_set_error_recovery( m_modbus, MODBUS_ERROR_RECOVERY_PROTOCOL );
     // Set the response timeout
     modbus_set_response_timeout( m_modbus, timeOut, 0 );
     return m_connected;
 }
+
+void ModbusAdapter::InitBuffers()
+{
+    // Setup memory for Data.
+    m_dest = (uint8_t *)malloc(2000 + sizeof(uint8_t));
+    m_dest16 = (uint16_t *)malloc(125 * sizeof(uint16_t));
+
+    this->ClearBuffers();
+}
+
+void ModbusAdapter::ClearBuffers()
+{
+    memset(m_dest, 0, 2000 * sizeof(uint8_t));
+    memset(m_dest16, 0, 125 * sizeof(uint16_t));
+}
@@ -6,14 +6,13 @@
 #include "ConnectionConfig.h"
 #include "IModbusAdapter.h"
+#include "modbus.h"
 // std
 #include <memory>
 #include <variant>
 #include <vector>
-typedef modbus_t;
-
 /// @brief The ModbusAdapter class represents a single modbus context. Each context will
 ///       result in an instance of this class. It is not intended to be
 ///       created directly but through a factory. The factory will create
@@ -34,7 +33,7 @@ public:
     /*!
      * \brief /// Create a modbus connection, accepting a configuration object.
      */
-    bool ModbusConnect( const ConnectionConfig &config ) override;
+    bool ModbusConnect( const ConnectionConfig &conncfg ) override;
     /*!
      * \brief   ModbusDisconnect
@@ -87,34 +86,21 @@ public:
      * \param   errnum          - error Number coming from the stack.
      * \return  The translated error as a string. Empty if not resolvable.
      */
-    std::string ErrorString( int errorNumber ) const override;
+    std::string ErrorString( int errnum ) const override;
 private:    // Methods
-    /*!
-     *  Create and connect to a serial port.
-     *
-     *  @param serialport   - The serial port by name ( i.e. /dev/ttyUSB0 )
-     *  @param baud         - The port speed in a serial port context ( default = 115200 )
-     *  @param parity       - The parity. ( Default : None, no parity )
-     *  @param dataBits     - The number of databits. RTU uses 8 (0 - 255), ASCII uses 7 (0 - 127). Default is RTU
-     *  @param stopBits     - The number of stopbits used to detect the end of the frame. ( Default = 1 )
-     *  @param timeOut      - Timeout in .1 secs. See the termios documentation for deviations on this.
-     */
-    bool    ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int timeOut );
-
-    /*!
-     * ConnectionConfig Constructor. Used to create a new TCP connection.
-     *
-     * @param ip            - The ip address of the ModBus device we want to connect to.
-     * @param portnum       - The portnumber the ModBus device is using
-     * @param timeOut       - Timeout in which a modbus device should respond.
-     */
+    bool    ModbusConnectRTU( const std::string &serialport, int baud, char parity, int dataBits, int stopBits, int RTS, int timeOut );
     bool    ModbusConnectTCP( const std::string &ip, int port, int timeOut = -1 );
+    void    InitBuffers();
+    void    ClearBuffers();
+
 private:    // Members
-    ConnectionType              connectionType  { ConnectionType::CT_UNKNOWN }; ///> The type of connection this instance provides. Needed for administration
-    bool                        connected { false };                            ///> Shows if the connection is still active.
-    modbus_t                   *modbus;                                         ///> The actual low-level modbus instance as a raw-pointer.
-                                                                                ///  ( unique_pointer gives an error on this struct )
+    ConnectionType              m_connType  { ConnectionType::CT_UNKNOWN };  ///> The type of connection this instance provides. Needed for administration
+    bool                        m_connected { false };                      ///> Shows if the connection is still active.
+    modbus_t                   *m_modbus;                                   ///> The actual low-level modbus instance as a raw-pointer. ( unique_pointer gives an error on this struct )
+    // Return value Buffers ( Room for improvement )
+    uint8_t                     *m_dest;
+    uint16_t                    *m_dest16;
 };
+#include "ModbusConnections.h"
+
+ModbusConnections::ModbusConnections()
+{
+
+}
+
+ModbusConnections::~ModbusConnections()
+{
+    if( m_mapSerial.size() > 0 )
+    {
+        // Iterate, remove and destroy ( Searching, Seek & Destroy )
+    }
+
+    if( m_mapTcp.size() > 0 )
+    {
+        // Iterate, remove and destroy ( Searching, Seek & Destroy )
+    }
+}
+
+bool ModbusConnections::CreateConnection( const ConnectionConfig &config )
+{
+    std::shared_ptr<IModbusAdapter> ptrNewAdapter = std::make_shared<ModbusAdapter>();
+    if( ptrNewAdapter == nullptr )
+    {
+        // Log a message and bail out
+        return false;
+    }
+
+    // It looks like the pointer is valid. Time to connect.
+    ptrNewAdapter->ModbusConnect( config );
+    if( !ptrNewAdapter->isConnected() )
+    {
+        // Unsuccessful. Delete the object and return false
+        ptrNewAdapter.reset();
+        return false;
+    }
+
+    std::shared_ptr<IModbusAdapter> ptr = connectionExist( config.getPortEnum(), createEndPoint( config ) );
+    if( ptr != nullptr )
+    {
+        if( !DeleteConnection( config.getPortEnum(), createEndPoint( config ) ) )
+        {
+            // Something went wrong here.. Our administration is "wonky" so report false and bail.
+            // New connection is not created.
+            ptrNewAdapter.reset();
+            return false;
+        }
+    }
+
+    if( config.getType() == ConnectionType::CT_TCP )
+    {
+        // === Critical Section ===
+        std::lock_guard<std::mutex> guard( m_mapTcpMutex );
+        m_mapTcp.insert( { createEndPoint( config ), ptrNewAdapter } );
+        // === End Critical Section ===
+    }
+    else if( config.getType() == ConnectionType::CT_SERIAL )
+    {
+        // === Critical Section ===
+        std::lock_guard<std::mutex> guard( m_mapSerialMutex );
+        m_mapSerial.insert( { config.getPortEnum(), ptrNewAdapter } );
+        // === End Critical Section ===
+    }
+    else
+    {
+        // No idea what the type is but not something we recognize.
+        ptrNewAdapter.reset();
+        return false;
+    }
+    return true;
+}
+
+bool ModbusConnections::DeleteConnection( const ConnectionPort portName, const std::string &endpoint )
+{
+    std::shared_ptr<IModbusAdapter> ptr = this->connectionExist( portName, endpoint );
+    if( ptr == nullptr )
+    {
+        //  Seems like it is already gone. Shouldn't happen, but ok.
+        return false;
+    }
+
+    // First remove it from the map.
+    if( portName == ConnectionPort::CP_TCP )
+    {
+        // === Critical Section ===
+        std::lock_guard<std::mutex> guard( m_mapTcpMutex );
+        m_mapTcp.erase( endpoint );
+        // === End Critical Section ===
+    }
+    else
+    {
+        // === Critical Section ===
+        std::lock_guard<std::mutex> guard( m_mapSerialMutex );
+        m_mapSerial.erase( portName );
+        // === End Critical Section ===
+    }
+
+    // Call the disconnect for a clean exit.
+    ptr->ModbusDisconnect();
+
+    // Delete the pointer or decrease the ref-count
+    ptr.reset();
+
+    return true;
+}
+
+int ModbusConnections::ConnectionCount()
+{
+    return ( m_mapSerial.size() + m_mapTcp.size() );
+}
+
+std::shared_ptr<IModbusAdapter> ModbusConnections::getConnection( const ConnectionPort portName, const std::string &endpoint )
+{
+    return this->connectionExist( portName, endpoint );
+}
+
+std::shared_ptr<IModbusAdapter> ModbusConnections::getConnection( const ConnectionPort portName, const std::string &ipAddress, int tcpPortNumber )
+{
+    return this->connectionExist( portName, this->createEndPoint( ipAddress, tcpPortNumber ) );
+}
+
+AdapterList ModbusConnections::getConnections()
+{
+    AdapterList lstResult;
+
+    {
+        // === Critical Section ===
+        std::lock_guard<std::mutex> guard(m_mapSerialMutex);
+        // First we pick up the Serial Map
+        for( auto const& [key, value] : m_mapSerial )
+        {
+            lstResult.push_back( value );
+        }
+        // === End Critical Section ===
+    }
+
+    {
+        // === Critical Section ===
+        std::lock_guard<std::mutex> guard(m_mapTcpMutex);
+        // Next we pick all the entries in the tcp-map
+        for( auto const& [key, value] : m_mapTcp )
+        {
+            lstResult.push_back( value );
+        }
+        // === End Critical Section ===
+    }
+
+    return lstResult;
+}
+
+std::shared_ptr<IModbusAdapter> ModbusConnections::connectionExist( const ConnectionPort portName, const std::string &endpoint )
+{
+    if( portName == ConnectionPort::CP_TCP )
+    {
+        auto search = m_mapTcp.find( endpoint );
+        if( search != m_mapTcp.end() )
+        {
+            return search->second;
+        }
+        else
+        {
+            return nullptr;
+        }
+    }
+    else
+    {
+        auto search = m_mapSerial.find( portName );
+        if( search != m_mapSerial.end() )
+        {
+            return search->second;
+        }
+        else
+        {
+            return nullptr;
+        }
+    }
+}
+
+std::string ModbusConnections::createEndPoint( const std::string &ipAddress, int portNumber )
+{
+    if( portNumber > 0 && !ipAddress.empty() )
+    {
+        return std::string( "tcp://" + ipAddress + ":" + std::to_string( portNumber ) );
+    }
+
+    return std::string();
+}
+
+std::string ModbusConnections::createEndPoint( const ConnectionConfig &config )
+{
+    if( config.getType() != ConnectionType::CT_TCP )
+    {
+        // Early opt-out
+        return std::string();
+    }
+
+    return createEndPoint( config.getIpAddress(), config.getTcpPort() );
+}
+/****************************************************************************
+ * Copyright (c) 2022 Priva B.V.
+ ****************************************************************************/
+#pragma once
+
+// Flexblox
+#include "IModbusAdapter.h"
+#include "ModbusAdapter.h"
+
+// std
+#include <memory>
+#include <mutex>
+#include <string>
+#include <unordered_map>
+
+/// Easy replacement of template construction
+using AdapterList = std::vector<std::shared_ptr<IModbusAdapter>>;
+
+/*!
+ * \brief The ModbusConnections class conatins a list of all modbus connections, Serial and TCP.
+ *        To access a specific connection, use its portname and / or endpoint to return it's interface.
+ */
+class ModbusConnections
+{
+public:
+    /*!
+     * \brief Default CTor
+     */
+    explicit ModbusConnections();
+
+    /*!
+     * \brief Default DTor
+     */
+    virtual ~ModbusConnections();
+
+    /*!
+     * \brief Create a modbus connection as described by the connection object.
+     * \param config    - The connection object describing the connection in detail.
+     * \return  The result of the creation.
+     *          True    = succesful.
+     *          False   = Failed.
+     */
+    bool CreateConnection( const ConnectionConfig &config );
+
+    /*!
+     * \brief Remove the connection described by its portname and/or endpoint.
+     * \param portName  - The portname as its enum
+     * \param endpoint  - combination of ip-address and portname in endpoint-format
+     *                    ( tcp://<ip_address>:<portNumber> )
+     *                    ( i.e. tcp://127.0.0.1:501 )
+     * \return
+     */
+    bool DeleteConnection( const ConnectionPort portName, const std::string &endpoint = std::string() );
+
+    /*!
+     * \brief Give the number of registered serial and tcp-connections combined.
+     */
+    int ConnectionCount();
+
+    /*!
+     * \brief Get the connection give by its parameters.
+     * \param   portName    - The portname by its enum
+     * \param   endpoint    - combination of ip-address and portname in endpoint-format
+     *                          ( tcp://<ip_address>:<portNumber> )
+     *                          ( i.e. tcp://127.0.0.1:501 )
+     * \return  Valid Pointer to the Modbus Connection Interface. nullptr if the connection wasn't found.
+     */
+    std::shared_ptr<IModbusAdapter> getConnection( const ConnectionPort portName, const std::string &endpoint = std::string() );
+
+    // Convenient functions
+    /*!
+     * \brief Get the connection given by its parameters. If applicable, ipaddress and portnumber will be used to create the endpoint.
+     * \param   portName    - The portname by its enum
+     * \param   ipAddress   - The ipaddress of the TCP-connection
+     * \param   tcpPortNumber   - The portnumber of the TCP-connection
+     * \return  Valid Pointer to the Modbus Connection Interface. nullptr if the connection wasn't found.
+     */
+    std::shared_ptr<IModbusAdapter> getConnection( const ConnectionPort portName, const std::string &ipAddress, int tcpPortNumber );
+
+    /*!
+     * \brief Returns a list of all registered connections by their interfaces. This is a mix of Serial and TCP-connections.
+     */
+    AdapterList getConnections();
+
+private:
+    /*!
+     * \brief Check if a connection already exist.
+     * \param portName  - The portName by its enum
+     * \param endpoint  - The endpoint this connection was registered with.
+     * \return  A valid pointer to the Interface if the connection exist. If the connection is unknown, it will return a nullptr.
+     *          shared_ptr can manifest themselves as booleans, so "!ptr" is sufficient to check.
+     */
+    std::shared_ptr<IModbusAdapter> connectionExist( const ConnectionPort portName, const std::string &endpoint = std::string() );
+
+    /*!
+     * \brief Create the TCP endpoint based on the connections IP-address and portnumber
+     * \param ipAddress     - The ipAddress as string.
+     * \param portNumber    - The portnumber as integer.
+     * \return  The endpoint in format : tcp://<ip_address>:<portNumber> i.e. tcp://127.0.0.1:501
+     */
+    std::string createEndPoint( const std::string &ipAddress, int portNumber );
+
+    /*!
+     * \brief Create the TCP endpoint based on the connections IP-address and portnumber extracted from the connection configuration.
+     * \param config - The configuration object used to create the connection.
+     * \return  The endpoint in format : tcp://<ip_address>:<portNumber> i.e. tcp://127.0.0.1:501
+     */
+    std::string createEndPoint( const ConnectionConfig &config );
+
+private:
+    std::mutex m_mapSerialMutex;                                                        ///< Mutex protecting the serialmap
+    std::mutex m_mapTcpMutex;                                                           ///< Mutex protecting the TCPmap
+    std::unordered_map<ConnectionPort, std::shared_ptr<IModbusAdapter>> m_mapSerial;    ///< Unordered map holding the Modbus connections By PortName
+    std::unordered_map<std::string, std::shared_ptr<IModbusAdapter>>    m_mapTcp;       ///< Unordered map holding the Modbus connections By tcp://endpoint:port
+};
+#include <iostream>
+
+#include "ModbusAdapter.h"
+
+int main( int argc, char* argv[] )
+{
+    ModbusAdapter oModbus;
+    ConnectionConfig oConfig( ConnectionPort::CP_EXTERNAL, 9600, Parity::PAR_NONE, 8, 1, 1 );
+
+    std::cout << "========================= [START] Connection test ================================" << std::endl;
+    oModbus.ModbusConnect( oConfig );
+    if( oModbus.isConnected() )
+    {
+        std::cout << "Successful connected to : " << oConfig.getPort() << std::endl;
+    }
+    else
+    {
+        std::cout << "There was a problem connecting to : " << oConfig.getPort() << std::endl;
+    }
+
+    oModbus.ModbusDisconnect();
+    if( !oModbus.isConnected() )
+    {
+        std::cout << "Successful disconnected from : " << oConfig.getPort() << std::endl;
+    }
+    else
+    {
+        std::cout << "There was a problem disconnecting from : " << oConfig.getPort() << std::endl;
+    }
+    std::cout << "========================= [END] Connection test ===============================" << std::endl;
+
+    std::cout << "========================= [START] Reading test ================================" << std::endl;
+    std::cout << "== Reading the Hold Registers                                                ==" << std::endl;
+    oModbus.ModbusConnect( oConfig );
+    if( oModbus.isConnected() )
+    {
+        std::cout << "Successful connected to : " << oConfig.getPort() << std::endl;
+        modbusData returnValues = oModbus.ModbusReadHoldReg( 1, MODBUS_FC_READ_HOLDING_REGISTERS, 2 );
+        if( returnValues.size() == 2 )
+        {
+            std::cout << "2 items returned from MODBUS_FC_READ_HOLDING_REGISTERS " << std::endl;
+        }
+        else
+        {
+            std::cout << "There was an error reading the Hold Registers " << std::endl;
+            std::cout << "Number of items returned :  " << returnValues.size() << std::endl;
+        }
+
+        std::cout << "== Reading the Temperature                                                   ==" << std::endl;
+        returnValues = oModbus.ModbusReadData( 0x01, MODBUS_FC_READ_INPUT_REGISTERS, 0x00, 0x02 );
+        if( returnValues.size() == 0 )
+        {
+            std::cout << "No values returned " << std::endl;
+        }
+        else
+        {
+            std::cout << "Number of items returned :  " << returnValues.size() << std::endl;
+        }
+    }
+    else
+    {
+        std::cout << "There was a problem connecting to : " << oConfig.getPort() << std::endl;
+        return -1;
+    }
+    oModbus.ModbusDisconnect();
+    std::cout << "=========================== [END] Reading test ================================" << std::endl;
+}
+/* Define to 1 if you have the <arpa/inet.h> header file. */
+#define HAVE_ARPA_INET_H 1
+
+/* Define to 1 if you have the declaration of `TIOCSRS485', and to 0 if you don't. */
+#define HAVE_DECL_TIOCSRS485 0
+
+/* Define to 1 if you have the declaration of `TIOCM_RTS', and to 0 if you don't. */
+#define HAVE_DECL_TIOCM_RTS 0
+
+/* Define to 1 if you have the declaration of `__CYGWIN__', and to 0 if you don't. */
+#define HAVE_DECL___CYGWIN__ 0
+
+/* Define to 1 if you have the <dlfcn.h> header file. */
+#define HAVE_DLFCN_H 1
+
+/* Define to 1 if you have the <errno.h> header file. */
+#define HAVE_ERRNO_H 1
+
+/* Define to 1 if you have the <fcntl.h> header file. */
+#define HAVE_FCNTL_H 1
+
+/* Define to 1 if you have the `fork' function. */
+#define HAVE_FORK 1
+
+/* Define to 1 if you have the `getaddrinfo' function. */
+#define HAVE_GETADDRINFO 1
+
+/* Define to 1 if you have the `gettimeofday' function. */
+#define HAVE_GETTIMEOFDAY 1
+
+/* Define to 1 if you have the `inet_ntoa' function. */
+#define HAVE_INET_NTOA 1
+
+/* Define to 1 if you have the <inttypes.h> header file. */
+#define HAVE_INTTYPES_H 1
+
+/* Define to 1 if you have the <limits.h> header file. */
+#define HAVE_LIMITS_H 1
+
+/* Define to 1 if you have the <linux/serial.h> header file. */
+#define HAVE_LINUX_SERIAL_H 1
+
+/* Define to 1 if you have the <memory.h> header file. */
+#define HAVE_MEMORY_H 1
+
+/* Define to 1 if you have the `memset' function. */
+#define HAVE_MEMSET 1
+
+/* Define to 1 if you have the <netdb.h> header file. */
+#define HAVE_NETDB_H 1
+
+/* Define to 1 if you have the <netinet/in.h> header file. */
+#define HAVE_NETINET_IN_H 1
+
+/* Define to 1 if you have the <netinet/tcp.h> header file. */
+#define HAVE_NETINET_TCP_H 1
+
+/* Define to 1 if you have the `select' function. */
+#define HAVE_SELECT 1
+
+/* Define to 1 if you have the `socket' function. */
+#define HAVE_SOCKET 1
+
+/* Define to 1 if you have the <stdint.h> header file. */
+#define HAVE_STDINT_H 1
+
+/* Define to 1 if you have the <stdlib.h> header file. */
+#define HAVE_STDLIB_H 1
+
+/* Define to 1 if you have the `strerror' function. */
+#define HAVE_STRERROR 1
+
+/* Define to 1 if you have the <strings.h> header file. */
+#define HAVE_STRINGS_H 1
+
+/* Define to 1 if you have the <string.h> header file. */
+#define HAVE_STRING_H 1
+
+/* Define to 1 if you have the `strlcpy' function. */
+/* #undef HAVE_STRLCPY */
+
+/* Define to 1 if you have the <sys/ioctl.h> header file. */
+#define HAVE_SYS_IOCTL_H 1
+
+/* Define to 1 if you have the <sys/socket.h> header file. */
+#define HAVE_SYS_SOCKET_H 1
+
+/* Define to 1 if you have the <sys/stat.h> header file. */
+#define HAVE_SYS_STAT_H 1
+
+/* Define to 1 if you have the <sys/time.h> header file. */
+#define HAVE_SYS_TIME_H 1
+
+/* Define to 1 if you have the <sys/types.h> header file. */
+#define HAVE_SYS_TYPES_H 1
+
+/* Define to 1 if you have the <termios.h> header file. */
+#define HAVE_TERMIOS_H 1
+
+/* Define to 1 if you have the <time.h> header file. */
+#define HAVE_TIME_H 1
+
+/* Define to 1 if you have the <unistd.h> header file. */
+#define HAVE_UNISTD_H 1
+
+/* Define to 1 if you have the `vfork' function. */
+#define HAVE_VFORK 1
+
+/* Define to 1 if you have the <vfork.h> header file. */
+/* #undef HAVE_VFORK_H */
+
+/* Define to 1 if you have the <winsock2.h> header file. */
+/* #undef HAVE_WINSOCK2_H */
+
+/* Define to 1 if `fork' works. */
+#define HAVE_WORKING_FORK 1
+
+/* Define to 1 if `vfork' works. */
+#define HAVE_WORKING_VFORK 1
+
+/* Define to the sub-directory in which libtool stores uninstalled libraries.
+   */
+#define LT_OBJDIR ".libs/"
+
+/* Name of package */
+#define PACKAGE "libmodbus"
+
+/* Define to the address where bug reports for this package should be sent. */
+#define PACKAGE_BUGREPORT "https://github.com/stephane/libmodbus/issues"
+
+/* Define to the full name of this package. */
+#define PACKAGE_NAME "libmodbus"
+
+/* Define to the full name and version of this package. */
+#define PACKAGE_STRING "libmodbus 3.1.0-1"
+
+/* Define to the one symbol short name of this package. */
+#define PACKAGE_TARNAME "modbus"
+
+/* Define to the home page for this package. */
+#define PACKAGE_URL ""
+
+/* Define to the version of this package. */
+#define PACKAGE_VERSION "1.0.0"
+
+/* Define to 1 if you have the ANSI C header files. */
+#define STDC_HEADERS 1
+
+/* Define to 1 if you can safely include both <sys/time.h> and <time.h>. */
+#define TIME_WITH_SYS_TIME 1
+
+/* Version number of package */
+#define VERSION "1.0.0"
+
+/* Define to empty if `const' does not conform to ANSI C. */
+/* #undef const */
+
+/* Define to `int' if <sys/types.h> does not define. */
+/* #undef pid_t */
+
+/* Define to `unsigned int' if <sys/types.h> does not define. */
+/* #undef size_t */
+
+/* Define as `fork' if `vfork' does not work. */
+/* #undef vfork */
+
+#include <stdlib.h>
+
+#include "stdint.h"
+
+#include <string.h>
+#include <assert.h>
+#include <arpa/inet.h>
+#include <config.h>
+
+#include "modbus.h"
+
+#if defined(HAVE_BYTESWAP_H)
+#  include <byteswap.h>
+#endif
+
+#if defined(__GNUC__)
+#  define GCC_VERSION (__GNUC__ * 100 + __GNUC_MINOR__ * 10)
+#  if GCC_VERSION >= 430
+// Since GCC >= 4.30, GCC provides __builtin_bswapXX() alternatives so we switch to them
+#    undef bswap_32
+#    define bswap_32 __builtin_bswap32
+#  endif
+#endif
+
+#if !defined(__CYGWIN__) && !defined(bswap_16)
+#pragma  message "Fallback on C functions for bswap_16"
+static inline uint16_t bswap_16(uint16_t x)
+{
+    return (x >> 8) | (x << 8);
+}
+#endif
+
+#if !defined(bswap_32)
+#pragma  message "Fallback on C functions for bswap_32"
+static inline uint32_t bswap_32(uint32_t x)
+{
+    return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
+}
+#endif
+
+/* Sets many bits from a single byte value (all 8 bits of the byte value are
+   set) */
+void modbus_set_bits_from_byte( uint8_t *dest, int idx, const uint8_t value )
+{
+    int i;
+
+    for (i=0; i < 8; i++) 
+    {
+        dest[idx+i] = (value & (1 << i)) ? 1 : 0;
+    }
+}
+
+/* Sets many bits from a table of bytes (only the bits between idx and
+   idx + nb_bits are set) */
+void modbus_set_bits_from_bytes( uint8_t *dest, int idx, unsigned int nb_bits, const uint8_t *tab_byte )
+{
+    unsigned int i;
+    int shift = 0;
+
+    for ( i = idx; i < idx + nb_bits; i++ ) 
+    {
+        dest[i] = tab_byte[(i - idx) / 8] & (1 << shift) ? 1 : 0;
+        /* gcc doesn't like: shift = (++shift) % 8; */
+        shift++;
+        shift %= 8;
+    }
+}
+
+/* Gets the byte value from many bits.
+   To obtain a full byte, set nb_bits to 8. */
+uint8_t modbus_get_byte_from_bits( const uint8_t *src, int idx, unsigned int nb_bits )
+{
+    unsigned int i;
+    uint8_t value = 0;
+
+    if (nb_bits > 8) 
+    {
+        /* Assert is ignored if NDEBUG is set */
+        assert(nb_bits < 8);
+        nb_bits = 8;
+    }
+
+    for (i=0; i < nb_bits; i++) 
+    {
+        value |= (src[idx+i] << i);
+    }
+
+    return value;
+}
+
+/* Get a float from 4 bytes (Modbus) without any conversion (ABCD) */
+float modbus_get_float_abcd(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl(((uint32_t)src[0] << 16) + src[1]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) in inversed format (DCBA) */
+float modbus_get_float_dcba(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl(bswap_32((((uint32_t)src[0]) << 16) + src[1]));
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) with swapped bytes (BADC) */
+float modbus_get_float_badc(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl((uint32_t)(bswap_16(src[0]) << 16) + bswap_16(src[1]));
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a float from 4 bytes (Modbus) with swapped words (CDAB) */
+float modbus_get_float_cdab(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = ntohl((((uint32_t)src[1]) << 16) + src[0]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* DEPRECATED - Get a float from 4 bytes in sort of Modbus format */
+float modbus_get_float(const uint16_t *src)
+{
+    float f;
+    uint32_t i;
+
+    i = (((uint32_t)src[1]) << 16) + src[0];
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Set a float to 4 bytes for Modbus w/o any conversion (ABCD) */
+void modbus_set_float_abcd(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = htonl(i);
+    dest[0] = (uint16_t)(i >> 16);
+    dest[1] = (uint16_t)i;
+}
+
+/* Set a float to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
+void modbus_set_float_dcba(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = bswap_32(htonl(i));
+    dest[0] = (uint16_t)(i >> 16);
+    dest[1] = (uint16_t)i;
+}
+
+/* Set a float to 4 bytes for Modbus with byte swap conversion (BADC) */
+void modbus_set_float_badc(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = htonl(i);
+    dest[0] = (uint16_t)bswap_16(i >> 16);
+    dest[1] = (uint16_t)bswap_16(i & 0xFFFF);
+}
+
+/* Set a float to 4 bytes for Modbus with word swap conversion (CDAB) */
+void modbus_set_float_cdab(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    i = htonl(i);
+    dest[0] = (uint16_t)i;
+    dest[1] = (uint16_t)(i >> 16);
+}
+
+/* DEPRECATED - Set a float to 4 bytes in a sort of Modbus format! */
+void modbus_set_float(float f, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &f, sizeof(uint32_t));
+    dest[0] = (uint16_t)i;
+    dest[1] = (uint16_t)(i >> 16);
+}
+
+#pragma once
+
+# include <stdint.h>
+# include <sys/time.h>
+#include <sys/types.h>
+#include <config.h>
+
+#include "modbus.h"
+
+MODBUS_BEGIN_DECLS
+
+/* It's not really the minimal length (the real one is report slave ID
+ * in RTU (4 bytes)) but it's a convenient size to use in RTU or TCP
+ * communications to read many values or write a single one.
+ * Maximum between :
+ * - HEADER_LENGTH_TCP (7) + function (1) + address (2) + number (2)
+ * - HEADER_LENGTH_RTU (1) + function (1) + address (2) + number (2) + CRC (2)
+ */
+#define _MIN_REQ_LENGTH 12
+#define _REPORT_SLAVE_ID 180
+#define _MODBUS_EXCEPTION_RSP_LENGTH 5
+
+/* Timeouts in microsecond (0.5 s) */
+#define _RESPONSE_TIMEOUT    500000
+#define _BYTE_TIMEOUT        500000
+
+typedef enum 
+{
+    _MODBUS_BACKEND_TYPE_RTU=0,
+    _MODBUS_BACKEND_TYPE_TCP
+} modbus_backend_type_t;
+
+/*
+ *  ---------- Request     Indication ----------
+ *  | Client | ---------------------->| Server |
+ *  ---------- Confirmation  Response ----------
+ */
+typedef enum 
+{
+    /* Request message on the server side */
+    MSG_INDICATION,
+    /* Request message on the client side */
+    MSG_CONFIRMATION
+} msg_type_t;
+
+/* This structure reduces the number of params in functions and so
+ * optimizes the speed of execution (~ 37%). */
+typedef struct _sft 
+{
+    int slave;
+    int function;
+    int t_id;
+} sft_t;
+
+typedef struct _modbus_backend 
+{
+    unsigned int backend_type;
+    unsigned int header_length;
+    unsigned int checksum_length;
+    unsigned int max_adu_length;
+    int (*set_slave) (modbus_t *ctx, int slave);
+    int (*build_request_basis) (modbus_t *ctx, int function, int addr, int nb, uint8_t *req);
+    int (*build_response_basis) (sft_t *sft, uint8_t *rsp);
+    int (*prepare_response_tid) (const uint8_t *req, int *req_length);
+    int (*send_msg_pre) (uint8_t *req, int req_length);
+    ssize_t (*send) (modbus_t *ctx, const uint8_t *req, int req_length);
+    int (*receive) (modbus_t *ctx, uint8_t *req);
+    ssize_t (*recv) (modbus_t *ctx, uint8_t *rsp, int rsp_length);
+    int (*check_integrity) (modbus_t *ctx, uint8_t *msg, const int msg_length);
+    int (*pre_check_confirmation) (modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length);
+    int (*connect) (modbus_t *ctx);
+    void (*close) (modbus_t *ctx);
+    int (*flush) (modbus_t *ctx);
+    int (*select) (modbus_t *ctx, fd_set *rset, struct timeval *tv, int msg_length);
+    void (*free) (modbus_t *ctx);
+} modbus_backend_t;
+
+struct _modbus 
+{
+    /* Slave address */
+    int slave;
+    /* Socket or file descriptor */
+    int s;
+    int debug;
+    int error_recovery;
+    struct timeval response_timeout;
+    struct timeval byte_timeout;
+    const modbus_backend_t *backend;
+    void *backend_data;
+};
+
+void _modbus_init_common(modbus_t *ctx);
+void _error_print(modbus_t *ctx, const char *context);
+int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type);
+
+#ifndef HAVE_STRLCPY
+size_t strlcpy(char *dest, const char *src, size_t dest_size);
+#endif
+
+MODBUS_END_DECLS
+
+#pragma once
+
+#include <stdint.h>
+#include <termios.h>
+
+#define _MODBUS_RTU_HEADER_LENGTH      1
+#define _MODBUS_RTU_PRESET_REQ_LENGTH  6
+#define _MODBUS_RTU_PRESET_RSP_LENGTH  2
+
+#define _MODBUS_RTU_CHECKSUM_LENGTH    2
+
+typedef struct _modbus_rtu
+{
+    /* Device: "/dev/ttyS0", "/dev/ttyUSB0" or "/dev/tty.USA19*" on Mac OS X. */
+    char *device;
+    /* Bauds: 9600, 19200, 57600, 115200, etc */
+    int baud;
+    /* Data bit */
+    uint8_t data_bit;
+    /* Stop bit */
+    uint8_t stop_bit;
+    /* Parity: 'N', 'O', 'E' */
+    char parity;
+    /* Save old termios settings */
+    struct termios old_tios;
+
+#if HAVE_DECL_TIOCSRS485
+    int serial_mode;
+#endif
+
+    /* To handle many slaves on the same link */
+    int confirmation_to_ignore;
+} modbus_rtu_t;
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <errno.h>
+#include <fcntl.h>
+#include <string.h>
+#include <unistd.h>
+#include <assert.h>
+
+#include "modbus-private.h"
+
+#include "modbus-rtu.h"
+#include "modbus-rtu-private.h"
+
+#if HAVE_DECL_TIOCSRS485 || HAVE_DECL_TIOCM_RTS
+#include <sys/ioctl.h>
+#endif
+
+#if HAVE_DECL_TIOCSRS485
+#include <linux/serial.h>
+#endif
+
+/* Table of CRC values for high-order byte */
+static const uint8_t table_crc_hi[] = {
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
+    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+    0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
+    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40,
+    0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1,
+    0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40
+};
+
+/* Table of CRC values for low-order byte */
+static const uint8_t table_crc_lo[] = {
+    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06,
+    0x07, 0xC7, 0x05, 0xC5, 0xC4, 0x04, 0xCC, 0x0C, 0x0D, 0xCD,
+    0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
+    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A,
+    0x1E, 0xDE, 0xDF, 0x1F, 0xDD, 0x1D, 0x1C, 0xDC, 0x14, 0xD4,
+    0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3,
+    0xF2, 0x32, 0x36, 0xF6, 0xF7, 0x37, 0xF5, 0x35, 0x34, 0xF4,
+    0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
+    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29,
+    0xEB, 0x2B, 0x2A, 0xEA, 0xEE, 0x2E, 0x2F, 0xEF, 0x2D, 0xED,
+    0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60,
+    0x61, 0xA1, 0x63, 0xA3, 0xA2, 0x62, 0x66, 0xA6, 0xA7, 0x67,
+    0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
+    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68,
+    0x78, 0xB8, 0xB9, 0x79, 0xBB, 0x7B, 0x7A, 0xBA, 0xBE, 0x7E,
+    0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71,
+    0x70, 0xB0, 0x50, 0x90, 0x91, 0x51, 0x93, 0x53, 0x52, 0x92,
+    0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
+    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B,
+    0x99, 0x59, 0x58, 0x98, 0x88, 0x48, 0x49, 0x89, 0x4B, 0x8B,
+    0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42,
+    0x43, 0x83, 0x41, 0x81, 0x80, 0x40
+};
+
+/* Define the slave ID of the remote device to talk in master mode or set the
+ * internal slave ID in slave mode */
+static int _modbus_set_slave( modbus_t *ctx, int slave )
+{
+    /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
+    if (slave >= 0 && slave <= 247) 
+    {
+        ctx->slave = slave;
+    } 
+    else 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return 0;
+}
+
+/* Builds a RTU request header */
+static int _modbus_rtu_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
+{
+    assert(ctx->slave != -1);
+    req[0] = ctx->slave;
+    req[1] = function;
+    req[2] = addr >> 8;
+    req[3] = addr & 0x00ff;
+    req[4] = nb >> 8;
+    req[5] = nb & 0x00ff;
+
+    return _MODBUS_RTU_PRESET_REQ_LENGTH;
+}
+
+/* Builds a RTU response header */
+static int _modbus_rtu_build_response_basis( sft_t *sft, uint8_t *rsp )
+{
+    /* In this case, the slave is certainly valid because a check is already
+     * done in _modbus_rtu_listen */
+    rsp[0] = sft->slave;
+    rsp[1] = sft->function;
+
+    return _MODBUS_RTU_PRESET_RSP_LENGTH;
+}
+
+static uint16_t crc16( uint8_t *buffer, uint16_t buffer_length )
+{
+    uint8_t crc_hi = 0xFF;  /* high CRC byte initialized */
+    uint8_t crc_lo = 0xFF;  /* low CRC byte initialized */
+    unsigned int i;         /* will index into CRC lookup */
+
+    /* pass through message buffer */
+    while (buffer_length--) {
+        i = crc_hi ^ *buffer++; /* calculate the CRC  */
+        crc_hi = crc_lo ^ table_crc_hi[i];
+        crc_lo = table_crc_lo[i];
+    }
+
+    return (crc_hi << 8 | crc_lo);
+}
+
+static int _modbus_rtu_prepare_response_tid( const uint8_t *req, int *req_length )
+{
+    (*req_length) -= _MODBUS_RTU_CHECKSUM_LENGTH;
+    /* No TID */
+    return 0;
+}
+
+static int _modbus_rtu_send_msg_pre( uint8_t *req, int req_length )
+{
+    uint16_t crc = crc16( req, req_length );
+    req[req_length++] = crc >> 8;
+    req[req_length++] = crc & 0x00FF;
+
+    return req_length;
+}
+
+#if HAVE_DECL_TIOCM_RTS
+static void _modbus_rtu_ioctl_rts( modbus_t *ctx, int on )
+{
+    int fd = ctx->s;
+    int flags;
+
+    ioctl(fd, TIOCMGET, &flags);
+    if (on) 
+    {
+        flags |= TIOCM_RTS;
+    } 
+    else 
+    {
+        flags &= ~TIOCM_RTS;
+    }
+    ioctl(fd, TIOCMSET, &flags);
+}
+#endif
+
+static ssize_t _modbus_rtu_send( modbus_t *ctx, const uint8_t *req, int req_length )
+{
+#if HAVE_DECL_TIOCM_RTS
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+    if (ctx_rtu->rts != MODBUS_RTU_RTS_NONE) 
+    {
+        ssize_t size;
+
+        if (ctx->debug) {
+            fprintf(stderr, "Sending request using RTS signal\n");
+        }
+
+        ctx_rtu->set_rts(ctx, ctx_rtu->rts == MODBUS_RTU_RTS_UP);
+        usleep(ctx_rtu->rts_delay);
+
+        size = write(ctx->s, req, req_length);
+
+        usleep(ctx_rtu->onebyte_time * req_length + ctx_rtu->rts_delay);
+        ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
+
+        return size;
+    } 
+    else 
+    {
+#endif
+        return write(ctx->s, req, req_length);
+#if HAVE_DECL_TIOCM_RTS
+    }
+#endif
+}
+
+static int _modbus_rtu_receive( modbus_t *ctx, uint8_t *req )
+{
+    int rc;
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+
+    if( ctx_rtu->confirmation_to_ignore ) 
+    {
+        _modbus_receive_msg(ctx, req, MSG_CONFIRMATION);
+        /* Ignore errors and reset the flag */
+        ctx_rtu->confirmation_to_ignore = FALSE;
+        rc = 0;
+        if( ctx->debug ) 
+        {
+            printf("Confirmation to ignore\n");
+        }
+    } 
+    else 
+    {
+        rc = _modbus_receive_msg( ctx, req, MSG_INDICATION );
+        if( rc == 0 ) 
+        {
+            /* The next expected message is a confirmation to ignore */
+            ctx_rtu->confirmation_to_ignore = TRUE;
+        }
+    }
+    return rc;
+}
+
+static ssize_t _modbus_rtu_recv( modbus_t *ctx, uint8_t *rsp, int rsp_length )
+{
+    return read(ctx->s, rsp, rsp_length);
+}
+
+static int _modbus_rtu_flush(modbus_t *);
+
+static int _modbus_rtu_pre_check_confirmation(modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length)
+{
+    /* Check responding slave is the slave we requested (except for broacast request) */
+    if (req[0] != rsp[0] && req[0] != MODBUS_BROADCAST_ADDRESS) 
+    {
+        if (ctx->debug) 
+        {
+            fprintf(stderr, "The responding slave %d isn't the requested slave %d\n", rsp[0], req[0]);
+        }
+        errno = EMBBADSLAVE;
+        return -1;
+    } 
+
+    return 0;
+}
+
+/* The check_crc16 function shall return 0 is the message is ignored and the
+   message length if the CRC is valid. Otherwise it shall return -1 and set
+   errno to EMBADCRC. */
+static int _modbus_rtu_check_integrity(modbus_t *ctx, uint8_t *msg, const int msg_length)
+{
+    uint16_t crc_calculated;
+    uint16_t crc_received;
+    int slave = msg[0];
+
+    /* Filter on the Modbus unit identifier (slave) in RTU mode to avoid useless
+     * CRC computing. */
+    if (slave != ctx->slave && slave != MODBUS_BROADCAST_ADDRESS) 
+    {
+        if (ctx->debug) 
+        {
+            printf("Request for slave %d ignored (not %d)\n", slave, ctx->slave);
+        }
+        /* Following call to check_confirmation handles this error */
+        return 0;
+    }
+
+    crc_calculated = crc16(msg, msg_length - 2);
+    crc_received = (msg[msg_length - 2] << 8) | msg[msg_length - 1];
+
+    /* Check CRC of msg */
+    if (crc_calculated == crc_received) 
+    {
+        return msg_length;
+    } 
+    else 
+    {
+        if (ctx->debug) 
+        {
+            fprintf(stderr, "ERROR CRC received 0x%0X != CRC calculated 0x%0X\n",
+                    crc_received, crc_calculated);
+        }
+
+        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL)
+        {
+            _modbus_rtu_flush(ctx);
+        }
+        errno = EMBBADCRC;
+        return -1;
+    }
+}
+
+/* Sets up a serial port for RTU communications */
+static int _modbus_rtu_connect(modbus_t *ctx)
+{
+    struct termios tios;
+    speed_t speed;
+    int flags;
+
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+
+    if (ctx->debug) 
+    {
+        printf("Opening %s at %d bauds (%c, %d, %d)\n",
+               ctx_rtu->device, ctx_rtu->baud, ctx_rtu->parity, ctx_rtu->data_bit, ctx_rtu->stop_bit);
+    }
+
+    /* The O_NOCTTY flag tells UNIX that this program doesn't want
+       to be the "controlling terminal" for that port. If you
+       don't specify this then any input (such as keyboard abort
+       signals and so forth) will affect your process
+
+       Timeouts are ignored in canonical input mode or when the
+       NDELAY option is set on the file via open or fcntl */
+    flags = O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL;
+#ifdef O_CLOEXEC
+    flags |= O_CLOEXEC;
+#endif
+
+    ctx->s = open(ctx_rtu->device, flags);
+    if (ctx->s == -1) {
+        if (ctx->debug) {
+            fprintf(stderr, "ERROR Can't open the device %s (%s)\n",
+                    ctx_rtu->device, strerror(errno));
+        }
+        return -1;
+    }
+
+    /* Save */
+    tcgetattr(ctx->s, &ctx_rtu->old_tios);
+
+    memset(&tios, 0, sizeof(struct termios));
+
+    /* C_ISPEED     Input baud (new interface)
+       C_OSPEED     Output baud (new interface)
+    */
+    switch (ctx_rtu->baud) 
+    {
+    case 110:
+        speed = B110;
+        break;
+    case 300:
+        speed = B300;
+        break;
+    case 600:
+        speed = B600;
+        break;
+    case 1200:
+        speed = B1200;
+        break;
+    case 2400:
+        speed = B2400;
+        break;
+    case 4800:
+        speed = B4800;
+        break;
+    case 9600:
+        speed = B9600;
+        break;
+    case 19200:
+        speed = B19200;
+        break;
+    case 38400:
+        speed = B38400;
+        break;
+#ifdef B57600
+    case 57600:
+        speed = B57600;
+        break;
+#endif
+#ifdef B115200
+    case 115200:
+        speed = B115200;
+        break;
+#endif
+#ifdef B230400
+    case 230400:
+        speed = B230400;
+        break;
+#endif
+#ifdef B460800
+    case 460800:
+        speed = B460800;
+        break;
+#endif
+#ifdef B500000
+    case 500000:
+        speed = B500000;
+        break;
+#endif
+#ifdef B576000
+    case 576000:
+        speed = B576000;
+        break;
+#endif
+#ifdef B921600
+    case 921600:
+        speed = B921600;
+        break;
+#endif
+#ifdef B1000000
+    case 1000000:
+        speed = B1000000;
+        break;
+#endif
+#ifdef B1152000
+   case 1152000:
+        speed = B1152000;
+        break;
+#endif
+#ifdef B1500000
+    case 1500000:
+        speed = B1500000;
+        break;
+#endif
+#ifdef B2500000
+    case 2500000:
+        speed = B2500000;
+        break;
+#endif
+#ifdef B3000000
+    case 3000000:
+        speed = B3000000;
+        break;
+#endif
+#ifdef B3500000
+    case 3500000:
+        speed = B3500000;
+        break;
+#endif
+#ifdef B4000000
+    case 4000000:
+        speed = B4000000;
+        break;
+#endif
+    default:
+        speed = B9600;
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "WARNING Unknown baud rate %d for %s (B9600 used)\n",
+                    ctx_rtu->baud, ctx_rtu->device);
+        }
+    }
+
+    /* Set the baud rate */
+    if ((cfsetispeed(&tios, speed) < 0) ||
+        (cfsetospeed(&tios, speed) < 0)) {
+        close(ctx->s);
+        ctx->s = -1;
+        return -1;
+    }
+
+    /* C_CFLAG      Control options
+       CLOCAL       Local line - do not change "owner" of port
+       CREAD        Enable receiver
+    */
+    tios.c_cflag |= (CREAD | CLOCAL);
+    /* CSIZE, HUPCL, CRTSCTS (hardware flow control) */
+
+    /* Set data bits (5, 6, 7, 8 bits)
+       CSIZE        Bit mask for data bits
+    */
+    tios.c_cflag &= ~CSIZE;
+    switch (ctx_rtu->data_bit) {
+    case 5:
+        tios.c_cflag |= CS5;
+        break;
+    case 6:
+        tios.c_cflag |= CS6;
+        break;
+    case 7:
+        tios.c_cflag |= CS7;
+        break;
+    case 8:
+    default:
+        tios.c_cflag |= CS8;
+        break;
+    }
+
+    /* Stop bit (1 or 2) */
+    if (ctx_rtu->stop_bit == 1)
+        tios.c_cflag &=~ CSTOPB;
+    else /* 2 */
+        tios.c_cflag |= CSTOPB;
+
+    /* PARENB       Enable parity bit
+       PARODD       Use odd parity instead of even */
+    if (ctx_rtu->parity == 'N') {
+        /* None */
+        tios.c_cflag &=~ PARENB;
+    } else if (ctx_rtu->parity == 'E') {
+        /* Even */
+        tios.c_cflag |= PARENB;
+        tios.c_cflag &=~ PARODD;
+    } else {
+        /* Odd */
+        tios.c_cflag |= PARENB;
+        tios.c_cflag |= PARODD;
+    }
+
+    /* Read the man page of termios if you need more information. */
+
+    /* This field isn't used on POSIX systems
+       tios.c_line = 0;
+    */
+
+    /* C_LFLAG      Line options
+
+       ISIG Enable SIGINTR, SIGSUSP, SIGDSUSP, and SIGQUIT signals
+       ICANON       Enable canonical input (else raw)
+       XCASE        Map uppercase \lowercase (obsolete)
+       ECHO Enable echoing of input characters
+       ECHOE        Echo erase character as BS-SP-BS
+       ECHOK        Echo NL after kill character
+       ECHONL       Echo NL
+       NOFLSH       Disable flushing of input buffers after
+       interrupt or quit characters
+       IEXTEN       Enable extended functions
+       ECHOCTL      Echo control characters as ^char and delete as ~?
+       ECHOPRT      Echo erased character as character erased
+       ECHOKE       BS-SP-BS entire line on line kill
+       FLUSHO       Output being flushed
+       PENDIN       Retype pending input at next read or input char
+       TOSTOP       Send SIGTTOU for background output
+
+       Canonical input is line-oriented. Input characters are put
+       into a buffer which can be edited interactively by the user
+       until a CR (carriage return) or LF (line feed) character is
+       received.
+
+       Raw input is unprocessed. Input characters are passed
+       through exactly as they are received, when they are
+       received. Generally you'll deselect the ICANON, ECHO,
+       ECHOE, and ISIG options when using raw input
+    */
+
+    /* Raw input */
+    tios.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG);
+
+    /* C_IFLAG      Input options
+
+       Constant     Description
+       INPCK        Enable parity check
+       IGNPAR       Ignore parity errors
+       PARMRK       Mark parity errors
+       ISTRIP       Strip parity bits
+       IXON Enable software flow control (outgoing)
+       IXOFF        Enable software flow control (incoming)
+       IXANY        Allow any character to start flow again
+       IGNBRK       Ignore break condition
+       BRKINT       Send a SIGINT when a break condition is detected
+       INLCR        Map NL to CR
+       IGNCR        Ignore CR
+       ICRNL        Map CR to NL
+       IUCLC        Map uppercase to lowercase
+       IMAXBEL      Echo BEL on input line too long
+    */
+    if (ctx_rtu->parity == 'N') {
+        /* None */
+        tios.c_iflag &= ~INPCK;
+    } else {
+        tios.c_iflag |= INPCK;
+    }
+
+    /* Software flow control is disabled */
+    tios.c_iflag &= ~(IXON | IXOFF | IXANY);
+
+    /* C_OFLAG      Output options
+       OPOST        Postprocess output (not set = raw output)
+       ONLCR        Map NL to CR-NL
+
+       ONCLR ant others needs OPOST to be enabled
+    */
+
+    /* Raw ouput */
+    tios.c_oflag &=~ OPOST;
+
+    /* C_CC         Control characters
+       VMIN         Minimum number of characters to read
+       VTIME        Time to wait for data (tenths of seconds)
+
+       UNIX serial interface drivers provide the ability to
+       specify character and packet timeouts. Two elements of the
+       c_cc array are used for timeouts: VMIN and VTIME. Timeouts
+       are ignored in canonical input mode or when the NDELAY
+       option is set on the file via open or fcntl.
+
+       VMIN specifies the minimum number of characters to read. If
+       it is set to 0, then the VTIME value specifies the time to
+       wait for every character read. Note that this does not mean
+       that a read call for N bytes will wait for N characters to
+       come in. Rather, the timeout will apply to the first
+       character and the read call will return the number of
+       characters immediately available (up to the number you
+       request).
+
+       If VMIN is non-zero, VTIME specifies the time to wait for
+       the first character read. If a character is read within the
+       time given, any read will block (wait) until all VMIN
+       characters are read. That is, once the first character is
+       read, the serial interface driver expects to receive an
+       entire packet of characters (VMIN bytes total). If no
+       character is read within the time allowed, then the call to
+       read returns 0. This method allows you to tell the serial
+       driver you need exactly N bytes and any read call will
+       return 0 or N bytes. However, the timeout only applies to
+       the first character read, so if for some reason the driver
+       misses one character inside the N byte packet then the read
+       call could block forever waiting for additional input
+       characters.
+
+       VTIME specifies the amount of time to wait for incoming
+       characters in tenths of seconds. If VTIME is set to 0 (the
+       default), reads will block (wait) indefinitely unless the
+       NDELAY option is set on the port with open or fcntl.
+    */
+    /* Unused because we use open with the NDELAY option */
+    tios.c_cc[VMIN] = 0;
+    tios.c_cc[VTIME] = 10;  // Set per default to 1 second
+
+    if (tcsetattr(ctx->s, TCSANOW, &tios) < 0) {
+        close(ctx->s);
+        ctx->s = -1;
+        return -1;
+    }
+    return 0;
+}
+
+int modbus_rtu_set_serial_mode(modbus_t *ctx, int mode)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCSRS485
+        modbus_rtu_t *ctx_rtu = ctx->backend_data;
+        struct serial_rs485 rs485conf;
+        memset(&rs485conf, 0x0, sizeof(struct serial_rs485));
+
+        if (mode == MODBUS_RTU_RS485) {
+            rs485conf.flags = SER_RS485_ENABLED;
+            if (ioctl(ctx->s, TIOCSRS485, &rs485conf) < 0) {
+                return -1;
+            }
+
+            ctx_rtu->serial_mode = MODBUS_RTU_RS485;
+            return 0;
+        } else if (mode == MODBUS_RTU_RS232) {
+            /* Turn off RS485 mode only if required */
+            if (ctx_rtu->serial_mode == MODBUS_RTU_RS485) {
+                /* The ioctl call is avoided because it can fail on some RS232 ports */
+                if (ioctl(ctx->s, TIOCSRS485, &rs485conf) < 0) {
+                    return -1;
+                }
+            }
+            ctx_rtu->serial_mode = MODBUS_RTU_RS232;
+            return 0;
+        }
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    }
+
+    /* Wrong backend and invalid mode specified */
+    errno = EINVAL;
+    return -1;
+}
+
+int modbus_rtu_get_serial_mode(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCSRS485
+        modbus_rtu_t *ctx_rtu = ctx->backend_data;
+        return ctx_rtu->serial_mode;
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
+int modbus_rtu_get_rts(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCM_RTS
+        modbus_rtu_t *ctx_rtu = ctx->backend_data;
+        return ctx_rtu->rts;
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
+int modbus_rtu_set_rts(modbus_t *ctx, int mode)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCM_RTS
+        modbus_rtu_t *ctx_rtu = ctx->backend_data;
+
+        if (mode == MODBUS_RTU_RTS_NONE || mode == MODBUS_RTU_RTS_UP ||
+            mode == MODBUS_RTU_RTS_DOWN) {
+            ctx_rtu->rts = mode;
+
+            /* Set the RTS bit in order to not reserve the RS485 bus */
+            ctx_rtu->set_rts(ctx, ctx_rtu->rts != MODBUS_RTU_RTS_UP);
+
+            return 0;
+        } else {
+            errno = EINVAL;
+            return -1;
+        }
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    }
+    /* Wrong backend or invalid mode specified */
+    errno = EINVAL;
+    return -1;
+}
+
+int modbus_rtu_set_custom_rts(modbus_t *ctx, void (*set_rts) (modbus_t *ctx, int on))
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCM_RTS
+        modbus_rtu_t *ctx_rtu = ctx->backend_data;
+        ctx_rtu->set_rts = set_rts;
+        return 0;
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
+int modbus_rtu_get_rts_delay(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCM_RTS
+        modbus_rtu_t *ctx_rtu;
+        ctx_rtu = (modbus_rtu_t *)ctx->backend_data;
+        return ctx_rtu->rts_delay;
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
+int modbus_rtu_set_rts_delay(modbus_t *ctx, int us)
+{
+    if (ctx == NULL || us < 0) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (ctx->backend->backend_type == _MODBUS_BACKEND_TYPE_RTU) {
+#if HAVE_DECL_TIOCM_RTS
+        modbus_rtu_t *ctx_rtu;
+        ctx_rtu = (modbus_rtu_t *)ctx->backend_data;
+        ctx_rtu->rts_delay = us;
+        return 0;
+#else
+        if (ctx->debug) {
+            fprintf(stderr, "This function isn't supported on your platform\n");
+        }
+        errno = ENOTSUP;
+        return -1;
+#endif
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
+static void _modbus_rtu_close(modbus_t *ctx)
+{
+    /* Restore line settings and close file descriptor in RTU mode */
+    modbus_rtu_t *ctx_rtu = ctx->backend_data;
+
+    if (ctx->s != -1) {
+        tcsetattr(ctx->s, TCSANOW, &ctx_rtu->old_tios);
+        close(ctx->s);
+        ctx->s = -1;
+    }
+}
+
+static int _modbus_rtu_flush(modbus_t *ctx)
+{
+    return tcflush(ctx->s, TCIOFLUSH);
+}
+
+static int _modbus_rtu_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
+{
+    int s_rc;
+    while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1)
+    {
+        if (errno == EINTR)
+        {
+            if (ctx->debug)
+            {
+                fprintf(stderr, "A non blocked signal was caught\n");
+            }
+            /* Necessary after an error */
+            FD_ZERO(rset);
+            FD_SET(ctx->s, rset);
+        }
+        else
+        {
+            return -1;
+        }
+    }
+
+    if (s_rc == 0) {
+        /* Timeout */
+        errno = ETIMEDOUT;
+        return -1;
+    }
+
+    return s_rc;
+}
+
+static void _modbus_rtu_free(modbus_t *ctx) {
+    free(((modbus_rtu_t*)ctx->backend_data)->device);
+    free(ctx->backend_data);
+    free(ctx);
+}
+
+const modbus_backend_t _modbus_rtu_backend = {
+    _MODBUS_BACKEND_TYPE_RTU,
+    _MODBUS_RTU_HEADER_LENGTH,
+    _MODBUS_RTU_CHECKSUM_LENGTH,
+    MODBUS_RTU_MAX_ADU_LENGTH,
+    _modbus_set_slave,
+    _modbus_rtu_build_request_basis,
+    _modbus_rtu_build_response_basis,
+    _modbus_rtu_prepare_response_tid,
+    _modbus_rtu_send_msg_pre,
+    _modbus_rtu_send,
+    _modbus_rtu_receive,
+    _modbus_rtu_recv,
+    _modbus_rtu_check_integrity,
+    _modbus_rtu_pre_check_confirmation,
+    _modbus_rtu_connect,
+    _modbus_rtu_close,
+    _modbus_rtu_flush,
+    _modbus_rtu_select,
+    _modbus_rtu_free
+};
+
+modbus_t* modbus_new_rtu(const char *device,
+                         int baud, char parity, int data_bit,
+                         int stop_bit)
+{
+    modbus_t *ctx;
+    modbus_rtu_t *ctx_rtu;
+
+    /* Check device argument */
+    if (device == NULL || *device == 0) {
+        fprintf(stderr, "The device string is empty\n");
+        errno = EINVAL;
+        return NULL;
+    }
+
+    /* Check baud argument */
+    if (baud == 0) {
+        fprintf(stderr, "The baud rate value must not be zero\n");
+        errno = EINVAL;
+        return NULL;
+    }
+
+    ctx = (modbus_t *)malloc(sizeof(modbus_t));
+    _modbus_init_common(ctx);
+    ctx->backend = &_modbus_rtu_backend;
+    ctx->backend_data = (modbus_rtu_t *)malloc(sizeof(modbus_rtu_t));
+    ctx_rtu = (modbus_rtu_t *)ctx->backend_data;
+    ctx_rtu->device = NULL;
+
+    /* Device name and \0 */
+    ctx_rtu->device = (char *)malloc((strlen(device) + 1) * sizeof(char));
+    strcpy(ctx_rtu->device, device);
+
+    ctx_rtu->baud = baud;
+    if (parity == 'N' || parity == 'E' || parity == 'O') {
+        ctx_rtu->parity = parity;
+    } else {
+        modbus_free(ctx);
+        errno = EINVAL;
+        return NULL;
+    }
+    ctx_rtu->data_bit = data_bit;
+    ctx_rtu->stop_bit = stop_bit;
+
+#if HAVE_DECL_TIOCSRS485
+    /* The RS232 mode has been set by default */
+    ctx_rtu->serial_mode = MODBUS_RTU_RS232;
+#endif
+
+#if HAVE_DECL_TIOCM_RTS
+    /* The RTS use has been set by default */
+    ctx_rtu->rts = MODBUS_RTU_RTS_NONE;
+
+    /* Calculate estimated time in micro second to send one byte */
+    ctx_rtu->onebyte_time = 1000000 * (1 + data_bit + (parity == 'N' ? 0 : 1) + stop_bit) / baud;
+
+    /* The internal function is used by default to set RTS */
+    ctx_rtu->set_rts = _modbus_rtu_ioctl_rts;
+
+    /* The delay before and after transmission when toggling the RTS pin */
+    ctx_rtu->rts_delay = ctx_rtu->onebyte_time;
+#endif
+
+    ctx_rtu->confirmation_to_ignore = FALSE;
+
+    return ctx;
+}
+
+#pragma once
+
+#include "modbus.h"
+
+MODBUS_BEGIN_DECLS
+
+/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
+ * RS232 / RS485 ADU = 253 bytes + slave (1 byte) + CRC (2 bytes) = 256 bytes
+ */
+#define MODBUS_RTU_MAX_ADU_LENGTH  256
+
+MODBUS_API modbus_t* modbus_new_rtu(const char *device, int baud, char parity, int data_bit, int stop_bit);
+
+#define MODBUS_RTU_RS232 0
+#define MODBUS_RTU_RS485 1
+
+MODBUS_API int modbus_rtu_set_serial_mode( modbus_t *ctx, int mode );
+MODBUS_API int modbus_rtu_get_serial_mode( modbus_t *ctx );
+
+#define MODBUS_RTU_RTS_NONE   0
+#define MODBUS_RTU_RTS_UP     1
+#define MODBUS_RTU_RTS_DOWN   2
+
+MODBUS_API int modbus_rtu_set_rts( modbus_t *ctx, int mode );
+MODBUS_API int modbus_rtu_get_rts( modbus_t *ctx );
+
+MODBUS_API int modbus_rtu_set_custom_rts( modbus_t *ctx, void ( *set_rts ) ( modbus_t *ctx, int on ) );
+
+MODBUS_API int modbus_rtu_set_rts_delay( modbus_t *ctx, int us );
+MODBUS_API int modbus_rtu_get_rts_delay( modbus_t *ctx );
+
+MODBUS_END_DECLS
+
+#pragma once
+
+#define _MODBUS_TCP_HEADER_LENGTH      7
+#define _MODBUS_TCP_PRESET_REQ_LENGTH 12
+#define _MODBUS_TCP_PRESET_RSP_LENGTH  8
+
+#define _MODBUS_TCP_CHECKSUM_LENGTH    0
+
+/* In both structures, the transaction ID must be placed on first position
+   to have a quick access not dependant of the TCP backend */
+typedef struct _modbus_tcp 
+{
+    /* Extract from MODBUS Messaging on TCP/IP Implementation Guide V1.0b
+       (page 23/46):
+       The transaction identifier is used to associate the future response
+       with the request. This identifier is unique on each TCP connection. */
+    uint16_t t_id;
+    /* TCP port */
+    int port;
+    /* IP address */
+    char ip[16];
+} modbus_tcp_t;
+
+#define _MODBUS_TCP_PI_NODE_LENGTH    1025
+#define _MODBUS_TCP_PI_SERVICE_LENGTH   32
+
+typedef struct _modbus_tcp_pi 
+{
+    /* Transaction ID */
+    uint16_t t_id;
+    /* TCP port */
+    int port;
+    /* Node */
+    char node[_MODBUS_TCP_PI_NODE_LENGTH];
+    /* Service */
+    char service[_MODBUS_TCP_PI_SERVICE_LENGTH];
+} modbus_tcp_pi_t;
+
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <errno.h>
+#include <unistd.h>
+#include <signal.h>
+#include <sys/types.h>
+
+# include <sys/socket.h>
+# include <sys/ioctl.h>
+
+#if defined(__OpenBSD__) || (defined(__FreeBSD__) && __FreeBSD__ < 5)
+# define OS_BSD
+# include <netinet/in_systm.h>
+#endif
+
+# include <netinet/in.h>
+# include <netinet/ip.h>
+# include <netinet/tcp.h>
+# include <arpa/inet.h>
+# include <netdb.h>
+
+#if !defined(MSG_NOSIGNAL)
+#define MSG_NOSIGNAL 0
+#endif
+
+#include "modbus-private.h"
+
+#include "modbus-tcp.h"
+#include "modbus-tcp-private.h"
+
+static int _modbus_set_slave(modbus_t *ctx, int slave)
+{
+    /* Broadcast address is 0 (MODBUS_BROADCAST_ADDRESS) */
+    if ( slave >= 0 && slave <= 247) 
+    {
+        ctx->slave = slave;
+    } 
+    else if ( slave == MODBUS_TCP_SLAVE ) 
+    {
+        /* The special value MODBUS_TCP_SLAVE (0xFF) can be used in TCP mode to
+         * restore the default value. */
+        ctx->slave = slave;
+    } 
+    else 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return 0;
+}
+
+/* Builds a TCP request header */
+static int _modbus_tcp_build_request_basis( modbus_t *ctx, int function, int addr, int nb, uint8_t *req )
+{
+    modbus_tcp_t *ctx_tcp = ctx->backend_data;
+
+    /* Increase transaction ID */
+    if (ctx_tcp->t_id < UINT16_MAX)
+        ctx_tcp->t_id++;
+    else
+        ctx_tcp->t_id = 0;
+
+    req[0] = ctx_tcp->t_id >> 8;
+    req[1] = ctx_tcp->t_id & 0x00ff;
+
+    /* Protocol Modbus */
+    req[2] = 0;
+    req[3] = 0;
+
+    /* Length will be defined later by set_req_length_tcp at offsets 4
+       and 5 */
+
+    req[6] = ctx->slave;
+    req[7] = function;
+    req[8] = addr >> 8;
+    req[9] = addr & 0x00ff;
+    req[10] = nb >> 8;
+    req[11] = nb & 0x00ff;
+
+    return _MODBUS_TCP_PRESET_REQ_LENGTH;
+}
+
+/* Builds a TCP response header */
+static int _modbus_tcp_build_response_basis( sft_t *sft, uint8_t *rsp )
+{
+    /* Extract from MODBUS Messaging on TCP/IP Implementation
+       Guide V1.0b (page 23/46):
+       The transaction identifier is used to associate the future
+       response with the request. */
+    rsp[0] = sft->t_id >> 8;
+    rsp[1] = sft->t_id & 0x00ff;
+
+    /* Protocol Modbus */
+    rsp[2] = 0;
+    rsp[3] = 0;
+
+    /* Length will be set later by send_msg (4 and 5) */
+
+    /* The slave ID is copied from the indication */
+    rsp[6] = sft->slave;
+    rsp[7] = sft->function;
+
+    return _MODBUS_TCP_PRESET_RSP_LENGTH;
+}
+
+
+static int _modbus_tcp_prepare_response_tid( const uint8_t *req, int *req_length )
+{
+    return (req[0] << 8) + req[1];
+}
+
+static int _modbus_tcp_send_msg_pre( uint8_t *req, int req_length )
+{
+    /* Substract the header length to the message length */
+    int mbap_length = req_length - 6;
+
+    req[4] = mbap_length >> 8;
+    req[5] = mbap_length & 0x00FF;
+
+    return req_length;
+}
+
+static ssize_t _modbus_tcp_send( modbus_t *ctx, const uint8_t *req, int req_length )
+{
+    /* MSG_NOSIGNAL
+       Requests not to send SIGPIPE on errors on stream oriented
+       sockets when the other end breaks the connection.  The EPIPE
+       error is still returned. */
+    return send(ctx->s, (const char *)req, req_length, MSG_NOSIGNAL);
+}
+
+static int _modbus_tcp_receive(modbus_t *ctx, uint8_t *req) 
+{
+    return _modbus_receive_msg(ctx, req, MSG_INDICATION);
+}
+
+static ssize_t _modbus_tcp_recv(modbus_t *ctx, uint8_t *rsp, int rsp_length) 
+{
+    return recv( ctx->s, (char *)rsp, rsp_length, 0 );
+}
+
+static int _modbus_tcp_check_integrity( modbus_t *ctx, uint8_t *msg, const int msg_length )
+{
+    return msg_length;
+}
+
+static int _modbus_tcp_pre_check_confirmation( modbus_t *ctx, const uint8_t *req, const uint8_t *rsp, int rsp_length )
+{
+    /* Check transaction ID */
+    if (req[0] != rsp[0] || req[1] != rsp[1]) {
+        if (ctx->debug) 
+        {
+            fprintf(stderr, "Invalid transaction ID received 0x%X (not 0x%X)\n",
+                    (rsp[0] << 8) + rsp[1], (req[0] << 8) + req[1]);
+        }
+        errno = EMBBADDATA;
+        return -1;
+    }
+
+    /* Check protocol ID */
+    if (rsp[2] != 0x0 && rsp[3] != 0x0) 
+    {
+        if (ctx->debug) 
+        {
+            fprintf(stderr, "Invalid protocol ID received 0x%X (not 0x0)\n",
+                    (rsp[2] << 8) + rsp[3]);
+        }
+        errno = EMBBADDATA;
+        return -1;
+    }
+
+    return 0;
+}
+
+static int _modbus_tcp_set_ipv4_options( int s )
+{
+    int rc;
+    int option;
+
+    /* Set the TCP no delay flag */
+    /* SOL_TCP = IPPROTO_TCP */
+    option = 1;
+    rc = setsockopt(s, IPPROTO_TCP, TCP_NODELAY, (const void *)&option, sizeof(int) );
+    if (rc == -1) 
+    {
+        return -1;
+    }
+
+    /* If the OS does not offer SOCK_NONBLOCK, fall back to setting FIONBIO to
+     * make sockets non-blocking */
+    /* Do not care about the return value, this is optional */
+#if !defined(SOCK_NONBLOCK) && defined(FIONBIO)
+    option = 1;
+    ioctl(s, FIONBIO, &option);
+#endif
+
+    /**
+     * Cygwin defines IPTOS_LOWDELAY but can't handle that flag so it's
+     * necessary to workaround that problem.
+     **/
+    /* Set the IP low delay option */
+    option = IPTOS_LOWDELAY;
+    rc = setsockopt(s, IPPROTO_IP, IP_TOS,
+                    (const void *)&option, sizeof(int));
+    if (rc == -1) {
+        return -1;
+    }
+
+    return 0;
+}
+
+static int _connect( int sockfd, const struct sockaddr *addr, socklen_t addrlen, const struct timeval *ro_tv )
+{
+    int rc = connect(sockfd, addr, addrlen);
+
+    if (rc == -1 && errno == EINPROGRESS) 
+    {
+        fd_set wset;
+        int optval;
+        socklen_t optlen = sizeof(optval);
+        struct timeval tv = *ro_tv;
+
+        /* Wait to be available in writing */
+        FD_ZERO(&wset);
+        FD_SET(sockfd, &wset);
+        rc = select(sockfd + 1, NULL, &wset, NULL, &tv);
+        if (rc <= 0) 
+        {
+            /* Timeout or fail */
+            return -1;
+        }
+
+        /* The connection is established if SO_ERROR and optval are set to 0 */
+        rc = getsockopt(sockfd, SOL_SOCKET, SO_ERROR, (void *)&optval, &optlen);
+        if ( rc == 0 && optval == 0 ) 
+        {
+            return 0;
+        } 
+        else 
+        {
+            errno = ECONNREFUSED;
+            return -1;
+        }
+    }
+    return rc;
+}
+
+/* Establishes a modbus TCP connection with a Modbus server. */
+static int _modbus_tcp_connect( modbus_t *ctx )
+{
+    int rc;
+    /* Specialized version of sockaddr for Internet socket address (same size) */
+    struct sockaddr_in addr;
+    modbus_tcp_t *ctx_tcp = ctx->backend_data;
+    int flags = SOCK_STREAM;
+
+#ifdef SOCK_CLOEXEC
+    flags |= SOCK_CLOEXEC;
+#endif
+
+#ifdef SOCK_NONBLOCK
+    flags |= SOCK_NONBLOCK;
+#endif
+
+    ctx->s = socket(PF_INET, flags, 0);
+    if (ctx->s == -1) 
+    {
+        return -1;
+    }
+
+    rc = _modbus_tcp_set_ipv4_options(ctx->s);
+    if (rc == -1) 
+    {
+        close(ctx->s);
+        ctx->s = -1;
+        return -1;
+    }
+
+    if (ctx->debug) 
+    {
+        printf("Connecting to %s:%d\n", ctx_tcp->ip, ctx_tcp->port);
+    }
+
+    addr.sin_family = AF_INET;
+    addr.sin_port = htons(ctx_tcp->port);
+    addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
+    rc = _connect(ctx->s, (struct sockaddr *)&addr, sizeof(addr), &ctx->response_timeout);
+    if( rc == -1 ) 
+    {
+        close( ctx->s );
+        ctx->s = -1;
+        return -1;
+    }
+
+    return 0;
+}
+
+/* Establishes a modbus TCP PI connection with a Modbus server. */
+static int _modbus_tcp_pi_connect( modbus_t *ctx )
+{
+    int rc;
+    struct addrinfo *ai_list;
+    struct addrinfo *ai_ptr;
+    struct addrinfo ai_hints;
+    modbus_tcp_pi_t *ctx_tcp_pi = ctx->backend_data;
+
+    memset(&ai_hints, 0, sizeof(ai_hints));
+#ifdef AI_ADDRCONFIG
+    ai_hints.ai_flags |= AI_ADDRCONFIG;
+#endif
+    ai_hints.ai_family = AF_UNSPEC;
+    ai_hints.ai_socktype = SOCK_STREAM;
+    ai_hints.ai_addr = NULL;
+    ai_hints.ai_canonname = NULL;
+    ai_hints.ai_next = NULL;
+
+    ai_list = NULL;
+    rc = getaddrinfo(ctx_tcp_pi->node, ctx_tcp_pi->service, &ai_hints, &ai_list);
+    if (rc != 0) 
+    {
+        if (ctx->debug) 
+        {
+            fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
+        }
+        errno = ECONNREFUSED;
+        return -1;
+    }
+
+    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) 
+    {
+        int flags = ai_ptr->ai_socktype;
+        int s;
+
+#ifdef SOCK_CLOEXEC
+        flags |= SOCK_CLOEXEC;
+#endif
+
+#ifdef SOCK_NONBLOCK
+        flags |= SOCK_NONBLOCK;
+#endif
+
+        s = socket(ai_ptr->ai_family, flags, ai_ptr->ai_protocol);
+        if (s < 0)
+            continue;
+
+        if (ai_ptr->ai_family == AF_INET)
+            _modbus_tcp_set_ipv4_options(s);
+
+        if (ctx->debug) 
+        {
+            printf("Connecting to [%s]:%s\n", ctx_tcp_pi->node, ctx_tcp_pi->service);
+        }
+
+        rc = _connect(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen, &ctx->response_timeout);
+        if (rc == -1) 
+        {
+            close(s);
+            continue;
+        }
+
+        ctx->s = s;
+        break;
+    }
+
+    freeaddrinfo(ai_list);
+
+    if (ctx->s < 0) 
+    {
+        return -1;
+    }
+
+    return 0;
+}
+
+/* Closes the network connection and socket in TCP mode */
+static void _modbus_tcp_close( modbus_t *ctx )
+{
+    if (ctx->s != -1) 
+    {
+        shutdown(ctx->s, SHUT_RDWR);
+        close(ctx->s);
+        ctx->s = -1;
+    }
+}
+
+static int _modbus_tcp_flush( modbus_t *ctx )
+{
+    int rc;
+    int rc_sum = 0;
+
+    do 
+    {
+        /* Extract the garbage from the socket */
+        char devnull[MODBUS_TCP_MAX_ADU_LENGTH];
+
+        rc = recv(ctx->s, devnull, MODBUS_TCP_MAX_ADU_LENGTH, MSG_DONTWAIT);
+        if ( rc > 0 ) 
+        {
+            rc_sum += rc;
+        }
+    } while ( rc == MODBUS_TCP_MAX_ADU_LENGTH );
+
+    return rc_sum;
+}
+
+/* Listens for any request from one or many modbus masters in TCP */
+int modbus_tcp_listen( modbus_t *ctx, int nb_connection )
+{
+    int new_s;
+    int enable;
+    struct sockaddr_in addr;
+    modbus_tcp_t *ctx_tcp;
+
+    if (ctx == NULL) 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx_tcp = ctx->backend_data;
+
+    new_s = socket(PF_INET, SOCK_STREAM, IPPROTO_TCP);
+    if ( new_s == -1 ) 
+    {
+        return -1;
+    }
+
+    enable = 1;
+    if (setsockopt(new_s, SOL_SOCKET, SO_REUSEADDR, (char *)&enable, sizeof(enable)) == -1) 
+    {
+        close(new_s);
+        return -1;
+    }
+
+    memset(&addr, 0, sizeof(addr));
+    addr.sin_family = AF_INET;
+
+    /* If the modbus port is < to 1024, we need the setuid root. */
+    addr.sin_port = htons(ctx_tcp->port);
+    if (ctx_tcp->ip[0] == '0') 
+    {
+        /* Listen any addresses */
+        addr.sin_addr.s_addr = htonl(INADDR_ANY);
+    } 
+    else 
+    {
+        /* Listen only specified IP address */
+        addr.sin_addr.s_addr = inet_addr(ctx_tcp->ip);
+    }
+    if (bind( new_s, (struct sockaddr *)&addr, sizeof(addr)) == -1) 
+    {
+        close(new_s);
+        return -1;
+    }
+
+    if (listen(new_s, nb_connection) == -1) 
+    {
+        close(new_s);
+        return -1;
+    }
+
+    return new_s;
+}
+
+int modbus_tcp_pi_listen( modbus_t *ctx, int nb_connection )
+{
+    int rc;
+    struct addrinfo *ai_list;
+    struct addrinfo *ai_ptr;
+    struct addrinfo ai_hints;
+    const char *node;
+    const char *service;
+    int new_s;
+    modbus_tcp_pi_t *ctx_tcp_pi;
+
+    if (ctx == NULL) 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx_tcp_pi = ctx->backend_data;
+
+    if ( ctx_tcp_pi->node[0] == 0) 
+    {
+        node = NULL; /* == any */
+    } 
+    else 
+    {
+        node = ctx_tcp_pi->node;
+    }
+
+    if ( ctx_tcp_pi->service[0] == 0 ) 
+    {
+        service = "502";
+    } 
+    else 
+    {
+        service = ctx_tcp_pi->service;
+    }
+
+    memset( &ai_hints, 0, sizeof (ai_hints) );
+    /* If node is not NULL, than the AI_PASSIVE flag is ignored. */
+    ai_hints.ai_flags |= AI_PASSIVE;
+#ifdef AI_ADDRCONFIG
+    ai_hints.ai_flags |= AI_ADDRCONFIG;
+#endif
+    ai_hints.ai_family = AF_UNSPEC;
+    ai_hints.ai_socktype = SOCK_STREAM;
+    ai_hints.ai_addr = NULL;
+    ai_hints.ai_canonname = NULL;
+    ai_hints.ai_next = NULL;
+
+    ai_list = NULL;
+    rc = getaddrinfo(node, service, &ai_hints, &ai_list);
+    if (rc != 0) 
+    {
+        if (ctx->debug) 
+        {
+            fprintf(stderr, "Error returned by getaddrinfo: %s\n", gai_strerror(rc));
+        }
+        errno = ECONNREFUSED;
+        return -1;
+    }
+
+    new_s = -1;
+    for (ai_ptr = ai_list; ai_ptr != NULL; ai_ptr = ai_ptr->ai_next) 
+    {
+        int s;
+
+        s = socket( ai_ptr->ai_family, ai_ptr->ai_socktype, ai_ptr->ai_protocol );
+        if (s < 0) 
+        {
+            if (ctx->debug) 
+            {
+                perror("socket");
+            }
+            continue;
+        } 
+        else 
+        {
+            int enable = 1;
+            rc = setsockopt( s, SOL_SOCKET, SO_REUSEADDR, (void *)&enable, sizeof (enable) );
+            if (rc != 0) 
+            {
+                close(s);
+                if (ctx->debug) 
+                {
+                    perror("setsockopt");
+                }
+                continue;
+            }
+        }
+
+        rc = bind(s, ai_ptr->ai_addr, ai_ptr->ai_addrlen);
+        if (rc != 0) 
+        {
+            close(s);
+            if (ctx->debug) 
+            {
+                perror("bind");
+            }
+            continue;
+        }
+
+        rc = listen(s, nb_connection);
+        if (rc != 0) 
+        {
+            close(s);
+            if (ctx->debug) 
+            {
+                perror("listen");
+            }
+            continue;
+        }
+
+        new_s = s;
+        break;
+    }
+    freeaddrinfo(ai_list);
+
+    if (new_s < 0) 
+    {
+        return -1;
+    }
+
+    return new_s;
+}
+
+int modbus_tcp_accept(modbus_t *ctx, int *s)
+{
+    struct sockaddr_in addr;
+    socklen_t addrlen;
+
+    if (ctx == NULL) 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    addrlen = sizeof(addr);
+#ifdef HAVE_ACCEPT4
+    /* Inherit socket flags and use accept4 call */
+    ctx->s = accept4(*s, (struct sockaddr *)&addr, &addrlen, SOCK_CLOEXEC);
+#else
+    ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
+#endif
+
+    if (ctx->s == -1) 
+    {
+        close(*s);
+        *s = -1;
+        return -1;
+    }
+
+    if (ctx->debug) 
+    {
+        printf("The client connection from %s is accepted\n",
+               inet_ntoa(addr.sin_addr));
+    }
+
+    return ctx->s;
+}
+
+int modbus_tcp_pi_accept( modbus_t *ctx, int *s )
+{
+    struct sockaddr_storage addr;
+    socklen_t addrlen;
+
+    if (ctx == NULL) 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    addrlen = sizeof(addr);
+#ifdef HAVE_ACCEPT4
+    /* Inherit socket flags and use accept4 call */
+    ctx->s = accept4(*s, (struct sockaddr *)&addr, &addrlen, SOCK_CLOEXEC);
+#else
+    ctx->s = accept(*s, (struct sockaddr *)&addr, &addrlen);
+#endif
+    if (ctx->s == -1) 
+    {
+        close(*s);
+        *s = -1;
+    }
+
+    if (ctx->debug) 
+    {
+        printf("The client connection is accepted.\n");
+    }
+
+    return ctx->s;
+}
+
+static int _modbus_tcp_select(modbus_t *ctx, fd_set *rset, struct timeval *tv, int length_to_read)
+{
+    int s_rc;
+    while ((s_rc = select(ctx->s+1, rset, NULL, NULL, tv)) == -1) 
+    {
+        if (errno == EINTR) 
+        {
+            if (ctx->debug) 
+            {
+                fprintf(stderr, "A non blocked signal was caught\n");
+            }
+            /* Necessary after an error */
+            FD_ZERO(rset);
+            FD_SET(ctx->s, rset);
+        } 
+        else 
+        {
+            return -1;
+        }
+    }
+
+    if ( s_rc == 0 ) 
+    {
+        errno = ETIMEDOUT;
+        return -1;
+    }
+
+    return s_rc;
+}
+
+static void _modbus_tcp_free( modbus_t *ctx ) 
+{
+    free(ctx->backend_data);
+    free(ctx);
+}
+
+const modbus_backend_t _modbus_tcp_backend = 
+{
+    _MODBUS_BACKEND_TYPE_TCP,
+    _MODBUS_TCP_HEADER_LENGTH,
+    _MODBUS_TCP_CHECKSUM_LENGTH,
+    MODBUS_TCP_MAX_ADU_LENGTH,
+    _modbus_set_slave,
+    _modbus_tcp_build_request_basis,
+    _modbus_tcp_build_response_basis,
+    _modbus_tcp_prepare_response_tid,
+    _modbus_tcp_send_msg_pre,
+    _modbus_tcp_send,
+    _modbus_tcp_receive,
+    _modbus_tcp_recv,
+    _modbus_tcp_check_integrity,
+    _modbus_tcp_pre_check_confirmation,
+    _modbus_tcp_connect,
+    _modbus_tcp_close,
+    _modbus_tcp_flush,
+    _modbus_tcp_select,
+    _modbus_tcp_free
+};
+
+
+const modbus_backend_t _modbus_tcp_pi_backend = 
+{
+    _MODBUS_BACKEND_TYPE_TCP,
+    _MODBUS_TCP_HEADER_LENGTH,
+    _MODBUS_TCP_CHECKSUM_LENGTH,
+    MODBUS_TCP_MAX_ADU_LENGTH,
+    _modbus_set_slave,
+    _modbus_tcp_build_request_basis,
+    _modbus_tcp_build_response_basis,
+    _modbus_tcp_prepare_response_tid,
+    _modbus_tcp_send_msg_pre,
+    _modbus_tcp_send,
+    _modbus_tcp_receive,
+    _modbus_tcp_recv,
+    _modbus_tcp_check_integrity,
+    _modbus_tcp_pre_check_confirmation,
+    _modbus_tcp_pi_connect,
+    _modbus_tcp_close,
+    _modbus_tcp_flush,
+    _modbus_tcp_select,
+    _modbus_tcp_free
+};
+
+modbus_t* modbus_new_tcp( const char *ip, int port )
+{
+    modbus_t *ctx;
+    modbus_tcp_t *ctx_tcp;
+    size_t dest_size;
+    size_t ret_size;
+
+    ctx = (modbus_t *)malloc(sizeof(modbus_t) );
+    _modbus_init_common(ctx);
+
+    /* Could be changed after to reach a remote serial Modbus device */
+    ctx->slave = MODBUS_TCP_SLAVE;
+
+    ctx->backend = &_modbus_tcp_backend;
+
+    ctx->backend_data = ( modbus_tcp_t *)malloc(sizeof(modbus_tcp_t) );
+    ctx_tcp = (modbus_tcp_t *)ctx->backend_data;
+
+    if (ip != NULL) 
+    {
+        dest_size = sizeof(char) * 16;
+        ret_size = strlcpy(ctx_tcp->ip, ip, dest_size);
+        if (ret_size == 0) 
+        {
+            fprintf(stderr, "The IP string is empty\n");
+            modbus_free(ctx);
+            errno = EINVAL;
+            return NULL;
+        }
+
+        if (ret_size >= dest_size) 
+        {
+            fprintf(stderr, "The IP string has been truncated\n");
+            modbus_free(ctx);
+            errno = EINVAL;
+            return NULL;
+        }
+    } 
+    else 
+    {
+        ctx_tcp->ip[0] = '0';
+    }
+    ctx_tcp->port = port;
+    ctx_tcp->t_id = 0;
+
+    return ctx;
+}
+
+
+modbus_t* modbus_new_tcp_pi( const char *node, const char *service )
+{
+    modbus_t        *ctx;
+    modbus_tcp_pi_t *ctx_tcp_pi;
+    size_t           dest_size;
+    size_t           ret_size;
+
+    ctx = (modbus_t *)malloc(sizeof(modbus_t));
+    _modbus_init_common(ctx);
+
+    /* Could be changed after to reach a remote serial Modbus device */
+    ctx->slave = MODBUS_TCP_SLAVE;
+
+    ctx->backend = &_modbus_tcp_pi_backend;
+
+    ctx->backend_data = (modbus_tcp_pi_t *)malloc(sizeof(modbus_tcp_pi_t));
+    ctx_tcp_pi = (modbus_tcp_pi_t *)ctx->backend_data;
+
+    if (node == NULL) 
+    {
+        /* The node argument can be empty to indicate any hosts */
+        ctx_tcp_pi->node[0] = 0;
+    } 
+    else 
+    {
+        dest_size = sizeof(char) * _MODBUS_TCP_PI_NODE_LENGTH;
+        ret_size = strlcpy(ctx_tcp_pi->node, node, dest_size);
+        if (ret_size == 0) 
+        {
+            fprintf(stderr, "The node string is empty\n");
+            modbus_free(ctx);
+            errno = EINVAL;
+            return NULL;
+        }
+
+        if (ret_size >= dest_size) 
+        {
+            fprintf(stderr, "The node string has been truncated\n");
+            modbus_free(ctx);
+            errno = EINVAL;
+            return NULL;
+        }
+    }
+
+    if (service != NULL) 
+    {
+        dest_size = sizeof(char) * _MODBUS_TCP_PI_SERVICE_LENGTH;
+        ret_size = strlcpy(ctx_tcp_pi->service, service, dest_size);
+    } 
+    else 
+    {
+        /* Empty service is not allowed, error catched below. */
+        ret_size = 0;
+    }
+
+    if (ret_size == 0) 
+    {
+        fprintf(stderr, "The service string is empty\n");
+        modbus_free(ctx);
+        errno = EINVAL;
+        return NULL;
+    }
+
+    if (ret_size >= dest_size) 
+    {
+        fprintf(stderr, "The service string has been truncated\n");
+        modbus_free(ctx);
+        errno = EINVAL;
+        return NULL;
+    }
+
+    ctx_tcp_pi->t_id = 0;
+
+    return ctx;
+}
+#pragma once
+
+#include "modbus.h"
+
+MODBUS_BEGIN_DECLS
+
+#define MODBUS_TCP_DEFAULT_PORT   502
+#define MODBUS_TCP_SLAVE         0xFF
+
+/* Modbus_Application_Protocol_V1_1b.pdf Chapter 4 Section 1 Page 5
+ * TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes
+ */
+#define MODBUS_TCP_MAX_ADU_LENGTH  260
+
+MODBUS_API modbus_t* modbus_new_tcp(const char *ip_address, int port);
+MODBUS_API int modbus_tcp_listen(modbus_t *ctx, int nb_connection);
+MODBUS_API int modbus_tcp_accept(modbus_t *ctx, int *s);
+
+MODBUS_API modbus_t* modbus_new_tcp_pi(const char *node, const char *service);
+MODBUS_API int modbus_tcp_pi_listen(modbus_t *ctx, int nb_connection);
+MODBUS_API int modbus_tcp_pi_accept(modbus_t *ctx, int *s);
+
+MODBUS_END_DECLS
+
+#pragma once 
+
+/* The major version, (1, if %LIBMODBUS_VERSION is 1.2.3) */
+#define LIBMODBUS_VERSION_MAJOR (3)
+
+/* The minor version (2, if %LIBMODBUS_VERSION is 1.2.3) */
+#define LIBMODBUS_VERSION_MINOR (1)
+
+/* The micro version (3, if %LIBMODBUS_VERSION is 1.2.3) */
+#define LIBMODBUS_VERSION_MICRO (4)
+
+/* The full version, like 1.2.3 */
+#define LIBMODBUS_VERSION        3.1.4
+
+/* The full version, in string form (suited for string concatenation)
+ */
+#define LIBMODBUS_VERSION_STRING "3.1.4"
+
+/* Numerically encoded version, like 0x010203 */
+#define LIBMODBUS_VERSION_HEX ((LIBMODBUS_VERSION_MAJOR << 24) |  \
+                               (LIBMODBUS_VERSION_MINOR << 16) |  \
+                               (LIBMODBUS_VERSION_MICRO << 8))
+
+/* Evaluates to True if the version is greater than @major, @minor and @micro
+ */
+#define LIBMODBUS_VERSION_CHECK(major,minor,micro)      \
+    (LIBMODBUS_VERSION_MAJOR > (major) ||               \
+     (LIBMODBUS_VERSION_MAJOR == (major) &&             \
+      LIBMODBUS_VERSION_MINOR > (minor)) ||             \
+     (LIBMODBUS_VERSION_MAJOR == (major) &&             \
+      LIBMODBUS_VERSION_MINOR == (minor) &&             \
+      LIBMODBUS_VERSION_MICRO >= (micro)))
+
+
+#include <config.h>
+#include <errno.h>
+#include <limits.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdarg.h>
+#include <time.h>
+#include <unistd.h>
+
+#include "modbus.h"
+#include "modbus-private.h"
+
+/* Internal use */
+#define MSG_LENGTH_UNDEFINED -1
+
+/* Max between RTU and TCP max adu length (so TCP) */
+#define MAX_MESSAGE_LENGTH 260
+
+/* 3 steps are used to parse the query */
+typedef enum 
+{
+    _STEP_FUNCTION,
+    _STEP_META,
+    _STEP_DATA
+} _step_t;
+
+const char *modbus_strerror( int errnum ) 
+{
+    switch (errnum) 
+    {
+        case EMBXILFUN:
+            return "Illegal function";
+        case EMBXILADD:
+            return "Illegal data address";
+        case EMBXILVAL:
+            return "Illegal data value";
+        case EMBXSFAIL:
+            return "Slave device or server failure";
+        case EMBXACK:
+            return "Acknowledge";
+        case EMBXSBUSY:
+            return "Slave device or server is busy";
+        case EMBXNACK:
+            return "Negative acknowledge";
+        case EMBXMEMPAR:
+            return "Memory parity error";
+        case EMBXGPATH:
+            return "Gateway path unavailable";
+        case EMBXGTAR:
+            return "Target device failed to respond";
+        case EMBBADCRC:
+            return "Invalid CRC";
+        case EMBBADDATA:
+            return "Invalid data";
+        case EMBBADEXC:
+            return "Invalid exception code";
+        case EMBMDATA:
+            return "Too many data";
+        case EMBBADSLAVE:
+            return "Response not from requested slave";
+        default:
+            return strerror(errnum);
+    }
+}
+
+void _error_print(modbus_t *ctx, const char *context)
+{
+    if (ctx->debug) 
+    {
+        fprintf(stderr, "ERROR %s", modbus_strerror(errno));
+        if (context != NULL) 
+        {
+            fprintf(stderr, ": %s\n", context);
+        } 
+        else 
+        {
+            fprintf(stderr, "\n");
+        }
+    }
+}
+
+static void _sleep_response_timeout(modbus_t *ctx)
+{
+    /* Response timeout is always positive */
+    /* usleep source code */
+    struct timespec request, remaining;
+    request.tv_sec = ctx->response_timeout.tv_sec;
+    request.tv_nsec = ((long int)ctx->response_timeout.tv_usec) * 1000;
+    while ( nanosleep( &request, &remaining ) == -1 && errno == EINTR ) 
+    {
+        request = remaining;
+    }
+}
+
+int modbus_flush(modbus_t *ctx)
+{
+    int rc;
+
+    if (ctx == NULL) 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    rc = ctx->backend->flush( ctx );
+    if (rc != -1 && ctx->debug) 
+    {
+        /* Not all backends are able to return the number of bytes flushed */
+        printf("Bytes flushed (%d)\n", rc);
+    }
+    return rc;
+}
+
+/* Computes the length of the expected response */
+static unsigned int compute_response_length_from_request(modbus_t *ctx, uint8_t *req)
+{
+    int length;
+    const int offset = ctx->backend->header_length;
+
+    switch (req[offset]) 
+    {
+        case MODBUS_FC_READ_COILS:
+        case MODBUS_FC_READ_DISCRETE_INPUTS: 
+            /* Header + nb values (code from write_bits) */
+            int nb = (req[offset + 3] << 8) | req[offset + 4];
+            length = 2 + (nb / 8) + ((nb % 8) ? 1 : 0);
+            break;
+        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
+        case MODBUS_FC_READ_HOLDING_REGISTERS:
+        case MODBUS_FC_READ_INPUT_REGISTERS:
+            /* Header + 2 * nb values */
+            length = 2 + 2 * (req[offset + 3] << 8 | req[offset + 4]);
+            break;
+        case MODBUS_FC_READ_EXCEPTION_STATUS:
+            length = 3;
+            break;
+        case MODBUS_FC_REPORT_SLAVE_ID:
+            /* The response is device specific (the header provides the
+               length) */
+            return MSG_LENGTH_UNDEFINED;
+        case MODBUS_FC_MASK_WRITE_REGISTER:
+            length = 7;
+            break;
+        default:
+            length = 5;
+    }
+
+    return offset + length + ctx->backend->checksum_length;
+}
+
+/* Sends a request/response */
+static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
+{
+    int rc;
+    int i;
+
+    msg_length = ctx->backend->send_msg_pre(msg, msg_length);
+
+    if (ctx->debug) 
+    {
+        for (i = 0; i < msg_length; i++)
+        {
+            printf("[%.2X]", msg[i]);
+        }
+        printf("\n");
+    }
+    
+	/* In recovery mode, the write command will be issued until to be
+       successful! Disabled by default. */
+    do 
+    {
+        rc = ctx->backend->send(ctx, msg, msg_length);
+        if (rc == -1) 
+        {
+            _error_print(ctx, NULL);
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) 
+            {
+                int saved_errno = errno;
+
+                if ( (errno == EBADF || errno == ECONNRESET || errno == EPIPE)) 
+                {
+                    modbus_close(ctx);
+                    _sleep_response_timeout(ctx);
+                    modbus_connect(ctx);
+                } 
+                else 
+                {
+                    _sleep_response_timeout(ctx);
+                    modbus_flush(ctx);
+                }
+                errno = saved_errno;
+            }
+        }
+    } while ( (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) && rc == -1);
+
+    if (rc > 0 && rc != msg_length) {
+        errno = EMBBADDATA;
+        return -1;
+    }
+
+    return rc;
+}
+
+int modbus_send_raw_request( modbus_t *ctx, uint8_t *raw_req, int raw_req_length )
+{
+    sft_t sft;
+    uint8_t req[MAX_MESSAGE_LENGTH];
+    int req_length;
+
+    if (ctx == NULL) 
+    {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (raw_req_length < 2 || raw_req_length > (MODBUS_MAX_PDU_LENGTH + 1)) 
+    {
+        /* The raw request must contain function and slave at least and
+           must not be longer than the maximum pdu length plus the slave
+           address. */
+        errno = EINVAL;
+        return -1;
+    }
+
+    sft.slave = raw_req[0];
+    sft.function = raw_req[1];
+    /* The t_id is left to zero */
+    sft.t_id = 0;
+    /* This response function only set the header so it's convenient here */
+    req_length = ctx->backend->build_response_basis(&sft, req);
+
+    if( raw_req_length > 2 ) 
+    {
+        /* Copy data after function code */
+        memcpy(req + req_length, raw_req + 2, raw_req_length - 2);
+        req_length += raw_req_length - 2;
+    }
+
+    return send_msg(ctx, req, req_length);
+}
+
+/*
+ *  ---------- Request     Indication ----------
+ *  | Client | ---------------------->| Server |
+ *  ---------- Confirmation  Response ----------
+ */
+
+/* Computes the length to read after the function received */
+static uint8_t compute_meta_length_after_function( int function, msg_type_t msg_type )
+{
+    int length;
+
+    if (msg_type == MSG_INDICATION) 
+    {
+        if (function <= MODBUS_FC_WRITE_SINGLE_REGISTER) 
+        {
+            length = 4;
+        } 
+        else if (function == MODBUS_FC_WRITE_MULTIPLE_COILS || function == MODBUS_FC_WRITE_MULTIPLE_REGISTERS) 
+        {
+            length = 5;
+        } 
+        else if (function == MODBUS_FC_MASK_WRITE_REGISTER) 
+        {
+            length = 6;
+        } 
+        else if (function == MODBUS_FC_WRITE_AND_READ_REGISTERS) 
+        {
+            length = 9;
+        } 
+        else 
+        {
+            /* MODBUS_FC_READ_EXCEPTION_STATUS, MODBUS_FC_REPORT_SLAVE_ID */
+            length = 0;
+        }
+    } 
+    else 
+    {
+        /* MSG_CONFIRMATION */
+        switch (function) 
+        {
+            case MODBUS_FC_WRITE_SINGLE_COIL:
+            case MODBUS_FC_WRITE_SINGLE_REGISTER:
+            case MODBUS_FC_WRITE_MULTIPLE_COILS:
+            case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
+                length = 4;
+                break;
+            case MODBUS_FC_MASK_WRITE_REGISTER:
+                length = 6;
+                break;
+            default:
+                length = 1;
+        }
+    }
+
+    return length;
+}
+
+/* Computes the length to read after the meta information (address, count, etc) */
+static int compute_data_length_after_meta(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
+{
+    int function = msg[ctx->backend->header_length];
+    int length;
+
+    if (msg_type == MSG_INDICATION) 
+    {
+        switch (function) 
+        {
+            case MODBUS_FC_WRITE_MULTIPLE_COILS:
+            case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
+                length = msg[ctx->backend->header_length + 5];
+                break;
+            case MODBUS_FC_WRITE_AND_READ_REGISTERS:
+                length = msg[ctx->backend->header_length + 9];
+                break;
+            default:
+                length = 0;
+        }
+    } 
+    else 
+    {
+        /* MSG_CONFIRMATION */
+        if (function <= MODBUS_FC_READ_INPUT_REGISTERS ||
+            function == MODBUS_FC_REPORT_SLAVE_ID ||
+            function == MODBUS_FC_WRITE_AND_READ_REGISTERS) 
+        {
+            length = msg[ctx->backend->header_length + 1];
+        } 
+        else 
+        {
+            length = 0;
+        }
+    }
+
+    length += ctx->backend->checksum_length;
+
+    return length;
+}
+
+
+/* Waits a response from a modbus server or a request from a modbus client.
+   This function blocks if there is no replies (3 timeouts).
+
+   The function shall return the number of received characters and the received
+   message in an array of uint8_t if successful. Otherwise it shall return -1
+   and errno is set to one of the values defined below:
+   - ECONNRESET
+   - EMBBADDATA
+   - EMBUNKEXC
+   - ETIMEDOUT
+   - read() or recv() error codes
+*/
+
+int _modbus_receive_msg( modbus_t *ctx, uint8_t *msg, msg_type_t msg_type )
+{
+    int rc;
+    fd_set rset;
+    struct timeval tv;
+    struct timeval *p_tv;
+    int length_to_read;
+    int msg_length = 0;
+    _step_t step;
+
+    if (ctx->debug) 
+    {
+        if (msg_type == MSG_INDICATION) 
+        {
+            printf("Waiting for a indication...\n");
+        } 
+        else 
+        {
+            printf("Waiting for a confirmation...\n");
+        }
+    }
+
+    /* Add a file descriptor to the set */
+    FD_ZERO(&rset);
+    FD_SET(ctx->s, &rset);
+
+    /* We need to analyse the message step by step.  At the first step, we want
+     * to reach the function code because all packets contain this
+     * information. */
+    step = _STEP_FUNCTION;
+    length_to_read = ctx->backend->header_length + 1;
+
+    if (msg_type == MSG_INDICATION) 
+    {
+        /* Wait for a message, we don't know when the message will be
+         * received */
+        p_tv = NULL;
+    } 
+    else 
+    {
+        tv.tv_sec = ctx->response_timeout.tv_sec;
+        tv.tv_usec = ctx->response_timeout.tv_usec;
+        p_tv = &tv;
+    }
+
+    while (length_to_read != 0) 
+    {
+        rc = ctx->backend->select(ctx, &rset, p_tv, length_to_read);
+        if (rc == -1) 
+        {
+            _error_print(ctx, "select");
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) 
+            {
+                int saved_errno = errno;
+
+                if (errno == ETIMEDOUT) 
+                {
+                    _sleep_response_timeout(ctx);
+                    modbus_flush(ctx);
+                } 
+                else if (errno == EBADF) 
+                {
+                    modbus_close(ctx);
+                    modbus_connect(ctx);
+                }
+                errno = saved_errno;
+            }
+            return -1;
+        }
+
+        rc = ctx->backend->recv(ctx, msg + msg_length, length_to_read);
+        if (rc == 0) 
+        {
+            errno = ECONNRESET;
+            rc = -1;
+        }
+
+        if (rc == -1) 
+        {
+            _error_print(ctx, "read");
+            if ((ctx->error_recovery & MODBUS_ERROR_RECOVERY_LINK) &&
+                (errno == ECONNRESET || errno == ECONNREFUSED ||
+                 errno == EBADF)) 
+            {
+                int saved_errno = errno;
+                modbus_close(ctx);
+                modbus_connect(ctx);
+                /* Could be removed by previous calls */
+                errno = saved_errno;
+            }
+            return -1;
+        }
+
+        /* Display the hex code of each character received */
+        if (ctx->debug) 
+        {
+            int i;
+            for (i=0; i < rc; i++)
+                printf("<%.2X>", msg[msg_length + i]);
+        }
+
+        /* Sums bytes received */
+        msg_length += rc;
+        /* Computes remaining bytes */
+        length_to_read -= rc;
+
+        if (length_to_read == 0) 
+        {
+            switch (step) 
+            {
+            case _STEP_FUNCTION:
+                /* Function code position */
+                length_to_read = compute_meta_length_after_function( msg[ctx->backend->header_length], msg_type);
+                if (length_to_read != 0) 
+                {
+                    step = _STEP_META;
+                    break;
+                } /* else switches straight to the next step */
+            case _STEP_META:
+                length_to_read = compute_data_length_after_meta( ctx, msg, msg_type);
+                if ( (msg_length + length_to_read) > (int)ctx->backend->max_adu_length ) 
+                {
+                    errno = EMBBADDATA;
+                    _error_print(ctx, "too many data");
+                    return -1;
+                }
+                step = _STEP_DATA;
+                break;
+            default:
+                break;
+            }
+        }
+
+        if (length_to_read > 0 &&
+            (ctx->byte_timeout.tv_sec > 0 || ctx->byte_timeout.tv_usec > 0)) {
+            /* If there is no character in the buffer, the allowed timeout
+               interval between two consecutive bytes is defined by
+               byte_timeout */
+            tv.tv_sec = ctx->byte_timeout.tv_sec;
+            tv.tv_usec = ctx->byte_timeout.tv_usec;
+            p_tv = &tv;
+        }
+        /* else timeout isn't set again, the full response must be read before
+           expiration of response timeout (for CONFIRMATION only) */
+    }
+
+    if (ctx->debug)
+        printf("\n");
+
+    return ctx->backend->check_integrity(ctx, msg, msg_length);
+}
+
+/* Receive the request from a modbus master */
+int modbus_receive(modbus_t *ctx, uint8_t *req)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return ctx->backend->receive(ctx, req);
+}
+
+/* Receives the confirmation.
+
+   The function shall store the read response in rsp and return the number of
+   values (bits or words). Otherwise, its shall return -1 and errno is set.
+
+   The function doesn't check the confirmation is the expected response to the
+   initial request.
+*/
+int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+}
+
+static int check_confirmation(modbus_t *ctx, uint8_t *req,
+                              uint8_t *rsp, int rsp_length)
+{
+    int rc;
+    int rsp_length_computed;
+    const int offset = ctx->backend->header_length;
+    const int function = rsp[offset];
+
+    if (ctx->backend->pre_check_confirmation) {
+        rc = ctx->backend->pre_check_confirmation(ctx, req, rsp, rsp_length);
+        if (rc == -1) {
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+                _sleep_response_timeout(ctx);
+                modbus_flush(ctx);
+            }
+            return -1;
+        }
+    }
+
+    rsp_length_computed = compute_response_length_from_request(ctx, req);
+
+    /* Exception code */
+    if (function >= 0x80) {
+        if (rsp_length == (offset + 2 + (int)ctx->backend->checksum_length) &&
+            req[offset] == (rsp[offset] - 0x80)) {
+            /* Valid exception code received */
+
+            int exception_code = rsp[offset + 1];
+            if (exception_code < MODBUS_EXCEPTION_MAX) {
+                errno = MODBUS_ENOBASE + exception_code;
+            } else {
+                errno = EMBBADEXC;
+            }
+            _error_print(ctx, NULL);
+            return -1;
+        } else {
+            errno = EMBBADEXC;
+            _error_print(ctx, NULL);
+            return -1;
+        }
+    }
+
+    /* Check length */
+    if ((rsp_length == rsp_length_computed ||
+         rsp_length_computed == MSG_LENGTH_UNDEFINED) &&
+        function < 0x80) {
+        int req_nb_value;
+        int rsp_nb_value;
+
+        /* Check function code */
+        if (function != req[offset]) {
+            if (ctx->debug) {
+                fprintf(stderr,
+                        "Received function not corresponding to the request (0x%X != 0x%X)\n",
+                        function, req[offset]);
+            }
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+                _sleep_response_timeout(ctx);
+                modbus_flush(ctx);
+            }
+            errno = EMBBADDATA;
+            return -1;
+        }
+
+        /* Check the number of values is corresponding to the request */
+        switch (function) {
+        case MODBUS_FC_READ_COILS:
+        case MODBUS_FC_READ_DISCRETE_INPUTS:
+            /* Read functions, 8 values in a byte (nb
+             * of values in the request and byte count in
+             * the response. */
+            req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
+            req_nb_value = (req_nb_value / 8) + ((req_nb_value % 8) ? 1 : 0);
+            rsp_nb_value = rsp[offset + 1];
+            break;
+        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
+        case MODBUS_FC_READ_HOLDING_REGISTERS:
+        case MODBUS_FC_READ_INPUT_REGISTERS:
+            /* Read functions 1 value = 2 bytes */
+            req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
+            rsp_nb_value = (rsp[offset + 1] / 2);
+            break;
+        case MODBUS_FC_WRITE_MULTIPLE_COILS:
+        case MODBUS_FC_WRITE_MULTIPLE_REGISTERS:
+            /* N Write functions */
+            req_nb_value = (req[offset + 3] << 8) + req[offset + 4];
+            rsp_nb_value = (rsp[offset + 3] << 8) | rsp[offset + 4];
+            break;
+        case MODBUS_FC_REPORT_SLAVE_ID:
+            /* Report slave ID (bytes received) */
+            req_nb_value = rsp_nb_value = rsp[offset + 1];
+            break;
+        default:
+            /* 1 Write functions & others */
+            req_nb_value = rsp_nb_value = 1;
+        }
+
+        if (req_nb_value == rsp_nb_value) {
+            rc = rsp_nb_value;
+        } else {
+            if (ctx->debug) {
+                fprintf(stderr,
+                        "Quantity not corresponding to the request (%d != %d)\n",
+                        rsp_nb_value, req_nb_value);
+            }
+
+            if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+                _sleep_response_timeout(ctx);
+                modbus_flush(ctx);
+            }
+
+            errno = EMBBADDATA;
+            rc = -1;
+        }
+    } else {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "Message length not corresponding to the computed length (%d != %d)\n",
+                    rsp_length, rsp_length_computed);
+        }
+        if (ctx->error_recovery & MODBUS_ERROR_RECOVERY_PROTOCOL) {
+            _sleep_response_timeout(ctx);
+            modbus_flush(ctx);
+        }
+        errno = EMBBADDATA;
+        rc = -1;
+    }
+
+    return rc;
+}
+
+static int response_io_status(uint8_t *tab_io_status,
+                              int address, int nb,
+                              uint8_t *rsp, int offset)
+{
+    int shift = 0;
+    /* Instead of byte (not allowed in Win32) */
+    int one_byte = 0;
+    int i;
+
+    for (i = address; i < address + nb; i++) {
+        one_byte |= tab_io_status[i] << shift;
+        if (shift == 7) {
+            /* Byte is full */
+            rsp[offset++] = one_byte;
+            one_byte = shift = 0;
+        } else {
+            shift++;
+        }
+    }
+
+    if (shift != 0)
+        rsp[offset++] = one_byte;
+
+    return offset;
+}
+
+/* Build the exception response */
+static int response_exception(modbus_t *ctx, sft_t *sft,
+                              int exception_code, uint8_t *rsp,
+                              unsigned int to_flush,
+                              const char* template, ...)
+{
+    int rsp_length;
+
+    /* Print debug message */
+    if (ctx->debug) {
+        va_list ap;
+
+        va_start(ap, template);
+        vfprintf(stderr, template, ap);
+        va_end(ap);
+    }
+
+    /* Flush if required */
+    if (to_flush) {
+        _sleep_response_timeout(ctx);
+        modbus_flush(ctx);
+    }
+
+    /* Build exception response */
+    sft->function = sft->function + 0x80;
+    rsp_length = ctx->backend->build_response_basis(sft, rsp);
+    rsp[rsp_length++] = exception_code;
+
+    return rsp_length;
+}
+
+/* Send a response to the received request.
+   Analyses the request and constructs a response.
+
+   If an error occurs, this function construct the response
+   accordingly.
+*/
+int modbus_reply(modbus_t *ctx, const uint8_t *req,
+                 int req_length, modbus_mapping_t *mb_mapping)
+{
+    int offset;
+    int slave;
+    int function;
+    uint16_t address;
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+    int rsp_length = 0;
+    sft_t sft;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    offset = ctx->backend->header_length;
+    slave = req[offset - 1];
+    function = req[offset];
+    address = (req[offset + 1] << 8) + req[offset + 2];
+
+    sft.slave = slave;
+    sft.function = function;
+    sft.t_id = ctx->backend->prepare_response_tid(req, &req_length);
+
+    /* Data are flushed on illegal number of values errors. */
+    switch (function) {
+    case MODBUS_FC_READ_COILS:
+    case MODBUS_FC_READ_DISCRETE_INPUTS: {
+        unsigned int is_input = (function == MODBUS_FC_READ_DISCRETE_INPUTS);
+        int start_bits = is_input ? mb_mapping->start_input_bits : mb_mapping->start_bits;
+        int nb_bits = is_input ? mb_mapping->nb_input_bits : mb_mapping->nb_bits;
+        uint8_t *tab_bits = is_input ? mb_mapping->tab_input_bits : mb_mapping->tab_bits;
+        const char * const name = is_input ? "read_input_bits" : "read_bits";
+        int nb = (req[offset + 3] << 8) + req[offset + 4];
+        /* The mapping can be shifted to reduce memory consumption and it
+           doesn't always start at address zero. */
+        int mapping_address = address - start_bits;
+
+        if (nb < 1 || MODBUS_MAX_READ_BITS < nb) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
+                "Illegal nb of values %d in %s (max %d)\n",
+                nb, name, MODBUS_MAX_READ_BITS);
+        } else if (mapping_address < 0 || (mapping_address + nb) > nb_bits) {
+            rsp_length = response_exception(
+                ctx, &sft,
+                MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in %s\n",
+                mapping_address < 0 ? address : address + nb, name);
+        } else {
+            rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+            rsp[rsp_length++] = (nb / 8) + ((nb % 8) ? 1 : 0);
+            rsp_length = response_io_status(tab_bits, mapping_address, nb,
+                                            rsp, rsp_length);
+        }
+    }
+        break;
+    case MODBUS_FC_READ_HOLDING_REGISTERS:
+    case MODBUS_FC_READ_INPUT_REGISTERS: {
+        unsigned int is_input = (function == MODBUS_FC_READ_INPUT_REGISTERS);
+        int start_registers = is_input ? mb_mapping->start_input_registers : mb_mapping->start_registers;
+        int nb_registers = is_input ? mb_mapping->nb_input_registers : mb_mapping->nb_registers;
+        uint16_t *tab_registers = is_input ? mb_mapping->tab_input_registers : mb_mapping->tab_registers;
+        const char * const name = is_input ? "read_input_registers" : "read_registers";
+        int nb = (req[offset + 3] << 8) + req[offset + 4];
+        /* The mapping can be shifted to reduce memory consumption and it
+           doesn't always start at address zero. */
+        int mapping_address = address - start_registers;
+
+        if (nb < 1 || MODBUS_MAX_READ_REGISTERS < nb) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
+                "Illegal nb of values %d in %s (max %d)\n",
+                nb, name, MODBUS_MAX_READ_REGISTERS);
+        } else if (mapping_address < 0 || (mapping_address + nb) > nb_registers) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in %s\n",
+                mapping_address < 0 ? address : address + nb, name);
+        } else {
+            int i;
+
+            rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+            rsp[rsp_length++] = nb << 1;
+            for (i = mapping_address; i < mapping_address + nb; i++) {
+                rsp[rsp_length++] = tab_registers[i] >> 8;
+                rsp[rsp_length++] = tab_registers[i] & 0xFF;
+            }
+        }
+    }
+        break;
+    case MODBUS_FC_WRITE_SINGLE_COIL: {
+        int mapping_address = address - mb_mapping->start_bits;
+
+        if (mapping_address < 0 || mapping_address >= mb_mapping->nb_bits) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in write_bit\n",
+                address);
+        } else {
+            int data = (req[offset + 3] << 8) + req[offset + 4];
+
+            if (data == 0xFF00 || data == 0x0) {
+                mb_mapping->tab_bits[mapping_address] = data ? ON : OFF;
+                memcpy(rsp, req, req_length);
+                rsp_length = req_length;
+            } else {
+                rsp_length = response_exception(
+                    ctx, &sft,
+                    MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, FALSE,
+                    "Illegal data value 0x%0X in write_bit request at address %0X\n",
+                    data, address);
+            }
+        }
+    }
+        break;
+    case MODBUS_FC_WRITE_SINGLE_REGISTER: {
+        int mapping_address = address - mb_mapping->start_registers;
+
+        if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
+            rsp_length = response_exception(
+                ctx, &sft,
+                MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in write_register\n",
+                address);
+        } else {
+            int data = (req[offset + 3] << 8) + req[offset + 4];
+
+            mb_mapping->tab_registers[mapping_address] = data;
+            memcpy(rsp, req, req_length);
+            rsp_length = req_length;
+        }
+    }
+        break;
+    case MODBUS_FC_WRITE_MULTIPLE_COILS: {
+        int nb = (req[offset + 3] << 8) + req[offset + 4];
+        int mapping_address = address - mb_mapping->start_bits;
+
+        if (nb < 1 || MODBUS_MAX_WRITE_BITS < nb) {
+            /* May be the indication has been truncated on reading because of
+             * invalid address (eg. nb is 0 but the request contains values to
+             * write) so it's necessary to flush. */
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
+                "Illegal number of values %d in write_bits (max %d)\n",
+                nb, MODBUS_MAX_WRITE_BITS);
+        } else if (mapping_address < 0 ||
+                   (mapping_address + nb) > mb_mapping->nb_bits) {
+            rsp_length = response_exception(
+                ctx, &sft,
+                MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in write_bits\n",
+                mapping_address < 0 ? address : address + nb);
+        } else {
+            /* 6 = byte count */
+            modbus_set_bits_from_bytes(mb_mapping->tab_bits, mapping_address, nb,
+                                       &req[offset + 6]);
+
+            rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+            /* 4 to copy the bit address (2) and the quantity of bits */
+            memcpy(rsp + rsp_length, req + rsp_length, 4);
+            rsp_length += 4;
+        }
+    }
+        break;
+    case MODBUS_FC_WRITE_MULTIPLE_REGISTERS: {
+        int nb = (req[offset + 3] << 8) + req[offset + 4];
+        int mapping_address = address - mb_mapping->start_registers;
+
+        if (nb < 1 || MODBUS_MAX_WRITE_REGISTERS < nb) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
+                "Illegal number of values %d in write_registers (max %d)\n",
+                nb, MODBUS_MAX_WRITE_REGISTERS);
+        } else if (mapping_address < 0 ||
+                   (mapping_address + nb) > mb_mapping->nb_registers) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in write_registers\n",
+                mapping_address < 0 ? address : address + nb);
+        } else {
+            int i, j;
+            for (i = mapping_address, j = 6; i < mapping_address + nb; i++, j += 2) {
+                /* 6 and 7 = first value */
+                mb_mapping->tab_registers[i] =
+                    (req[offset + j] << 8) + req[offset + j + 1];
+            }
+
+            rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+            /* 4 to copy the address (2) and the no. of registers */
+            memcpy(rsp + rsp_length, req + rsp_length, 4);
+            rsp_length += 4;
+        }
+    }
+        break;
+    case MODBUS_FC_REPORT_SLAVE_ID: 
+    {
+        int str_len;
+        int byte_count_pos;
+
+        rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+        /* Skip byte count for now */
+        byte_count_pos = rsp_length++;
+        rsp[rsp_length++] = _REPORT_SLAVE_ID;
+        /* Run indicator status to ON */
+        rsp[rsp_length++] = 0xFF;
+        str_len = 3 + strlen(MODBUS_VERSION_STRING);
+        memcpy(rsp + rsp_length, "LMB" MODBUS_VERSION_STRING, str_len);
+        rsp_length += str_len;
+        rsp[byte_count_pos] = rsp_length - byte_count_pos - 1;
+    }
+        break;
+    case MODBUS_FC_READ_EXCEPTION_STATUS:
+        if (ctx->debug) {
+            fprintf(stderr, "FIXME Not implemented\n");
+        }
+        errno = ENOPROTOOPT;
+        return -1;
+        break;
+    case MODBUS_FC_MASK_WRITE_REGISTER: {
+        int mapping_address = address - mb_mapping->start_registers;
+
+        if (mapping_address < 0 || mapping_address >= mb_mapping->nb_registers) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data address 0x%0X in write_register\n",
+                address);
+        } else {
+            uint16_t data = mb_mapping->tab_registers[mapping_address];
+            uint16_t and = (req[offset + 3] << 8) + req[offset + 4];
+            uint16_t or = (req[offset + 5] << 8) + req[offset + 6];
+
+            data = (data & and) | (or & (~and));
+            mb_mapping->tab_registers[mapping_address] = data;
+            memcpy(rsp, req, req_length);
+            rsp_length = req_length;
+        }
+    }
+        break;
+    case MODBUS_FC_WRITE_AND_READ_REGISTERS: {
+        int nb = (req[offset + 3] << 8) + req[offset + 4];
+        uint16_t address_write = (req[offset + 5] << 8) + req[offset + 6];
+        int nb_write = (req[offset + 7] << 8) + req[offset + 8];
+        int nb_write_bytes = req[offset + 9];
+        int mapping_address = address - mb_mapping->start_registers;
+        int mapping_address_write = address_write - mb_mapping->start_registers;
+
+        if (nb_write < 1 || MODBUS_MAX_WR_WRITE_REGISTERS < nb_write ||
+            nb < 1 || MODBUS_MAX_WR_READ_REGISTERS < nb ||
+            nb_write_bytes != nb_write * 2) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE,
+                "Illegal nb of values (W%d, R%d) in write_and_read_registers (max W%d, R%d)\n",
+                nb_write, nb, MODBUS_MAX_WR_WRITE_REGISTERS, MODBUS_MAX_WR_READ_REGISTERS);
+        } else if (mapping_address < 0 ||
+                   (mapping_address + nb) > mb_mapping->nb_registers ||
+                   mapping_address < 0 ||
+                   (mapping_address_write + nb_write) > mb_mapping->nb_registers) {
+            rsp_length = response_exception(
+                ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE,
+                "Illegal data read address 0x%0X or write address 0x%0X write_and_read_registers\n",
+                mapping_address < 0 ? address : address + nb,
+                mapping_address_write < 0 ? address_write : address_write + nb_write);
+        } else {
+            int i, j;
+            rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+            rsp[rsp_length++] = nb << 1;
+
+            /* Write first.
+               10 and 11 are the offset of the first values to write */
+            for (i = mapping_address_write, j = 10;
+                 i < mapping_address_write + nb_write; i++, j += 2) {
+                mb_mapping->tab_registers[i] =
+                    (req[offset + j] << 8) + req[offset + j + 1];
+            }
+
+            /* and read the data for the response */
+            for (i = mapping_address; i < mapping_address + nb; i++) {
+                rsp[rsp_length++] = mb_mapping->tab_registers[i] >> 8;
+                rsp[rsp_length++] = mb_mapping->tab_registers[i] & 0xFF;
+            }
+        }
+    }
+        break;
+
+    default:
+        rsp_length = response_exception(
+            ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, TRUE,
+            "Unknown Modbus function code: 0x%0X\n", function);
+        break;
+    }
+
+    /* Suppress any responses when the request was a broadcast */
+    return (slave == MODBUS_BROADCAST_ADDRESS) ? 0 : send_msg(ctx, rsp, rsp_length);
+}
+
+int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,
+                           unsigned int exception_code)
+{
+    int offset;
+    int slave;
+    int function;
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+    int rsp_length;
+    int dummy_length = 99;
+    sft_t sft;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    offset = ctx->backend->header_length;
+    slave = req[offset - 1];
+    function = req[offset];
+
+    sft.slave = slave;
+    sft.function = function + 0x80;;
+    sft.t_id = ctx->backend->prepare_response_tid(req, &dummy_length);
+    rsp_length = ctx->backend->build_response_basis(&sft, rsp);
+
+    /* Positive exception code */
+    if (exception_code < MODBUS_EXCEPTION_MAX) {
+        rsp[rsp_length++] = exception_code;
+        return send_msg(ctx, rsp, rsp_length);
+    } else {
+        errno = EINVAL;
+        return -1;
+    }
+}
+
+/* Reads IO status */
+static int read_io_status(modbus_t *ctx, int function,
+                          int addr, int nb, uint8_t *dest)
+{
+    int rc;
+    int req_length;
+
+    uint8_t req[_MIN_REQ_LENGTH];
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+    req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        int i, temp, bit;
+        int pos = 0;
+        int offset;
+        int offset_end;
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+        if (rc == -1)
+            return -1;
+
+        offset = ctx->backend->header_length + 2;
+        offset_end = offset + rc;
+        for (i = offset; i < offset_end; i++) {
+            /* Shift reg hi_byte to temp */
+            temp = rsp[i];
+
+            for (bit = 0x01; (bit & 0xff) && (pos < nb);) {
+                dest[pos++] = (temp & bit) ? TRUE : FALSE;
+                bit = bit << 1;
+            }
+
+        }
+    }
+
+    return rc;
+}
+
+/* Reads the boolean status of bits and sets the array elements
+   in the destination to TRUE or FALSE (single bits). */
+int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
+{
+    int rc;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_BITS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Too many bits requested (%d > %d)\n",
+                    nb, MODBUS_MAX_READ_BITS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    rc = read_io_status(ctx, MODBUS_FC_READ_COILS, addr, nb, dest);
+
+    if (rc == -1)
+        return -1;
+    else
+        return nb;
+}
+
+
+/* Same as modbus_read_bits but reads the remote device input table */
+int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest)
+{
+    int rc;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_BITS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Too many discrete inputs requested (%d > %d)\n",
+                    nb, MODBUS_MAX_READ_BITS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    rc = read_io_status(ctx, MODBUS_FC_READ_DISCRETE_INPUTS, addr, nb, dest);
+
+    if (rc == -1)
+        return -1;
+    else
+        return nb;
+}
+
+/* Reads the data from a remove device and put that data into an array */
+static int read_registers(modbus_t *ctx, int function, int addr, int nb, uint16_t *dest)
+{
+    int rc;
+    int req_length;
+    uint8_t req[_MIN_REQ_LENGTH];
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+    if (nb > MODBUS_MAX_READ_REGISTERS)
+    {
+        if (ctx->debug)
+        {
+            fprintf(stderr,
+                    "ERROR Too many registers requested (%d > %d)\n",
+                    nb, MODBUS_MAX_READ_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    req_length = ctx->backend->build_request_basis(ctx, function, addr, nb, req);
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0)
+    {
+        int offset;
+        int i;
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+        if (rc == -1)
+            return -1;
+
+        offset = ctx->backend->header_length;
+
+        for (i = 0; i < rc; i++) {
+            /* shift reg hi_byte to temp OR with lo_byte */
+            dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
+                rsp[offset + 3 + (i << 1)];
+        }
+    }
+
+    return rc;
+}
+
+/* Reads the holding registers of remote device and put the data into an
+   array */
+int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest)
+{
+    int status;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_REGISTERS)
+    {
+        if (ctx->debug)
+        {
+            fprintf(stderr,
+                    "ERROR Too many registers requested (%d > %d)\n",
+                    nb, MODBUS_MAX_READ_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    status = read_registers(ctx, MODBUS_FC_READ_HOLDING_REGISTERS,
+                            addr, nb, dest);
+    return status;
+}
+
+/* Reads the input registers of remote device and put the data into an array */
+int modbus_read_input_registers(modbus_t *ctx, int addr, int nb,
+                                uint16_t *dest)
+{
+    int status;
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_REGISTERS) {
+        fprintf(stderr,
+                "ERROR Too many input registers requested (%d > %d)\n",
+                nb, MODBUS_MAX_READ_REGISTERS);
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    status = read_registers(ctx, MODBUS_FC_READ_INPUT_REGISTERS,
+                            addr, nb, dest);
+
+    return status;
+}
+
+/* Write a value to the specified register of the remote device.
+   Used by write_bit and write_register */
+static int write_single(modbus_t *ctx, int function, int addr, int value)
+{
+    int rc;
+    int req_length;
+    uint8_t req[_MIN_REQ_LENGTH];
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    req_length = ctx->backend->build_request_basis(ctx, function, addr, value, req);
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        /* Used by write_bit and write_register */
+        uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+    }
+
+    return rc;
+}
+
+/* Turns ON or OFF a single bit of the remote device */
+int modbus_write_bit(modbus_t *ctx, int addr, int status)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return write_single(ctx, MODBUS_FC_WRITE_SINGLE_COIL, addr,
+                        status ? 0xFF00 : 0);
+}
+
+/* Writes a value in one register of the remote device */
+int modbus_write_register(modbus_t *ctx, int addr, int value)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return write_single(ctx, MODBUS_FC_WRITE_SINGLE_REGISTER, addr, value);
+}
+
+/* Write the bits of the array in the remote device */
+int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *src)
+{
+    int rc;
+    int i;
+    int byte_count;
+    int req_length;
+    int bit_check = 0;
+    int pos = 0;
+    uint8_t req[MAX_MESSAGE_LENGTH];
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_WRITE_BITS) {
+        if (ctx->debug) {
+            fprintf(stderr, "ERROR Writing too many bits (%d > %d)\n",
+                    nb, MODBUS_MAX_WRITE_BITS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    req_length = ctx->backend->build_request_basis(ctx,
+                                                   MODBUS_FC_WRITE_MULTIPLE_COILS,
+                                                   addr, nb, req);
+    byte_count = (nb / 8) + ((nb % 8) ? 1 : 0);
+    req[req_length++] = byte_count;
+
+    for (i = 0; i < byte_count; i++) {
+        int bit;
+
+        bit = 0x01;
+        req[req_length] = 0;
+
+        while ((bit & 0xFF) && (bit_check++ < nb)) {
+            if (src[pos++])
+                req[req_length] |= bit;
+            else
+                req[req_length] &=~ bit;
+
+            bit = bit << 1;
+        }
+        req_length++;
+    }
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+    }
+
+
+    return rc;
+}
+
+/* Write the values from the array to the registers of the remote device */
+int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *src)
+{
+    int rc;
+    int i;
+    int req_length;
+    int byte_count;
+    uint8_t req[MAX_MESSAGE_LENGTH];
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_WRITE_REGISTERS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Trying to write to too many registers (%d > %d)\n",
+                    nb, MODBUS_MAX_WRITE_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    req_length = ctx->backend->build_request_basis(ctx,
+                                                   MODBUS_FC_WRITE_MULTIPLE_REGISTERS,
+                                                   addr, nb, req);
+    byte_count = nb * 2;
+    req[req_length++] = byte_count;
+
+    for (i = 0; i < nb; i++) {
+        req[req_length++] = src[i] >> 8;
+        req[req_length++] = src[i] & 0x00FF;
+    }
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+    }
+
+    return rc;
+}
+
+int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask)
+{
+    int rc;
+    int req_length;
+    /* The request length can not exceed _MIN_REQ_LENGTH - 2 and 4 bytes to
+     * store the masks. The ugly substraction is there to remove the 'nb' value
+     * (2 bytes) which is not used. */
+    uint8_t req[_MIN_REQ_LENGTH + 2];
+
+    req_length = ctx->backend->build_request_basis(ctx,
+                                                   MODBUS_FC_MASK_WRITE_REGISTER,
+                                                   addr, 0, req);
+
+    /* HACKISH, count is not used */
+    req_length -= 2;
+
+    req[req_length++] = and_mask >> 8;
+    req[req_length++] = and_mask & 0x00ff;
+    req[req_length++] = or_mask >> 8;
+    req[req_length++] = or_mask & 0x00ff;
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        /* Used by write_bit and write_register */
+        uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+    }
+
+    return rc;
+}
+
+/* Write multiple registers from src array to remote device and read multiple
+   registers from remote device to dest array. */
+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)
+
+{
+    int rc;
+    int req_length;
+    int i;
+    int byte_count;
+    uint8_t req[MAX_MESSAGE_LENGTH];
+    uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    if (write_nb > MODBUS_MAX_WR_WRITE_REGISTERS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Too many registers to write (%d > %d)\n",
+                    write_nb, MODBUS_MAX_WR_WRITE_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+
+    if (read_nb > MODBUS_MAX_WR_READ_REGISTERS) {
+        if (ctx->debug) {
+            fprintf(stderr,
+                    "ERROR Too many registers requested (%d > %d)\n",
+                    read_nb, MODBUS_MAX_WR_READ_REGISTERS);
+        }
+        errno = EMBMDATA;
+        return -1;
+    }
+    req_length = ctx->backend->build_request_basis(ctx,
+                                                   MODBUS_FC_WRITE_AND_READ_REGISTERS,
+                                                   read_addr, read_nb, req);
+
+    req[req_length++] = write_addr >> 8;
+    req[req_length++] = write_addr & 0x00ff;
+    req[req_length++] = write_nb >> 8;
+    req[req_length++] = write_nb & 0x00ff;
+    byte_count = write_nb * 2;
+    req[req_length++] = byte_count;
+
+    for (i = 0; i < write_nb; i++) {
+        req[req_length++] = src[i] >> 8;
+        req[req_length++] = src[i] & 0x00FF;
+    }
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        int offset;
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+        if (rc == -1)
+            return -1;
+
+        offset = ctx->backend->header_length;
+        for (i = 0; i < rc; i++) {
+            /* shift reg hi_byte to temp OR with lo_byte */
+            dest[i] = (rsp[offset + 2 + (i << 1)] << 8) |
+                rsp[offset + 3 + (i << 1)];
+        }
+    }
+
+    return rc;
+}
+
+/* Send a request to get the slave ID of the device (only available in serial
+   communication). */
+int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest)
+{
+    int rc;
+    int req_length;
+    uint8_t req[_MIN_REQ_LENGTH];
+
+    if (ctx == NULL || max_dest <= 0) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    req_length = ctx->backend->build_request_basis(ctx, MODBUS_FC_REPORT_SLAVE_ID,
+                                                   0, 0, req);
+
+    /* HACKISH, addr and count are not used */
+    req_length -= 4;
+
+    rc = send_msg(ctx, req, req_length);
+    if (rc > 0) {
+        int i;
+        int offset;
+        uint8_t rsp[MAX_MESSAGE_LENGTH];
+
+        rc = _modbus_receive_msg(ctx, rsp, MSG_CONFIRMATION);
+        if (rc == -1)
+            return -1;
+
+        rc = check_confirmation(ctx, req, rsp, rc);
+        if (rc == -1)
+            return -1;
+
+        offset = ctx->backend->header_length + 2;
+
+        /* Byte count, slave id, run indicator status and
+           additional data. Truncate copy to max_dest. */
+        for (i=0; i < rc && i < max_dest; i++) {
+            dest[i] = rsp[offset + i];
+        }
+    }
+
+    return rc;
+}
+
+void _modbus_init_common(modbus_t *ctx)
+{
+    /* Slave and socket are initialized to -1 */
+    ctx->slave = -1;
+    ctx->s = -1;
+
+    ctx->debug = FALSE;
+    ctx->error_recovery = MODBUS_ERROR_RECOVERY_NONE;
+
+    ctx->response_timeout.tv_sec = 0;
+    ctx->response_timeout.tv_usec = _RESPONSE_TIMEOUT;
+
+    ctx->byte_timeout.tv_sec = 0;
+    ctx->byte_timeout.tv_usec = _BYTE_TIMEOUT;
+}
+
+/* Define the slave number */
+int modbus_set_slave(modbus_t *ctx, int slave)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return ctx->backend->set_slave(ctx, slave);
+}
+
+int modbus_set_error_recovery(modbus_t *ctx,
+                              modbus_error_recovery_mode error_recovery)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    /* The type of modbus_error_recovery_mode is unsigned enum */
+    ctx->error_recovery = (uint8_t) error_recovery;
+    return 0;
+}
+
+int modbus_set_socket(modbus_t *ctx, int s)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx->s = s;
+    return 0;
+}
+
+int modbus_get_socket(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return ctx->s;
+}
+
+/* Get the timeout interval used to wait for a response */
+int modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *to_sec = ctx->response_timeout.tv_sec;
+    *to_usec = ctx->response_timeout.tv_usec;
+    return 0;
+}
+
+int modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
+{
+    if (ctx == NULL ||
+        (to_sec == 0 && to_usec == 0) || to_usec > 999999) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx->response_timeout.tv_sec = to_sec;
+    ctx->response_timeout.tv_usec = to_usec;
+    return 0;
+}
+
+/* Get the timeout interval between two consecutive bytes of a message */
+int modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    *to_sec = ctx->byte_timeout.tv_sec;
+    *to_usec = ctx->byte_timeout.tv_usec;
+    return 0;
+}
+
+int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec)
+{
+    /* Byte timeout can be disabled when both values are zero */
+    if (ctx == NULL || to_usec > 999999) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx->byte_timeout.tv_sec = to_sec;
+    ctx->byte_timeout.tv_usec = to_usec;
+    return 0;
+}
+
+int modbus_get_header_length(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return ctx->backend->header_length;
+}
+
+int modbus_connect(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    return ctx->backend->connect(ctx);
+}
+
+void modbus_close(modbus_t *ctx)
+{
+    if (ctx == NULL)
+        return;
+
+    ctx->backend->close(ctx);
+}
+
+void modbus_free(modbus_t *ctx)
+{
+    if (ctx == NULL)
+        return;
+
+    ctx->backend->free(ctx);
+}
+
+int modbus_set_debug(modbus_t *ctx, int flag)
+{
+    if (ctx == NULL) {
+        errno = EINVAL;
+        return -1;
+    }
+
+    ctx->debug = flag;
+    return 0;
+}
+
+/* Allocates 4 arrays to store bits, input bits, registers and inputs
+   registers. The pointers are stored in modbus_mapping structure.
+
+   The modbus_mapping_new_ranges() function shall return the new allocated
+   structure if successful. Otherwise it shall return NULL and set errno to
+   ENOMEM. */
+modbus_mapping_t* modbus_mapping_new_start_address(
+    unsigned int start_bits, unsigned int nb_bits,
+    unsigned int start_input_bits, unsigned int nb_input_bits,
+    unsigned int start_registers, unsigned int nb_registers,
+    unsigned int start_input_registers, unsigned int nb_input_registers)
+{
+    modbus_mapping_t *mb_mapping;
+
+    mb_mapping = (modbus_mapping_t *)malloc(sizeof(modbus_mapping_t));
+    if (mb_mapping == NULL) {
+        return NULL;
+    }
+
+    /* 0X */
+    mb_mapping->nb_bits = nb_bits;
+    mb_mapping->start_bits = start_bits;
+    if (nb_bits == 0) {
+        mb_mapping->tab_bits = NULL;
+    } else {
+        /* Negative number raises a POSIX error */
+        mb_mapping->tab_bits =
+            (uint8_t *) malloc(nb_bits * sizeof(uint8_t));
+        if (mb_mapping->tab_bits == NULL) {
+            free(mb_mapping);
+            return NULL;
+        }
+        memset(mb_mapping->tab_bits, 0, nb_bits * sizeof(uint8_t));
+    }
+
+    /* 1X */
+    mb_mapping->nb_input_bits = nb_input_bits;
+    mb_mapping->start_input_bits = start_input_bits;
+    if (nb_input_bits == 0) {
+        mb_mapping->tab_input_bits = NULL;
+    } else {
+        mb_mapping->tab_input_bits =
+            (uint8_t *) malloc(nb_input_bits * sizeof(uint8_t));
+        if (mb_mapping->tab_input_bits == NULL) {
+            free(mb_mapping->tab_bits);
+            free(mb_mapping);
+            return NULL;
+        }
+        memset(mb_mapping->tab_input_bits, 0, nb_input_bits * sizeof(uint8_t));
+    }
+
+    /* 4X */
+    mb_mapping->nb_registers = nb_registers;
+    mb_mapping->start_registers = start_registers;
+    if (nb_registers == 0) {
+        mb_mapping->tab_registers = NULL;
+    } else {
+        mb_mapping->tab_registers =
+            (uint16_t *) malloc(nb_registers * sizeof(uint16_t));
+        if (mb_mapping->tab_registers == NULL) {
+            free(mb_mapping->tab_input_bits);
+            free(mb_mapping->tab_bits);
+            free(mb_mapping);
+            return NULL;
+        }
+        memset(mb_mapping->tab_registers, 0, nb_registers * sizeof(uint16_t));
+    }
+
+    /* 3X */
+    mb_mapping->nb_input_registers = nb_input_registers;
+    mb_mapping->start_input_registers = start_input_registers;
+    if (nb_input_registers == 0) {
+        mb_mapping->tab_input_registers = NULL;
+    } else {
+        mb_mapping->tab_input_registers =
+            (uint16_t *) malloc(nb_input_registers * sizeof(uint16_t));
+        if (mb_mapping->tab_input_registers == NULL) {
+            free(mb_mapping->tab_registers);
+            free(mb_mapping->tab_input_bits);
+            free(mb_mapping->tab_bits);
+            free(mb_mapping);
+            return NULL;
+        }
+        memset(mb_mapping->tab_input_registers, 0,
+               nb_input_registers * sizeof(uint16_t));
+    }
+
+    return mb_mapping;
+}
+
+modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits,
+                                     int nb_registers, int nb_input_registers)
+{
+    return modbus_mapping_new_start_address(
+        0, nb_bits, 0, nb_input_bits, 0, nb_registers, 0, nb_input_registers);
+}
+
+/* Frees the 4 arrays */
+void modbus_mapping_free(modbus_mapping_t *mb_mapping)
+{
+    if (mb_mapping == NULL) {
+        return;
+    }
+
+    free(mb_mapping->tab_input_registers);
+    free(mb_mapping->tab_registers);
+    free(mb_mapping->tab_input_bits);
+    free(mb_mapping->tab_bits);
+    free(mb_mapping);
+}
+
+#ifndef HAVE_STRLCPY
+/*
+ * Function strlcpy was originally developed by
+ * Todd C. Miller <Todd.Miller@courtesan.com> to simplify writing secure code.
+ * See ftp://ftp.openbsd.org/pub/OpenBSD/src/lib/libc/string/strlcpy.3
+ * for more information.
+ *
+ * Thank you Ulrich Drepper... not!
+ *
+ * Copy src to string dest of size dest_size.  At most dest_size-1 characters
+ * will be copied.  Always NUL terminates (unless dest_size == 0).  Returns
+ * strlen(src); if retval >= dest_size, truncation occurred.
+ */
+size_t strlcpy(char *dest, const char *src, size_t dest_size)
+{
+    register char *d = dest;
+    register const char *s = src;
+    register size_t n = dest_size;
+
+    /* Copy as many bytes as will fit */
+    if (n != 0 && --n != 0) {
+        do {
+            if ((*d++ = *s++) == 0)
+                break;
+        } while (--n != 0);
+    }
+
+    /* Not enough room in dest, add NUL and traverse rest of src */
+    if (n == 0) {
+        if (dest_size != 0)
+            *d = '\0'; /* NUL-terminate dest */
+        while (*s++)
+            ;
+    }
+
+    return (s - src - 1); /* count does not include NUL */
+}
+#endif
+
+#pragma once
+
+/* Add this for macros that defined unix flavor */
+#if (defined(__unix__) || defined(unix)) && !defined(USG)
+#include <sys/param.h>
+#endif
+
+#include <stdint.h>
+
+#define MODBUS_API
+
+#define MODBUS_VERSION_STRING "1.0.0"
+
+#ifdef  __cplusplus
+#define MODBUS_BEGIN_DECLS  extern "C" {
+#define MODBUS_END_DECLS }
+#else
+# define MODBUS_BEGIN_DECLS
+# define MODBUS_END_DECLS
+#endif
+
+MODBUS_BEGIN_DECLS
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef OFF
+#define OFF 0
+#endif
+
+#ifndef ON
+#define ON 1
+#endif
+
+/* Modbus function codes */
+#define MODBUS_FC_READ_COILS                0x01
+#define MODBUS_FC_READ_DISCRETE_INPUTS      0x02
+#define MODBUS_FC_READ_HOLDING_REGISTERS    0x03
+#define MODBUS_FC_READ_INPUT_REGISTERS      0x04
+#define MODBUS_FC_WRITE_SINGLE_COIL         0x05
+#define MODBUS_FC_WRITE_SINGLE_REGISTER     0x06
+#define MODBUS_FC_READ_EXCEPTION_STATUS     0x07
+#define MODBUS_FC_WRITE_MULTIPLE_COILS      0x0F
+#define MODBUS_FC_WRITE_MULTIPLE_REGISTERS  0x10
+#define MODBUS_FC_REPORT_SLAVE_ID           0x11
+#define MODBUS_FC_MASK_WRITE_REGISTER       0x16
+#define MODBUS_FC_WRITE_AND_READ_REGISTERS  0x17
+
+#define MODBUS_BROADCAST_ADDRESS    0
+
+/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 1 page 12)
+ * Quantity of Coils to read (2 bytes): 1 to 2000 (0x7D0)
+ * (chapter 6 section 11 page 29)
+ * Quantity of Coils to write (2 bytes): 1 to 1968 (0x7B0)
+ */
+#define MODBUS_MAX_READ_BITS              2000
+#define MODBUS_MAX_WRITE_BITS             1968
+
+/* Modbus_Application_Protocol_V1_1b.pdf (chapter 6 section 3 page 15)
+ * Quantity of Registers to read (2 bytes): 1 to 125 (0x7D)
+ * (chapter 6 section 12 page 31)
+ * Quantity of Registers to write (2 bytes) 1 to 123 (0x7B)
+ * (chapter 6 section 17 page 38)
+ * Quantity of Registers to write in R/W registers (2 bytes) 1 to 121 (0x79)
+ */
+#define MODBUS_MAX_READ_REGISTERS          125
+#define MODBUS_MAX_WRITE_REGISTERS         123
+#define MODBUS_MAX_WR_WRITE_REGISTERS      121
+#define MODBUS_MAX_WR_READ_REGISTERS       125
+
+/* The size of the MODBUS PDU is limited by the size constraint inherited from
+ * the first MODBUS implementation on Serial Line network (max. RS485 ADU = 256
+ * bytes). Therefore, MODBUS PDU for serial line communication = 256 - Server
+ * address (1 byte) - CRC (2 bytes) = 253 bytes.
+ */
+#define MODBUS_MAX_PDU_LENGTH              253
+
+/* Consequently:
+ * - RTU MODBUS ADU = 253 bytes + Server address (1 byte) + CRC (2 bytes) = 256
+ *   bytes.
+ * - TCP MODBUS ADU = 253 bytes + MBAP (7 bytes) = 260 bytes.
+ * so the maximum of both backend in 260 bytes. This size can used to allocate
+ * an array of bytes to store responses and it will be compatible with the two
+ * backends.
+ */
+#define MODBUS_MAX_ADU_LENGTH              260
+
+/* Random number to avoid errno conflicts */
+#define MODBUS_ENOBASE 112345678
+
+/* Protocol exceptions */
+enum {
+    MODBUS_EXCEPTION_ILLEGAL_FUNCTION = 0x01,
+    MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS,
+    MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE,
+    MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE,
+    MODBUS_EXCEPTION_ACKNOWLEDGE,
+    MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY,
+    MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE,
+    MODBUS_EXCEPTION_MEMORY_PARITY,
+    MODBUS_EXCEPTION_NOT_DEFINED,
+    MODBUS_EXCEPTION_GATEWAY_PATH,
+    MODBUS_EXCEPTION_GATEWAY_TARGET,
+    MODBUS_EXCEPTION_MAX
+};
+
+#define EMBXILFUN  (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_FUNCTION)
+#define EMBXILADD  (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS)
+#define EMBXILVAL  (MODBUS_ENOBASE + MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE)
+#define EMBXSFAIL  (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_FAILURE)
+#define EMBXACK    (MODBUS_ENOBASE + MODBUS_EXCEPTION_ACKNOWLEDGE)
+#define EMBXSBUSY  (MODBUS_ENOBASE + MODBUS_EXCEPTION_SLAVE_OR_SERVER_BUSY)
+#define EMBXNACK   (MODBUS_ENOBASE + MODBUS_EXCEPTION_NEGATIVE_ACKNOWLEDGE)
+#define EMBXMEMPAR (MODBUS_ENOBASE + MODBUS_EXCEPTION_MEMORY_PARITY)
+#define EMBXGPATH  (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_PATH)
+#define EMBXGTAR   (MODBUS_ENOBASE + MODBUS_EXCEPTION_GATEWAY_TARGET)
+
+/* Native libmodbus error codes */
+#define EMBBADCRC  (EMBXGTAR + 1)
+#define EMBBADDATA (EMBXGTAR + 2)
+#define EMBBADEXC  (EMBXGTAR + 3)
+#define EMBUNKEXC  (EMBXGTAR + 4)
+#define EMBMDATA   (EMBXGTAR + 5)
+#define EMBBADSLAVE (EMBXGTAR + 6)
+
+typedef struct _modbus modbus_t;
+
+typedef struct {
+    int nb_bits;
+    int start_bits;
+    int nb_input_bits;
+    int start_input_bits;
+    int nb_input_registers;
+    int start_input_registers;
+    int nb_registers;
+    int start_registers;
+    uint8_t *tab_bits;
+    uint8_t *tab_input_bits;
+    uint16_t *tab_input_registers;
+    uint16_t *tab_registers;
+} modbus_mapping_t;
+
+typedef enum
+{
+    MODBUS_ERROR_RECOVERY_NONE          = 0,
+    MODBUS_ERROR_RECOVERY_LINK          = (1<<1),
+    MODBUS_ERROR_RECOVERY_PROTOCOL      = (1<<2)
+} modbus_error_recovery_mode;
+
+MODBUS_API int modbus_set_slave(modbus_t* ctx, int slave);
+MODBUS_API int modbus_set_error_recovery(modbus_t *ctx, modbus_error_recovery_mode error_recovery);
+MODBUS_API int modbus_set_socket(modbus_t *ctx, int s);
+MODBUS_API int modbus_get_socket(modbus_t *ctx);
+
+MODBUS_API int modbus_get_response_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
+MODBUS_API int modbus_set_response_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
+
+MODBUS_API int modbus_get_byte_timeout(modbus_t *ctx, uint32_t *to_sec, uint32_t *to_usec);
+MODBUS_API int modbus_set_byte_timeout(modbus_t *ctx, uint32_t to_sec, uint32_t to_usec);
+
+MODBUS_API int modbus_get_header_length(modbus_t *ctx);
+
+MODBUS_API int modbus_connect(modbus_t *ctx);
+MODBUS_API void modbus_close(modbus_t *ctx);
+
+MODBUS_API void modbus_free(modbus_t *ctx);
+
+MODBUS_API int modbus_flush(modbus_t *ctx);
+MODBUS_API int modbus_set_debug(modbus_t *ctx, int flag);
+
+MODBUS_API const char *modbus_strerror(int errnum);
+
+MODBUS_API int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
+MODBUS_API int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
+MODBUS_API int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
+MODBUS_API int modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
+MODBUS_API int modbus_write_bit(modbus_t *ctx, int coil_addr, int status);
+MODBUS_API int modbus_write_register(modbus_t *ctx, int reg_addr, int value);
+MODBUS_API int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
+MODBUS_API int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
+MODBUS_API int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask);
+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);
+MODBUS_API int modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest);
+
+MODBUS_API modbus_mapping_t* modbus_mapping_new_start_address
+(
+    unsigned int start_bits, unsigned int nb_bits,
+    unsigned int start_input_bits, unsigned int nb_input_bits,
+    unsigned int start_registers, unsigned int nb_registers,
+    unsigned int start_input_registers, unsigned int nb_input_registers
+);
+
+MODBUS_API modbus_mapping_t* modbus_mapping_new(int nb_bits, int nb_input_bits, int nb_registers, int nb_input_registers);
+MODBUS_API void modbus_mapping_free(modbus_mapping_t *mb_mapping);
+
+MODBUS_API int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length);
+
+MODBUS_API int modbus_receive(modbus_t *ctx, uint8_t *req);
+
+MODBUS_API int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp);
+
+MODBUS_API int modbus_reply(modbus_t *ctx, const uint8_t *req, int req_length, modbus_mapping_t *mb_mapping);
+MODBUS_API int modbus_reply_exception(modbus_t *ctx, const uint8_t *req, unsigned int exception_code);
+
+/**
+ * UTILS FUNCTIONS
+ **/
+
+#define MODBUS_GET_HIGH_BYTE(data) (((data) >> 8) & 0xFF)
+#define MODBUS_GET_LOW_BYTE(data) ((data) & 0xFF)
+#define MODBUS_GET_INT64_FROM_INT16(tab_int16, index) \
+    (((int64_t)tab_int16[(index)    ] << 48) + \
+     ((int64_t)tab_int16[(index) + 1] << 32) + \
+     ((int64_t)tab_int16[(index) + 2] << 16) + \
+      (int64_t)tab_int16[(index) + 3])
+#define MODBUS_GET_INT32_FROM_INT16(tab_int16, index) ((tab_int16[(index)] << 16) + tab_int16[(index) + 1])
+#define MODBUS_GET_INT16_FROM_INT8(tab_int8, index) ((tab_int8[(index)] << 8) + tab_int8[(index) + 1])
+#define MODBUS_SET_INT16_TO_INT8(tab_int8, index, value) \
+    do { \
+        tab_int8[(index)] = (value) >> 8;  \
+        tab_int8[(index) + 1] = (value) & 0xFF; \
+    } while (0)
+#define MODBUS_SET_INT32_TO_INT16(tab_int16, index, value) \
+    do { \
+        tab_int16[(index)    ] = (value) >> 16; \
+        tab_int16[(index) + 1] = (value); \
+    } while (0)
+#define MODBUS_SET_INT64_TO_INT16(tab_int16, index, value) \
+    do { \
+        tab_int16[(index)    ] = (value) >> 48; \
+        tab_int16[(index) + 1] = (value) >> 32; \
+        tab_int16[(index) + 2] = (value) >> 16; \
+        tab_int16[(index) + 3] = (value); \
+    } while (0)
+
+MODBUS_API void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value);
+MODBUS_API void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,
+                                       const uint8_t *tab_byte);
+MODBUS_API uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx, unsigned int nb_bits);
+MODBUS_API float modbus_get_float(const uint16_t *src);
+MODBUS_API float modbus_get_float_abcd(const uint16_t *src);
+MODBUS_API float modbus_get_float_dcba(const uint16_t *src);
+MODBUS_API float modbus_get_float_badc(const uint16_t *src);
+MODBUS_API float modbus_get_float_cdab(const uint16_t *src);
+
+MODBUS_API void modbus_set_float(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_abcd(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_dcba(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_badc(float f, uint16_t *dest);
+MODBUS_API void modbus_set_float_cdab(float f, uint16_t *dest);
+
+#include "modbus-tcp.h"
+#include "modbus-rtu.h"
+
+MODBUS_END_DECLS
+