modbus rtos

.gitignore

@@ -0,0 +1,54 @@
+# Prerequisites
+*.d
+
+# Object files
+*.o
+*.ko
+*.obj
+*.elf
+
+# Linker output
+*.ilk
+*.map
+*.exp
+
+# Precompiled Headers
+*.gch
+*.pch
+
+# Libraries
+*.lib
+*.a
+*.la
+*.lo
+
+# Shared objects (inc. Windows DLLs)
+*.dll
+*.so
+*.so.*
+*.dylib
+
+# Executables
+*.exe
+*.out
+*.app
+*.i*86
+*.x86_64
+*.hex
+
+# Debug files
+*.dSYM/
+*.su
+*.idb
+*.pdb
+
+# Kernel Module Compile Results
+*.mod*
+*.cmd
+.tmp_versions/
+modules.order
+Module.symvers
+Mkfile.old
+dkms.conf
+/package.json
+/Kconfig

SConscript

@@ -0,0 +1,18 @@
+Import('RTT_ROOT')
+from building import *
+
+# get current directory
+cwd = GetCurrentDir()
+
+# The set of source files associated with this SConscript file.
+src = Glob('src/*.c')
+
+if GetDepend(['PKG_USING_SMALL_MODBUS_TEST']):
+    src += Glob('samples/*.c')
+
+path = [cwd + '/paho.mqtt.c/src']
+path += [cwd + '/src']
+
+group = DefineGroup('small_modbus', src, depend = ['PKG_USING_SMALL_MODBUS'], CPPPATH = path)
+
+Return('group')

src/modbus-data.c

@@ -0,0 +1,279 @@
+/*
+ * Copyright © 2010-2014 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+
+#include <stdlib.h>
+#include <string.h>
+#include "stdint.h"
+
+#include "modbus.h"
+
+
+static inline uint16_t bswap_16(uint16_t x)
+{
+    return (x >> 8) | (x << 8);
+}
+
+static inline uint32_t bswap_32(uint32_t x)
+{
+    return (bswap_16(x & 0xffff) << 16) | (bswap_16(x >> 16));
+}
+
+#define modbus_ntohl(x)     (x)
+#define modbus_htonl(x)     (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)
+{
+    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 */
+        MODBUS_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 = modbus_ntohl(((uint32_t)src[0] << 16) + src[1]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a long from 4 bytes (Modbus) without any conversion (ABCD) */
+long modbus_get_long_abcd(const uint16_t *src)
+{
+    long l;
+    uint32_t i;
+
+    i = modbus_ntohl(((uint32_t)src[0] << 16) + src[1]);
+    memcpy(&l, &i, sizeof(float));
+
+    return l;
+}
+
+/* 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 = modbus_ntohl(bswap_32((((uint32_t)src[0]) << 16) + src[1]));
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a long from 4 bytes (Modbus) in inversed format (DCBA) */
+long modbus_get_long_dcba(const uint16_t *src)
+{
+    long l;
+    uint32_t i;
+
+    i = modbus_ntohl(bswap_32((((uint32_t)src[0]) << 16) + src[1]));
+    memcpy(&l, &i, sizeof(float));
+
+    return l;
+}
+
+/* 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 = modbus_ntohl((uint32_t)(bswap_16(src[0]) << 16) + bswap_16(src[1]));
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a long from 4 bytes (Modbus) with swapped bytes (BADC) */
+long modbus_get_long_badc(const uint16_t *src)
+{
+    long l;
+    uint32_t i;
+
+    i = modbus_ntohl((uint32_t)(bswap_16(src[0]) << 16) + bswap_16(src[1]));
+    memcpy(&l, &i, sizeof(float));
+
+    return l;
+}
+
+/* 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 = modbus_ntohl((((uint32_t)src[1]) << 16) + src[0]);
+    memcpy(&f, &i, sizeof(float));
+
+    return f;
+}
+
+/* Get a long from 4 bytes (Modbus) with swapped words (CDAB) */
+long modbus_get_long_cdab(const uint16_t *src)
+{
+    long l;
+    uint32_t i;
+
+    i = modbus_ntohl((((uint32_t)src[1]) << 16) + src[0]);
+    memcpy(&l, &i, sizeof(float));
+
+    return l;
+}
+
+/* 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 = modbus_htonl(i);
+    dest[0] = (uint16_t)(i >> 16);
+    dest[1] = (uint16_t)i;
+}
+
+/* Set a long to 4 bytes for Modbus w/o any conversion (ABCD) */
+void modbus_set_long_abcd(long l, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &l, sizeof(uint32_t));
+    i = modbus_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(modbus_htonl(i));
+    dest[0] = (uint16_t)(i >> 16);
+    dest[1] = (uint16_t)i;
+}
+
+/* Set a long to 4 bytes for Modbus with byte and word swap conversion (DCBA) */
+void modbus_set_long_dcba(long l, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &l, sizeof(uint32_t));
+    i = bswap_32(modbus_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 = modbus_htonl(i);
+    dest[0] = (uint16_t)bswap_16(i >> 16);
+    dest[1] = (uint16_t)bswap_16(i & 0xFFFF);
+}
+
+/* Set a long to 4 bytes for Modbus with byte swap conversion (BADC) */
+void modbus_set_long_badc(long l, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &l, sizeof(uint32_t));
+    i = modbus_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 = modbus_htonl(i);
+    dest[0] = (uint16_t)i;
+    dest[1] = (uint16_t)(i >> 16);
+}
+
+/* Set a long to 4 bytes for Modbus with word swap conversion (CDAB) */
+void modbus_set_long_cdab(long l, uint16_t *dest)
+{
+    uint32_t i;
+
+    memcpy(&i, &l, sizeof(uint32_t));
+    i = modbus_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);
+}

src/modbus-version.h

@@ -0,0 +1,53 @@
+/*
+ * 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)
+
+/* 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)))
+
+#endif /* MODBUS_VERSION_H */

src/modbus.c

@@ -0,0 +1,1455 @@
+/*
+ * 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 "modbus.h"
+#include "modbus_rtu.h"
+
+/* 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 {
+    _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 " ";
+    }
+}
+
+void _error_print(modbus_t *ctx, const char *context)
+{
+   // ctx->backend->debug("ERROR %s", modbus_strerror(errno));
+}
+
+int modbus_flush(modbus_t *ctx)
+{
+    int rc;
+
+    if (ctx == NULL) {
+        return -1;
+    }
+    rc = ctx->backend->flush(ctx);
+    ctx->backend->debug(0,"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->core->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->core->checksum_length;
+}
+
+/* Sends a request/response */
+static int send_msg(modbus_t *ctx, uint8_t *msg, int msg_length)
+{
+    int rc;
+    int i;
+		if(ctx->core->send_msg_pre){
+			msg_length = ctx->core->send_msg_pre(msg, msg_length);
+			/* In recovery mode, the write command will be issued until to be
+				 successful! Disabled by default. */
+		}
+		rc = ctx->backend->write(ctx,msg, msg_length);
+    if (rc > 0 && rc != msg_length) {
+        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) {
+        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. */
+        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->core->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->core->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->core->header_length + 5];
+            break;
+        case MODBUS_FC_WRITE_AND_READ_REGISTERS:
+            length = msg[ctx->core->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->core->header_length + 1];
+        } else {
+            length = 0;
+        }
+    }
+    length += ctx->core->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).
+*/
+
+int _modbus_receive_msg(modbus_t *ctx, uint8_t *msg, msg_type_t msg_type)
+{
+    int rc;
+    int length_to_read;
+    int msg_length = 0;
+    int wait_time = 0;
+    _step_t step;
+
+    if (msg_type == MSG_INDICATION) {
+        /* Wait for a message, we don't know when the message will be* received */
+        wait_time = -1;//
+        //ctx->backend->debug(0,"Waiting %d for a indication...\n",wait_time);
+    } else {
+        wait_time = ctx->backend->read_timeout;//
+        //ctx->backend->debug(0,"Waiting %d for a confirmation...\n",wait_time);
+    }
+
+    step = _STEP_FUNCTION;
+    length_to_read = ctx->core->header_length + 1;
+		
+    while (length_to_read != 0) 
+	{
+        rc = ctx->backend->select(ctx,wait_time);
+        if(rc < 0)
+        {
+            ctx->backend->debug(0,"[%d]select(%d) \n",rc,wait_time);
+            return rc;
+        }
+        rc = ctx->backend->read(ctx,msg + msg_length, length_to_read);
+        if(rc <= 0)
+        {
+            ctx->backend->debug(0,"[%d]read(%d) \n",rc,length_to_read);
+            return MODBUS_FAIL;
+        }
+        if(rc!=length_to_read)
+        {
+            //当想读取和实际读取数据长度不同，buff没数据
+//            rc = -1;
+//            return MODBUS_FAIL;
+        }
+        /* 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->core->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->core->max_adu_length) {
+                                ctx->backend->debug(0,"too many data");
+                                return -1;
+                        }
+                        step = _STEP_DATA;
+                        break;
+                default:
+                        break;
+                }
+        }
+    }//数据读取正确后
+    return ctx->core->check_integrity(ctx, msg, msg_length);
+
+}
+
+/* Receive the request from a modbus master */
+int modbus_receive(modbus_t *ctx, uint8_t *req)
+{
+	int rc=-1;
+	if (ctx == NULL) {
+			return rc;
+	}
+	//return ctx->backend->receive(ctx, req);
+	if(ctx->msg_type == MSG_INDICATION)
+	{
+		rc = _modbus_receive_msg(ctx, req,MSG_INDICATION);
+	}else
+	{
+		rc = _modbus_receive_msg(ctx, req,MSG_CONFIRMATION);
+	}
+	return rc;
+}
+
+
+/* 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) {
+        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->core->header_length;
+    const int function = rsp[offset];
+
+    if (ctx->core->pre_check_confirmation)
+		{
+        rc = ctx->core->pre_check_confirmation(ctx,req, rsp, rsp_length);
+        if (rc == -1) {
+            return -1;
+        }
+    }
+    rsp_length_computed = compute_response_length_from_request(ctx, req);
+
+    /* Exception code */
+    if (function >= 0x80) {
+        if (rsp_length == (offset + 2 + (int)ctx->core->checksum_length) && req[offset] == (rsp[offset] - 0x80))
+				{
+            /* Valid exception code received */
+            int exception_code = rsp[offset + 1];
+            if (exception_code < MODBUS_EXCEPTION_MAX) {
+							
+            } else {
+            }
+            return -1;
+        } else {
+            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]) {
+            ctx->backend->debug(0,"Received function not corresponding to the request (0x%X != 0x%X)\n",function, req[offset]);
+            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 {
+            ctx->backend->debug(0,"Quantity not corresponding to the request (%d != %d)\n", rsp_nb_value, req_nb_value);
+            rc = -1;
+        }
+    } else {
+        ctx->backend->debug(0,"Message length not corresponding to the computed length (%d != %d)\n", rsp_length, rsp_length_computed);
+        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)
+{
+    int rsp_length;
+//	uint8_t buff[128];
+//    /* Print debug message */
+//    va_list ap;
+//
+//    va_start(ap, template);
+//    //vfdebug(0,stderr, template, ap);
+//    rt_vsnprintf(buff, sizeof(buff) - 1, template, ap);
+//    rt_kprintf(template);
+//    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->core->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) {
+        return -1;
+    }
+
+    offset = ctx->core->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->core->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) {
+            ctx->backend->debug(0,"Illegal nb of values %d in %s (max %d)\n",nb, name, MODBUS_MAX_READ_BITS);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE);
+        } else if (mapping_address < 0 || (mapping_address + nb) > nb_bits) {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in %s\n",mapping_address < 0 ? address : address + nb, name);
+            rsp_length = response_exception(ctx, &sft,MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } else {
+            rsp_length = ctx->core->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) {
+            ctx->backend->debug(0,"Illegal nb of values %d in %s (max %d)\n",nb, name, MODBUS_MAX_READ_REGISTERS);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE);
+        } else if (mapping_address < 0 || (mapping_address + nb) > nb_registers) {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in %s\n",mapping_address < 0 ? address : address + nb, name);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } else {
+            int i;
+            rsp_length = ctx->core->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) {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in write_bit\n",address);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } 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 {
+                ctx->backend->debug(0,"Illegal data value 0x%0X in write_bit request at address %0X\n",data, address);
+                rsp_length = response_exception(ctx, &sft,MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, FALSE);
+            }
+        }
+    }
+        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)
+        {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in write_register\n",address);
+            rsp_length = response_exception(ctx, &sft,MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } 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. */
+            ctx->backend->debug(0,"Illegal number of values %d in write_bits (max %d)\n",nb, MODBUS_MAX_WRITE_BITS);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE);
+
+        } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_bits)
+        {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in write_bits\n",mapping_address < 0 ? address : address + nb);
+            rsp_length = response_exception(ctx, &sft,MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } else {
+            /* 6 = byte count */
+            modbus_set_bits_from_bytes(mb_mapping->tab_bits, mapping_address, nb,&req[offset + 6]);
+            rsp_length = ctx->core->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)
+        {
+            ctx->backend->debug(0,"Illegal number of values %d in write_registers (max %d)\n",nb, MODBUS_MAX_WRITE_REGISTERS);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE);
+        } else if (mapping_address < 0 || (mapping_address + nb) > mb_mapping->nb_registers)
+        {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in write_registers\n",mapping_address < 0 ? address : address + nb);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } 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->core->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->core->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;
+        /* 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;
+    }
+        break;
+    case MODBUS_FC_READ_EXCEPTION_STATUS:
+        ctx->backend->debug(0,"FIXME Not implemented\n");
+        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)
+        {
+            ctx->backend->debug(0,"Illegal data address 0x%0X in write_register\n",address);
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } 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)
+        {
+                ctx->backend->debug(0,"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);
+
+                rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_VALUE, rsp, TRUE);
+        } else if (mapping_address < 0 ||
+                   (mapping_address + nb) > mb_mapping->nb_registers ||
+                   mapping_address < 0 ||
+                   (mapping_address_write + nb_write) > mb_mapping->nb_registers)
+        {
+
+            ctx->backend->debug(0,"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);
+
+            rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_DATA_ADDRESS, rsp, FALSE);
+        } else {
+            int i, j;
+            rsp_length = ctx->core->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:
+        ctx->backend->debug(0,"Unknown Modbus function code: 0x%0X\n", function);
+        rsp_length = response_exception(ctx, &sft, MODBUS_EXCEPTION_ILLEGAL_FUNCTION, rsp, TRUE);
+        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) {
+        return -1;
+    }
+
+    offset = ctx->core->header_length;
+    slave = req[offset - 1];
+    function = req[offset];
+
+    sft.slave = slave;
+    sft.function = function + 0x80;;
+    sft.t_id = ctx->core->prepare_response_tid(req, &dummy_length);
+    rsp_length = ctx->core->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 {
+        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->core->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->core->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) {
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_BITS) {
+        ctx->backend->debug(0,"ERROR Too many bits requested (%d > %d)\n",nb, MODBUS_MAX_READ_BITS);
+        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) {
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_BITS) {
+        ctx->backend->debug(0,"ERROR Too many discrete inputs requested (%d > %d)\n",nb, MODBUS_MAX_READ_BITS);
+        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) {
+        ctx->backend->debug(0,"ERROR Too many registers requested (%d > %d)\n",nb, MODBUS_MAX_READ_REGISTERS);
+        return -1;
+    }
+
+    req_length = ctx->core->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->core->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)
+        return -1;
+
+    if (nb > MODBUS_MAX_READ_REGISTERS) {
+        ctx->backend->debug(0,"ERROR Too many registers requested (%d > %d)\n",nb, MODBUS_MAX_READ_REGISTERS);
+        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) {
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_READ_REGISTERS) {
+        ctx->backend->debug(0,"ERROR Too many input registers requested (%d > %d)\n",nb, MODBUS_MAX_READ_REGISTERS);
+        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) {
+        return -1;
+    }
+
+    req_length = ctx->core->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) {
+        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) {
+        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) {
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_WRITE_BITS) {
+        ctx->backend->debug(0,"ERROR Writing too many bits (%d > %d)\n",nb, MODBUS_MAX_WRITE_BITS);
+        return -1;
+    }
+
+    req_length = ctx->core->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) {
+        return -1;
+    }
+
+    if (nb > MODBUS_MAX_WRITE_REGISTERS) {
+        ctx->backend->debug(0,"ERROR Trying to write to too many registers (%d > %d)\n",nb, MODBUS_MAX_WRITE_REGISTERS);
+        return -1;
+    }
+
+    req_length = ctx->core->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->core->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) {
+        return -1;
+    }
+
+    if (write_nb > MODBUS_MAX_WR_WRITE_REGISTERS) {
+        ctx->backend->debug(0,"ERROR Too many registers to write (%d > %d)\n",write_nb, MODBUS_MAX_WR_WRITE_REGISTERS);
+        return -1;
+    }
+
+    if (read_nb > MODBUS_MAX_WR_READ_REGISTERS) {
+        ctx->backend->debug(0,"ERROR Too many registers requested (%d > %d)\n",read_nb, MODBUS_MAX_WR_READ_REGISTERS);
+        return -1;
+    }
+    req_length = ctx->core->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->core->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) {
+        return -1;
+    }
+
+    req_length = ctx->core->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->core->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;
+}
+
+/* Define the slave number */
+int modbus_set_slave(modbus_t *ctx, int slave)
+{
+    if (ctx == NULL){
+        return -1;
+    }
+		
+    if (slave >= 0 && slave <= 247){
+       return ctx->slave_addr = slave;
+    } else {
+        return -1;
+    }
+}
+
+int modbus_get_header_length(modbus_t *ctx)
+{
+    if (ctx == NULL) {
+        return -1;
+    }
+
+    return ctx->core->header_length;
+}
+
+int modbus_connect(modbus_t *ctx)
+{
+    if (ctx == NULL)
+        return -1;
+    return ctx->backend->open(ctx);
+}
+
+int modbus_disconnect(modbus_t *ctx)
+{
+    if (ctx == NULL)
+        return -1;
+    return ctx->backend->close(ctx);
+}
+
+
+//void modbus_debug(modbus_t *ctx,int level,const char *fmt, ...)
+//{
+//    ctx->backend->debug(level,fmt,...);
+//}
+
+
+/* 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.
+// */
+//uint32_t strlcpy(char *dest, const char *src, uint32_t dest_size)
+//{
+//     char *d = dest;
+//     const char *s = src;
+//     uint32_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

src/modbus.h

@@ -0,0 +1,349 @@
+/*
+ * Copyright © 2001-2013 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+
+#ifndef MODBUS_H
+#define MODBUS_H
+
+#include "stdint.h"
+#include "string.h"
+#include "stdio.h"
+#include "modbus-version.h"
+
+#ifndef FALSE
+#define FALSE 0
+#endif
+
+#ifndef TRUE
+#define TRUE 1
+#endif
+
+#ifndef OFF
+#define OFF 0
+#endif
+
+#ifndef ON
+#define ON 1
+#endif
+
+#ifndef NULL
+#define NULL ((void *)0)
+#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)
+
+extern const unsigned int libmodbus_version_major;
+extern const unsigned int libmodbus_version_minor;
+extern const unsigned int libmodbus_version_micro;
+
+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  modbus_t;
+typedef struct _modbus_frontend modbus_frontend_t;  //IO寄存器配置
+typedef struct _modbus_backend  modbus_backend_t;
+typedef struct _modbus_core     modbus_core_t;
+typedef struct _modbus_mapping  modbus_mapping_t;
+
+struct _modbus_backend {
+    int  read_timeout;
+    int  write_timeout;
+    int (*open) (modbus_t *ctx);
+    int (*close)(modbus_t *ctx);
+    int (*read) (modbus_t *ctx,uint8_t *data,uint16_t length);  //读函数
+    int (*write)(modbus_t *ctx,uint8_t *data,uint16_t length);  //写函数
+    int (*flush)(modbus_t *ctx);  //清空fifo
+    int (*select)(modbus_t *ctx,int timeout);  //堵塞等待
+    void (*debug)(int level,const char *fmt, ...); //调试
+};
+
+struct _modbus_frontend {
+    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;
+//    struct bit{int start;int num;uint8_t *array;};
+//    struct input_bit{int start;int num;uint8_t *array;};
+//    struct registers{int start;int num;uint16_t *array;};
+//    struct input_registers{int start;int num;uint16_t *array;};
+};
+
+struct _modbus_core {
+    unsigned int backend_type;
+    unsigned int header_length;
+    unsigned int checksum_length;
+    unsigned int max_adu_length;
+    int (*build_request_basis) (modbus_t *ctx, int function,int regaddr, 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);
+    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);
+};
+
+struct _modbus{
+    /* Slave address */
+    int        fd;
+    int        error_num;
+    int        slave_addr;
+    uint8_t     dev_type;
+    uint8_t     msg_type;
+
+    modbus_core_t *core;
+    modbus_backend_t *backend;
+    modbus_frontend_t *frontend;
+    modbus_mapping_t *mapping;
+    void *backend_data;
+};
+
+struct _modbus_mapping{
+    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;
+};
+
+typedef enum
+{
+    MODBUS_ERROR_RECOVERY_NONE          = 0,
+    MODBUS_ERROR_RECOVERY_LINK          = (1<<1),
+    MODBUS_ERROR_RECOVERY_PROTOCOL      = (1<<2)
+} modbus_error_recovery_mode;
+
+enum returnCode {
+    MODBUS_DIS = -3,
+    MODBUS_FAIL = -2,
+    MODBUS_TIMEOUT = -1,
+    MODBUS_OK = 0
+};
+
+int modbus_connect(modbus_t *ctx);
+int modbus_disconnect(modbus_t *ctx);
+int modbus_flush(modbus_t *ctx);
+
+int modbus_set_slave(modbus_t* ctx, int slave);
+int modbus_set_debug(modbus_t *ctx, int flag);
+
+int modbus_read_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
+int modbus_read_input_bits(modbus_t *ctx, int addr, int nb, uint8_t *dest);
+int modbus_read_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
+int modbus_read_input_registers(modbus_t *ctx, int addr, int nb, uint16_t *dest);
+int modbus_write_bit(modbus_t *ctx, int coil_addr, int status);
+int modbus_write_register(modbus_t *ctx, int reg_addr, int value);
+int modbus_write_bits(modbus_t *ctx, int addr, int nb, const uint8_t *data);
+int modbus_write_registers(modbus_t *ctx, int addr, int nb, const uint16_t *data);
+int modbus_mask_write_register(modbus_t *ctx, int addr, uint16_t and_mask, uint16_t or_mask);
+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 modbus_report_slave_id(modbus_t *ctx, int max_dest, uint8_t *dest);
+
+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* modbus_mapping_new(int nb_bits, int nb_input_bits,int nb_registers, int nb_input_registers);
+
+void modbus_mapping_free(modbus_mapping_t *mb_mapping);
+
+int modbus_send_raw_request(modbus_t *ctx, uint8_t *raw_req, int raw_req_length);
+
+int modbus_receive(modbus_t *ctx, uint8_t *req);
+
+int modbus_receive_confirmation(modbus_t *ctx, uint8_t *rsp);
+
+int modbus_reply(modbus_t *ctx, const uint8_t *req,int req_length, modbus_mapping_t *mb_mapping);
+
+int modbus_reply_exception(modbus_t *ctx, const uint8_t *req,unsigned int exception_code);
+
+#define modbus_debug(ctx,level,...)  ctx->backend->debug(level,__VA_ARGS__)
+
+/**
+ * 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)
+
+void modbus_set_bits_from_byte(uint8_t *dest, int idx, const uint8_t value);
+void modbus_set_bits_from_bytes(uint8_t *dest, int idx, unsigned int nb_bits,const uint8_t *tab_byte);
+
+uint8_t modbus_get_byte_from_bits(const uint8_t *src, int idx, unsigned int nb_bits);
+float modbus_get_float(const uint16_t *src);
+float modbus_get_float_abcd(const uint16_t *src);
+float modbus_get_float_dcba(const uint16_t *src);
+float modbus_get_float_badc(const uint16_t *src);
+float modbus_get_float_cdab(const uint16_t *src);
+
+long modbus_get_long_abcd(const uint16_t *src);
+long modbus_get_long_dcba(const uint16_t *src);
+long modbus_get_long_badc(const uint16_t *src);
+long modbus_get_long_cdab(const uint16_t *src);
+
+void modbus_set_float(float f, uint16_t *dest);
+void modbus_set_float_abcd(float f, uint16_t *dest);
+void modbus_set_float_dcba(float f, uint16_t *dest);
+void modbus_set_float_badc(float f, uint16_t *dest);
+void modbus_set_float_cdab(float f, uint16_t *dest);
+
+void modbus_set_long_abcd(long l, uint16_t *dest);
+void modbus_set_long_dcba(long l, uint16_t *dest);
+void modbus_set_long_badc(long l, uint16_t *dest);
+void modbus_set_long_cdab(long l, uint16_t *dest);
+
+#endif  /* MODBUS_H */

src/modbus_ringbuffer.c

@@ -0,0 +1,305 @@
+/*
+#include <modbus_ringbuffer.h>
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ * 2012-09-30     Bernard      first version.
+ * 2013-05-08     Grissiom     reimplement
+ * 2016-08-18     heyuanjie    add interface
+ */
+#include <string.h>
+#include "modbus_ringbuffer.h"
+
+
+enum modbus_ringbuffer_state modbus_ringbuffer_status(struct modbus_ringbuffer *rb)
+{
+    if (rb->read_index == rb->write_index)
+    {
+        if (rb->read_mirror == rb->write_mirror)
+            return MODBUS_RINGBUFFER_EMPTY;
+        else
+            return MODBUS_RINGBUFFER_FULL;
+    }
+    return MODBUS_RINGBUFFER_HALFFULL;
+}
+
+void modbus_ringbuffer_init(struct modbus_ringbuffer *rb,uint8_t *pool,int16_t size)
+{
+    MODBUS_ASSERT(rb != RT_NULL);
+    RT_ASSERT(size > 0);
+
+    /* initialize read and write index */
+    rb->read_mirror = rb->read_index = 0;
+    rb->write_mirror = rb->write_index = 0;
+
+    /* set buffer pool and size */
+    rb->buffer_ptr = pool;
+    rb->buffer_size = MODBUS_ALIGN_DOWN(size, 4);
+}
+
+/**
+ * put a block of data into ring buffer
+ */
+int modbus_ringbuffer_put(struct modbus_ringbuffer *rb,const uint8_t *ptr,uint16_t length)
+{
+    uint16_t size;
+
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    /* whether has enough space */
+    size = modbus_ringbuffer_space_len(rb);
+
+    /* no space */
+    if (size == 0)
+        return 0;
+
+    /* drop some data */
+    if (size < length)
+        length = size;
+
+    if (rb->buffer_size - rb->write_index > length)
+    {
+        /* read_index - write_index = empty space */
+        memcpy(&rb->buffer_ptr[rb->write_index], ptr, length);
+        /* this should not cause overflow because there is enough space for
+         * length of data in current mirror */
+        rb->write_index += length;
+        return length;
+    }
+
+    memcpy(&rb->buffer_ptr[rb->write_index],
+           &ptr[0],
+           rb->buffer_size - rb->write_index);
+    memcpy(&rb->buffer_ptr[0],
+           &ptr[rb->buffer_size - rb->write_index],
+           length - (rb->buffer_size - rb->write_index));
+
+    /* we are going into the other side of the mirror */
+    rb->write_mirror = ~rb->write_mirror;
+    rb->write_index = length - (rb->buffer_size - rb->write_index);
+
+    return length;
+}
+
+/**
+ * put a block of data into ring buffer
+ *
+ * When the buffer is full, it will discard the old data.
+ */
+int modbus_ringbuffer_put_force(struct modbus_ringbuffer *rb,const uint8_t *ptr,uint16_t length)
+{
+    uint16_t space_length;
+
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    space_length = modbus_ringbuffer_space_len(rb);
+
+    if (length > rb->buffer_size)
+    {
+        ptr = &ptr[length - rb->buffer_size];
+        length = rb->buffer_size;
+    }
+
+    if (rb->buffer_size - rb->write_index > length)
+    {
+        /* read_index - write_index = empty space */
+        memcpy(&rb->buffer_ptr[rb->write_index], ptr, length);
+        /* this should not cause overflow because there is enough space for
+         * length of data in current mirror */
+        rb->write_index += length;
+
+        if (length > space_length)
+            rb->read_index = rb->write_index;
+
+        return length;
+    }
+
+    memcpy(&rb->buffer_ptr[rb->write_index],
+           &ptr[0],
+           rb->buffer_size - rb->write_index);
+    memcpy(&rb->buffer_ptr[0],
+           &ptr[rb->buffer_size - rb->write_index],
+           length - (rb->buffer_size - rb->write_index));
+
+    /* we are going into the other side of the mirror */
+    rb->write_mirror = ~rb->write_mirror;
+    rb->write_index = length - (rb->buffer_size - rb->write_index);
+
+    if (length > space_length)
+    {
+        rb->read_mirror = ~rb->read_mirror;
+        rb->read_index = rb->write_index;
+    }
+
+    return length;
+}
+
+/**
+ *  get data from ring buffer
+ */
+int modbus_ringbuffer_get(struct modbus_ringbuffer *rb,uint8_t *ptr,uint16_t length)
+{
+    int size;
+
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    /* whether has enough data  */
+    size = modbus_ringbuffer_data_len(rb);
+
+    /* no data */
+    if (size == 0)
+        return 0;
+
+    /* less data */
+    if (size < length)
+        length = size;
+
+    if (rb->buffer_size - rb->read_index > length)
+    {
+        /* copy all of data */
+        memcpy(ptr, &rb->buffer_ptr[rb->read_index], length);
+        /* this should not cause overflow because there is enough space for
+         * length of data in current mirror */
+        rb->read_index += length;
+        return length;
+    }
+
+    memcpy(&ptr[0],
+           &rb->buffer_ptr[rb->read_index],
+           rb->buffer_size - rb->read_index);
+    memcpy(&ptr[rb->buffer_size - rb->read_index],
+           &rb->buffer_ptr[0],
+           length - (rb->buffer_size - rb->read_index));
+
+    /* we are going into the other side of the mirror */
+    rb->read_mirror = ~rb->read_mirror;
+    rb->read_index = length - (rb->buffer_size - rb->read_index);
+
+    return length;
+}
+
+/**
+ * put a character into ring buffer
+ */
+int modbus_ringbuffer_putchar(struct modbus_ringbuffer *rb, const uint8_t ch)
+{
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    /* whether has enough space */
+    if (!modbus_ringbuffer_space_len(rb))
+        return 0;
+
+    rb->buffer_ptr[rb->write_index] = ch;
+
+    /* flip mirror */
+    if (rb->write_index == rb->buffer_size-1)
+    {
+        rb->write_mirror = ~rb->write_mirror;
+        rb->write_index = 0;
+    }
+    else
+    {
+        rb->write_index++;
+    }
+
+    return 1;
+}
+
+/**
+ * put a character into ring buffer
+ *
+ * When the buffer is full, it will discard one old data.
+ */
+int modbus_ringbuffer_putchar_force(struct modbus_ringbuffer *rb, const uint8_t ch)
+{
+    enum modbus_ringbuffer_state old_state;
+
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    old_state = modbus_ringbuffer_status(rb);
+
+    rb->buffer_ptr[rb->write_index] = ch;
+
+    /* flip mirror */
+    if (rb->write_index == rb->buffer_size-1)
+    {
+        rb->write_mirror = ~rb->write_mirror;
+        rb->write_index = 0;
+        if (old_state == MODBUS_RINGBUFFER_FULL)
+        {
+            rb->read_mirror = ~rb->read_mirror;
+            rb->read_index = rb->write_index;
+        }
+    }
+    else
+    {
+        rb->write_index++;
+        if (old_state == MODBUS_RINGBUFFER_FULL)
+            rb->read_index = rb->write_index;
+    }
+
+    return 1;
+}
+
+/**
+ * get a character from a ringbuffer
+ */
+int modbus_ringbuffer_getchar(struct modbus_ringbuffer *rb, uint8_t *ch)
+{
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    /* ringbuffer is empty */
+    if (!modbus_ringbuffer_data_len(rb))
+        return 0;
+
+    /* put character */
+    *ch = rb->buffer_ptr[rb->read_index];
+
+    if (rb->read_index == rb->buffer_size-1)
+    {
+        rb->read_mirror = ~rb->read_mirror;
+        rb->read_index = 0;
+    }
+    else
+    {
+        rb->read_index++;
+    }
+
+    return 1;
+}
+
+/** 
+ * get the size of data in rb 
+ */
+int modbus_ringbuffer_data_len(struct modbus_ringbuffer *rb)
+{
+    switch (modbus_ringbuffer_status(rb))
+    {
+    case MODBUS_RINGBUFFER_EMPTY:
+        return 0;
+    case MODBUS_RINGBUFFER_FULL:
+        return rb->buffer_size;
+    case MODBUS_RINGBUFFER_HALFFULL:
+    default:
+        if (rb->write_index > rb->read_index)
+            return rb->write_index - rb->read_index;
+        else
+            return rb->buffer_size - (rb->read_index - rb->write_index);
+    };
+}
+
+/** 
+ * empty the rb 
+ */
+void modbus_ringbuffer_reset(struct modbus_ringbuffer *rb)
+{
+    MODBUS_ASSERT(rb != RT_NULL);
+
+    rb->read_mirror = 0;
+    rb->read_index = 0;
+    rb->write_mirror = 0;
+    rb->write_index = 0;
+}

src/modbus_ringbuffer.h

@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2006-2018, RT-Thread Development Team
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Change Logs:
+ * Date           Author       Notes
+ */
+#ifndef MODBUS_RINGBUFFER_H__
+#define MODBUS_RINGBUFFER_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <string.h>
+#include <stdint.h>
+
+/* ring buffer */
+struct modbus_ringbuffer
+{
+    uint8_t *buffer_ptr;
+    /* use the msb of the {read,write}_index as mirror bit. You can see this as
+     * if the buffer adds a virtual mirror and the pointers point either to the
+     * normal or to the mirrored buffer. If the write_index has the same value
+     * with the read_index, but in a different mirror, the buffer is full.
+     * While if the write_index and the read_index are the same and within the
+     * same mirror, the buffer is empty. The ASCII art of the ringbuffer is:
+     *
+     *          mirror = 0                    mirror = 1
+     * +---+---+---+---+---+---+---+|+~~~+~~~+~~~+~~~+~~~+~~~+~~~+
+     * | 0 | 1 | 2 | 3 | 4 | 5 | 6 ||| 0 | 1 | 2 | 3 | 4 | 5 | 6 | Full
+     * +---+---+---+---+---+---+---+|+~~~+~~~+~~~+~~~+~~~+~~~+~~~+
+     *  read_idx-^                   write_idx-^
+     *
+     * +---+---+---+---+---+---+---+|+~~~+~~~+~~~+~~~+~~~+~~~+~~~+
+     * | 0 | 1 | 2 | 3 | 4 | 5 | 6 ||| 0 | 1 | 2 | 3 | 4 | 5 | 6 | Empty
+     * +---+---+---+---+---+---+---+|+~~~+~~~+~~~+~~~+~~~+~~~+~~~+
+     * read_idx-^ ^-write_idx
+     *
+     * The tradeoff is we could only use 32KiB of buffer for 16 bit of index.
+     * But it should be enough for most of the cases.
+     *
+     * Ref: http://en.wikipedia.org/wiki/Circular_buffer#Mirroring */
+    uint16_t read_mirror : 1;
+    uint16_t read_index : 15;
+    uint16_t write_mirror : 1;
+    uint16_t write_index : 15;
+    /* as we use msb of index as mirror bit, the size should be signed and
+     * could only be positive. */
+    int16_t buffer_size;
+};
+
+enum modbus_ringbuffer_state
+{
+    MODBUS_RINGBUFFER_EMPTY,
+    MODBUS_RINGBUFFER_FULL,
+    /* half full is neither full nor empty */
+    MODBUS_RINGBUFFER_HALFFULL,
+};
+
+void modbus_ringbuffer_init(struct modbus_ringbuffer *rb, uint8_t *pool, int16_t size);
+void modbus_ringbuffer_reset(struct modbus_ringbuffer *rb);
+int modbus_ringbuffer_put(struct modbus_ringbuffer *rb, const uint8_t *ptr, uint16_t length);
+int modbus_ringbuffer_put_force(struct modbus_ringbuffer *rb, const uint8_t *ptr, uint16_t length);
+int modbus_ringbuffer_putchar(struct modbus_ringbuffer *rb, const uint8_t ch);
+int modbus_ringbuffer_putchar_force(struct modbus_ringbuffer *rb, const uint8_t ch);
+int modbus_ringbuffer_get(struct modbus_ringbuffer *rb, uint8_t *ptr, uint16_t length);
+int modbus_ringbuffer_getchar(struct modbus_ringbuffer *rb, uint8_t *ch);
+int modbus_ringbuffer_data_len(struct modbus_ringbuffer *rb);
+
+uint16_t modbus_ringbuffer_get_size(struct modbus_ringbuffer *rb)
+{
+    return rb->buffer_size;
+}
+
+/** return the size of empty space in rb */
+#define modbus_ringbuffer_space_len(rb) ((rb)->buffer_size - modbus_ringbuffer_data_len(rb))
+
+#define MODBUS_ASSERT(EX)
+
+#define MODBUS_ALIGN_DOWN(size, align)      ((size) & ~((align) - 1))
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif

src/modbus_rtu.c

@@ -0,0 +1,179 @@
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+#include "modbus_rtu.h"
+
+/* 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
+};
+
+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);
+}
+
+/* Builds a RTU request header */
+static int _modbus_rtu_build_request_basis(modbus_t *ctx, int function,int regaddr, int nb,uint8_t *req)
+{
+    req[0] = ctx->slave_addr;
+    req[1] = function;
+    req[2] = regaddr >> 8;
+    req[3] = regaddr & 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 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;
+}
+
+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)
+    {
+        ctx->backend->debug(0,"The responding slave %d isn't the requested slave %d\n",rsp[0], req[0]);
+       return -1;
+    } 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)
+{
+    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_addr && slave != MODBUS_BROADCAST_ADDRESS)
+    {
+        ctx->backend->debug(0,"Request for slave %d ignored (not %d)\n", slave, ctx->slave_addr);
+        /* 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
+    {
+       ctx->backend->debug(0,"ERROR CRC received 0x%0X != CRC calculated 0x%0X\n",crc_received, crc_calculated);
+       return -1;
+    }
+}
+
+const modbus_core_t modbus_rtu_core =
+{
+    .backend_type       = _MODBUS_BACKEND_TYPE_RTU,
+    .header_length      = _MODBUS_RTU_HEADER_LENGTH,
+    .checksum_length    = _MODBUS_RTU_CHECKSUM_LENGTH,
+    .max_adu_length     = MODBUS_RTU_MAX_ADU_LENGTH,
+    .build_request_basis    = _modbus_rtu_build_request_basis,
+    .build_response_basis   = _modbus_rtu_build_response_basis,
+    .prepare_response_tid   = _modbus_rtu_prepare_response_tid,
+    .send_msg_pre           = _modbus_rtu_send_msg_pre,
+    .check_integrity        = _modbus_rtu_check_integrity,
+    .pre_check_confirmation = _modbus_rtu_pre_check_confirmation
+};

src/modbus_rtu.h

@@ -0,0 +1,81 @@
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+
+#ifndef MODBUS_RTU_H
+#define MODBUS_RTU_H
+
+#include "modbus.h"
+
+/* 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
+
+#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
+
+
+/* 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
+
+
+int modbus_rtu_init(modbus_t *ctx,modbus_backend_t *backend,void *config);
+
+int modbus_rtu_set_open_ops(modbus_t *ctx, int (*open)(modbus_t *ctx));
+int modbus_rtu_set_close_ops(modbus_t *ctx, int (*close)(modbus_t *ctx));
+int modbus_rtu_set_read_ops(modbus_t *ctx, int (*read)(modbus_t *ctx,uint8_t *data,uint16_t length));
+int modbus_rtu_set_write_ops(modbus_t *ctx, int (*write)(modbus_t *ctx,uint8_t *data,uint16_t length));
+int modbus_rtu_set_flush_ops(modbus_t *ctx, int (*flush)(modbus_t *ctx));
+int modbus_rtu_set_select_ops(modbus_t *ctx, int (*select)(modbus_t *ctx,int timeout_ms));
+int modbus_rtu_set_read_timeout(modbus_t *ctx,int timeout_ms);
+int modbus_rtu_set_write_timeout(modbus_t *ctx,int timeout_ms);
+
+
+#ifdef MODBUS_USED_RTOS
+
+#include <rtthread.h>
+#include <dfs_posix.h>
+#include <termios.h>
+
+typedef struct _modbus_rtu_config {
+    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;
+
+    int (*rts_set)(modbus_t *ctx, int on);
+} modbus_rtu_config_t;
+
+int modbus_rtu_config(modbus_t *ctx,char *device,int baud,uint8_t data_bit, uint8_t stop_bit,char parity);
+int modbus_rtu_set_rts_ops(modbus_t *ctx,int (*rts_set)(modbus_t *ctx, int on));
+
+#endif
+
+extern const modbus_core_t modbus_rtu_core;
+
+#endif
+

src/modbus_rtu_nonos.h

@@ -0,0 +1,7 @@
+#ifndef MODBUS_RTU_NONOS_H
+#define MODBUS_RTU_NONOS_H
+
+#include "rtthread.h"
+#include "modbus_rtu.h"
+
+#endif

src/modbus_rtu_rtos.c

@@ -0,0 +1,475 @@
+/*
+ * Copyright © 2001-2011 Stéphane Raimbault <stephane.raimbault@gmail.com>
+ *
+ * SPDX-License-Identifier: LGPL-2.1+
+ */
+#include "modbus_rtu_rtos.h"
+
+int rtos_open(modbus_t *ctx)
+{
+    struct termios tios;
+    speed_t speed;
+    int flags;
+
+    modbus_rtu_config_t *ctx_rtu = ctx->backend_data;
+
+    ctx->backend->debug(0,"open %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;
+    flags = O_RDWR | O_NOCTTY  | O_EXCL;
+#ifdef O_CLOEXEC
+    flags |= O_CLOEXEC;
+#endif
+
+    ctx->fd = open(ctx_rtu->device, flags);
+    if (ctx->fd == -1) {
+        ctx->backend->debug(0,"ERROR Can't open the device %s\n",ctx_rtu->device);
+        return -1;
+    }
+    /* Save */
+    tcgetattr(ctx->fd, &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 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
+    default:
+        speed = B9600;
+    }
+
+    /* Set the baud rate */
+    if ((cfsetispeed(&tios, speed) < 0) ||
+        (cfsetospeed(&tios, speed) < 0)) {
+        close(ctx->fd);
+        ctx->fd = -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] = 0;
+
+    if (tcsetattr(ctx->fd, TCSANOW, &tios) < 0) {
+        close(ctx->fd);
+        ctx->fd = -1;
+        return -1;
+    }
+
+    return 0;
+
+    return 0;
+}
+int rtos_close(modbus_t *ctx)
+{
+    modbus_rtu_config_t *ctx_rtu = ctx->backend_data;
+    if (ctx->fd != -1) {
+        tcsetattr(ctx->fd, TCSANOW, &ctx_rtu->old_tios);
+        close(ctx->fd);
+        ctx->fd = -1;
+    }
+    return 0;
+}
+
+int rtos_read(modbus_t *ctx,uint8_t *data, uint16_t length)
+{
+    int rc = 0;
+    //modbus_rtu_config_t *ctx_rtu = ctx->backend_data;
+    rc =  read(ctx->fd, data, length);
+
+//    int i;
+//    rt_kprintf("read %d,%d :",rc,length);
+//    for (i = 0; i < rc; i++)
+//    {
+//            rt_kprintf("<%02X>", data[i]);
+//    }
+//    rt_kprintf("\n");
+
+    return rc;
+}
+int rtos_write(modbus_t *ctx,uint8_t *data, uint16_t length)
+{
+    int rc;
+    modbus_rtu_config_t *ctx_rtu = ctx->backend_data;
+    if(ctx_rtu->rts_set)
+        ctx_rtu->rts_set(ctx,1);
+
+    rc = write(ctx->fd, data, length);
+
+//    int i;
+//    rt_kprintf("write %d :",length);
+//    for (i = 0; i < length; i++)
+//    {
+//            rt_kprintf("<%02X>", data[i]);
+//    }
+//    rt_kprintf("\n");
+
+    if(ctx_rtu->rts_set)
+        ctx_rtu->rts_set(ctx,0);
+    return length;
+}
+int rtos_flush(modbus_t *ctx)
+{
+    return tcflush(ctx->fd, TCIOFLUSH);
+}
+
+int rtos_select(modbus_t *ctx,int timeout_ms)
+{
+    fd_set rset;
+    struct timeval tv;
+    int rc;
+
+    /* Add a file descriptor to the set */
+    FD_ZERO(&rset);
+    FD_SET(ctx->fd, &rset);
+
+    if(timeout_ms == -1)
+    {
+        rc = select(ctx->fd+1, &rset, NULL, NULL, NULL);    //
+    }else {
+        tv.tv_sec = (timeout_ms/1000);
+        tv.tv_usec = (timeout_ms%1000)*1000;
+
+        rc = select(ctx->fd+1, &rset, NULL, NULL, &tv);
+    }
+    if(rc<0)
+    {
+        return MODBUS_FAIL;
+    }else if(rc == 0)
+    {
+        return MODBUS_TIMEOUT;
+    }
+//    else {
+//        if (FD_ISSET(ctx->fd, &rset))
+//        {
+//
+//        }
+//    }
+    return rc;
+}
+
+static uint8_t now_level = 6;
+
+void rtos_debug(int level,const char *fmt, ...)
+{
+    static char log_buf[32];
+    if(level < now_level)
+    {
+        va_list args;
+        va_start(args, fmt);
+        rt_vsnprintf(log_buf, 32, fmt, args);
+        va_end(args);
+        printf(log_buf);
+    }
+}
+
+int debug_modbus(int argc, char**argv)
+{
+    if(argc<2)
+    {
+        rt_kprintf("debug_modbus [0-5]");
+    }else
+    {
+        now_level  = atoi(argv[1])%6;
+    }
+    return RT_EOK;
+}
+MSH_CMD_EXPORT(debug_modbus,debug_modbus [0-5])
+
+modbus_backend_t modbus_rtu_rtos_backend =
+{
+    .read_timeout = 500,
+    .write_timeout = 100,
+    .open =  rtos_open,
+    .close = rtos_close,
+    .read =  rtos_read,
+    .write = rtos_write,
+    .flush = rtos_flush,
+    .select = rtos_select,
+    .debug = rtos_debug
+};
+
+
+int modbus_rtu_init(modbus_t *ctx,modbus_backend_t *backend,void *config)
+{
+    ctx->core = &modbus_rtu_core;
+    ctx->backend_data = config;
+    if(backend == NULL)
+    {
+        ctx->backend = &modbus_rtu_rtos_backend;
+    }
+    return 0;
+}
+
+int modbus_rtu_config(modbus_t *ctx,char *device,int baud,uint8_t data_bit, uint8_t stop_bit,char parity)
+{
+    modbus_rtu_config_t * config = ctx->backend_data;
+    config->device = device;
+    config->baud = baud;
+    config->data_bit = data_bit;
+    config->stop_bit = stop_bit;
+    config->parity = parity;
+    return 0;
+}
+
+int modbus_rtu_set_rts_ops(modbus_t *ctx,int (*rts_set)(modbus_t *ctx, int on))
+{
+    modbus_rtu_config_t * config = ctx->backend_data;
+    config->rts_set = rts_set;
+    return 0;
+}
+
+int modbus_rtu_set_open_ops(modbus_t *ctx, int (*open)(modbus_t *ctx))
+{
+    ctx->backend->open = open;
+    return 0;
+}
+
+int modbus_rtu_set_close_ops(modbus_t *ctx, int (*close)(modbus_t *ctx))
+{
+    ctx->backend->close = close;
+    return 0;
+}
+
+int modbus_rtu_set_read_ops(modbus_t *ctx, int (*read)(modbus_t *ctx,uint8_t *data,uint16_t length))
+{
+    ctx->backend->read = read;
+    return 0;
+}
+
+int modbus_rtu_set_write_ops(modbus_t *ctx, int (*write)(modbus_t *ctx,uint8_t *data,uint16_t length))
+{
+    ctx->backend->write = write;
+    return 0;
+}
+
+int modbus_rtu_set_flush_ops(modbus_t *ctx, int (*flush)(modbus_t *ctx))
+{
+    ctx->backend->flush = flush;
+    return 0;
+}
+
+int modbus_rtu_set_select_ops(modbus_t *ctx, int (*select)(modbus_t *ctx,int timeout_ms))
+{
+    ctx->backend->select = select;
+    return 0;
+}
+
+int modbus_rtu_set_read_timeout(modbus_t *ctx,int timeout_ms)
+{
+    ctx->backend->read_timeout = timeout_ms;
+    return timeout_ms;
+}
+
+int modbus_rtu_set_write_timeout(modbus_t *ctx,int timeout_ms)
+{
+    ctx->backend->write_timeout = timeout_ms;
+    return timeout_ms;
+}
+
+
+
+
+

src/modbus_rtu_rtos.h

@@ -0,0 +1,12 @@
+#ifndef MODBUS_RTU_RTOS_H
+#define MODBUS_RTU_RTOS_H
+
+#define MODBUS_USED_RTOS
+
+#include "modbus_rtu.h"
+
+#include <rtthread.h>
+#include <dfs_posix.h>
+#include <termios.h>
+
+#endif