implement modbus tcp slave at port 502, reg1=1042 as device ID

components/modbus_slave/CMakeLists.txt

@@ -0,0 +1,7 @@
+set(COMPONENT_ADD_INCLUDEDIRS include)
+set(COMPONENT_SRCS
+    "src/modbus.c"
+    "src/pp/payload_builder.c"
+    "src/pp/payload_parser.c")
+
+register_component()

components/modbus_slave/README.txt

@@ -0,0 +1 @@
+modbus slave

components/modbus_slave/component.mk

@@ -0,0 +1,3 @@
+
+COMPONENT_SRCDIRS := src
+COMPONENT_ADD_INCLUDEDIRS := include

components/modbus_slave/include/modbus.h

@@ -0,0 +1,103 @@
+/**
+ * Modbus slave
+ */
+
+#ifndef MODBUS_H
+#define MODBUS_H
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <sys/types.h>
+
+// Config - TODO do this via cmake
+//#define MB_SUPPORT_FC01  // READ_COILS
+//#define MB_SUPPORT_FC02  // READ_DISCRETES
+#define MB_SUPPORT_FC03  // READ_HOLDING_REGISTERS
+//#define MB_SUPPORT_FC04  // READ_INPUT_REGISTERS
+//#define MB_SUPPORT_FC05  // WRITE_SINGLE_COIL
+#define MB_SUPPORT_FC06  // WRITE_SINGLE_REGISTER
+//#define MB_SUPPORT_FC15  // WRITE_MULTIPLE_COILS
+#define MB_SUPPORT_FC16  // WRITE_MULTIPLE_REGISTERS
+//#define MB_SUPPORT_FC22  // MASK_WRITE_REGISTER
+//#define MB_SUPPORT_FC23  // READ_WRITE_MULTIPLE_REGISTERS
+
+typedef enum ModbusException {
+  MB_EXCEPTION_OK = 0,
+  MB_EXCEPTION_ILLEGAL_FUNCTION = 1,
+  MB_EXCEPTION_ILLEGAL_DATA_ADDRESS = 2,
+  MB_EXCEPTION_ILLEGAL_DATA_VALUE = 3,
+  MB_EXCEPTION_SLAVE_DEVICE_FAILURE = 4,
+  // other codes exist but are not meaningful for simple slave devices
+} ModbusException_t;
+
+typedef enum ModbusFunction {
+  FC01_READ_COILS = 1,
+  FC02_READ_DISCRETES = 2,
+  FC03_READ_HOLDING_REGISTERS = 3,
+  FC04_READ_INPUT_REGISTERS = 4,
+  FC05_WRITE_SINGLE_COIL = 5,
+  FC06_WRITE_SINGLE_REGISTER = 6,
+  FC15_WRITE_MULTIPLE_COILS = 15,
+  FC16_WRITE_MULTIPLE_REGISTERS = 16,
+  FC22_MASK_WRITE_REGISTER = 22,
+  FC23_READ_WRITE_MULTIPLE_REGISTERS = 23,
+} ModbusFunction_t;
+
+typedef enum ModbusError {
+  MB_OK = 0,
+  MB_ERROR = 1,
+  MB_ERR_NOTFORME = 2,
+  MB_ERR_NEEDMORE = 3,
+  MB_ERR_CHECKSUM = 4,
+  MB_ERR_BADPROTO = 5,
+} ModbusError_t;
+
+typedef enum ModbusProtocol {
+  MB_PROTO_RTU,
+  MB_PROTO_TCP
+} ModbusProtocol_t;
+
+typedef struct ModbusSlave ModbusSlave_t;
+
+struct ModbusSlave {
+  uint8_t addr;
+  ModbusProtocol_t proto;
+  void *userData;
+  ModbusException_t (*startOfAccess)(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id);
+  void (*endOfAccess)(ModbusSlave_t *ms);
+
+#ifdef MB_SUPPORT_FC01
+  ModbusException_t (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value);
+#endif
+
+#ifdef MB_SUPPORT_FC02
+  ModbusException_t (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value);
+#endif
+
+#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC22) || defined(MB_SUPPORT_FC23)
+  ModbusException_t (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
+#endif
+
+#ifdef MB_SUPPORT_FC04
+  ModbusException_t (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
+#endif
+
+#if defined(MB_SUPPORT_FC15) || defined(MB_SUPPORT_FC05)
+  ModbusException_t (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value);
+#endif
+
+#if defined(MB_SUPPORT_FC06) || defined(MB_SUPPORT_FC16) || defined(MB_SUPPORT_FC22) || defined(MB_SUPPORT_FC23)
+  ModbusException_t (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value);
+#endif
+};
+
+ModbusError_t mb_handleRequest(
+        ModbusSlave_t *ms,
+        const uint8_t *req,
+        size_t req_size,
+        uint8_t* resp,
+        size_t resp_capacity,
+        size_t *resp_size
+);
+
+#endif //MODBUS_H

components/modbus_slave/include/pp/payload_builder.h

@@ -0,0 +1,129 @@
+#ifndef PAYLOAD_BUILDER_H
+#define PAYLOAD_BUILDER_H
+
+/**
+ * PayloadBuilder, part of the TinyFrame utilities collection
+ *
+ * (c) Ondřej Hruška, 2014-2022. MIT license.
+ *
+ * The builder supports big and little endian which is selected when
+ * initializing it or by accessing the bigendian struct field.
+ *
+ * This module helps you with building payloads (not only for TinyFrame)
+ *
+ * The builder performs bounds checking and calls the provided handler when
+ * the requested write wouldn't fit. Use the handler to realloc / flush the buffer
+ * or report an error.
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+#include <stddef.h>
+#include "type_coerce.h"
+
+typedef struct PayloadBuilder_ PayloadBuilder;
+
+/**
+ * Full buffer handler.
+ *
+ * 'needed' more bytes should be written but the end of the buffer was reached.
+ *
+ * Return true if the problem was solved (e.g. buffer was flushed and the
+ * 'current' pointer moved to the beginning).
+ *
+ * If false is returned, the 'ok' flag on the struct is set to false
+ * and all following writes are discarded.
+ */
+typedef bool (*pb_full_handler)(PayloadBuilder *pb, uint32_t needed);
+
+struct PayloadBuilder_ {
+  uint8_t *start;   //!< Pointer to the beginning of the buffer
+  uint8_t *current; //!< Pointer to the next byte to be read
+  uint8_t *end;     //!< Pointer to the end of the buffer (start + length)
+  pb_full_handler full_handler; //!< Callback for buffer overrun
+  bool bigendian;   //!< Flag to use big-endian parsing
+  bool ok;          //!< Indicates that all reads were successful
+};
+
+// --- initializer helper macros ---
+
+/** Start the builder. */
+#define pb_start_e(buf, capacity, bigendian, full_handler) ((PayloadBuilder){buf, buf, (buf)+(capacity), full_handler, bigendian, 1})
+
+/** Start the builder in big-endian mode */
+#define pb_start_be(buf, capacity, full_handler) pb_start_e(buf, capacity, 1, full_handler)
+
+/** Start the builder in little-endian mode */
+#define pb_start_le(buf, capacity, full_handler) pb_start_e(buf, capacity, 0, full_handler)
+
+/** Start the parser in little-endian mode (default) */
+#define pb_start(buf, capacity, full_handler) pb_start_le(buf, capacity, full_handler)
+
+// --- utilities ---
+
+/** Returns true if the parser is still in OK state (not overrun) */
+#define pb_ok(pb) ((pb)->ok)
+
+/** Get already used bytes count */
+#define pb_length(pb) ((pb)->current - (pb)->start)
+
+/** Reset the current pointer to start */
+#define pb_rewind(pb) do { (pb)->current = (pb)->start; } while (0)
+
+/** save & restore position */
+typedef uint8_t* pb_mark_t;
+#define pb_save(pb) ((pb)->current)
+#define pb_restore(pb, mark) do { (pb)->current = (mark); } while (0)
+
+/** Write from a buffer */
+bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len);
+
+/** Write a zero terminated string */
+bool pb_string(PayloadBuilder *pb, const char *str);
+
+/** Write uint8_t to the buffer */
+bool pb_u8(PayloadBuilder *pb, uint8_t byte);
+
+/** Write boolean to the buffer. */
+static inline bool pb_bool(PayloadBuilder *pb, bool b)
+{
+    return pb_u8(pb, (uint8_t) b);
+}
+
+/** Write uint16_t to the buffer. */
+bool pb_u16(PayloadBuilder *pb, uint16_t word);
+
+/** Write uint32_t to the buffer. */
+bool pb_u32(PayloadBuilder *pb, uint32_t word);
+
+/** Write int8_t to the buffer. */
+static inline bool pb_i8(PayloadBuilder *pb, int8_t byte)
+{
+    return pb_u8(pb, ((union conv8) {.i8 = byte}).u8);
+}
+
+/** Write char (int8_t) to the buffer. */
+static inline bool pb_char(PayloadBuilder *pb, char c)
+{
+    return pb_i8(pb, c);
+}
+
+/** Write int16_t to the buffer. */
+static inline bool pb_i16(PayloadBuilder *pb, int16_t word)
+{
+    return pb_u16(pb, ((union conv16) {.i16 = word}).u16);
+}
+
+/** Write int32_t to the buffer. */
+static inline bool pb_i32(PayloadBuilder *pb, int32_t word)
+{
+    return pb_u32(pb, ((union conv32) {.i32 = word}).u32);
+}
+
+/** Write 4-byte float to the buffer. */
+static inline bool pb_float(PayloadBuilder *pb, float f)
+{
+    return pb_u32(pb, ((union conv32) {.f32 = f}).u32);
+}
+
+#endif // PAYLOAD_BUILDER_H

components/modbus_slave/include/pp/payload_parser.h

@@ -0,0 +1,167 @@
+#ifndef PAYLOAD_PARSER_H
+#define PAYLOAD_PARSER_H
+
+/**
+ * PayloadParser, part of the TinyFrame utilities collection
+ *
+ * (c) Ondřej Hruška, 2016-2022. MIT license.
+ *
+ * This module helps you with parsing payloads (not only from TinyFrame).
+ *
+ * The parser supports big and little-endian which is selected when
+ * initializing it or by accessing the bigendian struct field.
+ *
+ * The parser performs bounds checking and calls the provided handler when
+ * the requested read doesn't have enough data. Use the callback to take
+ * appropriate action, e.g. report an error.
+ *
+ * If the handler function is not defined, the pb->ok flag is set to false
+ * (use this to check for success), and further reads won't have any effect
+ * and always result in 0 or empty array.
+ */
+
+#include <stdint.h>
+#include <stddef.h>
+#include <stdbool.h>
+#include "type_coerce.h"
+
+typedef struct PayloadParser_ PayloadParser;
+
+/**
+ * Empty buffer handler.
+ *
+ * 'needed' more bytes should be read but the end was reached.
+ *
+ * Return true if the problem was solved (e.g. new data loaded into
+ * the buffer and the 'current' pointer moved to the beginning).
+ *
+ * If false is returned, the 'ok' flag on the struct is set to false
+ * and all following reads will fail / return 0.
+ */
+typedef bool (*pp_empty_handler)(PayloadParser *pp, uint32_t needed);
+
+struct PayloadParser_ {
+    const uint8_t *start;   //!< Pointer to the beginning of the buffer
+    const uint8_t *current; //!< Pointer to the next byte to be read
+    const uint8_t *end;     //!< Pointer to the end of the buffer (start + length)
+    pp_empty_handler empty_handler; //!< Callback for buffer underrun
+    bool bigendian;   //!< Flag to use big-endian parsing
+    bool ok;          //!< Indicates that all reads were successful
+};
+
+// --- initializer helper macros ---
+
+/** Start the parser. */
+#define pp_start_e(buf, length, bigendian, empty_handler) ((PayloadParser){buf, buf, (buf)+(length), empty_handler, bigendian, 1})
+
+/** Start the parser in big-endian mode */
+#define pp_start_be(buf, length, empty_handler) pp_start_e(buf, length, 1, empty_handler)
+
+/** Start the parser in little-endian mode */
+#define pp_start_le(buf, length, empty_handler) pp_start_e(buf, length, 0, empty_handler)
+
+/** Start the parser in little-endian mode (default) */
+#define pp_start(buf, length, empty_handler) pp_start_le(buf, length, empty_handler)
+
+// --- utilities ---
+
+/** Get remaining length */
+#define pp_remains(pp) ((pp)->end - (pp)->current)
+
+/** Reset the current pointer to start */
+#define pp_rewind(pp) do { (pp)->current = (pp)->start; } while (0)
+
+/** save & restore position */
+typedef const uint8_t* pp_mark_t;
+#define pp_save(pp) ((pp)->current)
+#define pp_restore(pp, mark) do { (pp)->current = (mark); } while (0)
+
+/** Returns true if the parser is still in OK state (not overrun) */
+#define pp_ok(pp) ((pp)->ok)
+/** Returns true if end was reached */
+#define pp_end(pp) ((pp)->end == (pp)->current)
+
+/**
+ * @brief Get the remainder of the buffer.
+ *
+ * Returns NULL and sets 'length' to 0 if there are no bytes left.
+ *
+ * @param pp
+ * @param length : here the buffer length will be stored. NULL to do not store.
+ * @return the remaining portion of the input buffer
+ */
+const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length);
+
+/** Read uint8_t from the payload. */
+uint8_t pp_u8(PayloadParser *pp);
+
+/** Read bool from the payload. */
+static inline bool pp_bool(PayloadParser *pp)
+{
+    return pp_u8(pp) != 0;
+}
+
+/** Skip bytes */
+static inline void pp_skip(PayloadParser *pp, uint32_t num)
+{
+    pp->current += num;
+}
+
+/** Read uint16_t from the payload. */
+uint16_t pp_u16(PayloadParser *pp);
+
+/** Read uint32_t from the payload. */
+uint32_t pp_u32(PayloadParser *pp);
+
+/** Read int8_t from the payload. */
+static inline int8_t pp_i8(PayloadParser *pp)
+{
+    return ((union conv8) {.u8 = pp_u8(pp)}).i8;
+}
+
+/** Read char (int8_t) from the payload. */
+static inline int8_t pp_char(PayloadParser *pp)
+{
+    return pp_i8(pp);
+}
+
+/** Read int16_t from the payload. */
+static inline int16_t pp_i16(PayloadParser *pp)
+{
+    return ((union conv16) {.u16 = pp_u16(pp)}).i16;
+}
+
+/** Read int32_t from the payload. */
+static inline int32_t pp_i32(PayloadParser *pp)
+{
+    return ((union conv32) {.u32 = pp_u32(pp)}).i32;
+}
+
+/** Read 4-byte float from the payload. */
+static inline float pp_float(PayloadParser *pp)
+{
+    return ((union conv32) {.u32 = pp_u32(pp)}).f32;
+}
+
+/**
+ * Parse a zero-terminated string
+ *
+ * @param pp - parser
+ * @param buffer - target buffer
+ * @param maxlen - buffer size
+ * @return actual number of bytes, excluding terminator
+ */
+uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen);
+
+/**
+ * Parse a buffer
+ *
+ * @param pp - parser
+ * @param buffer - target buffer
+ * @param maxlen - buffer size
+ * @return actual number of bytes, excluding terminator
+ */
+uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen);
+
+
+#endif // PAYLOAD_PARSER_H

components/modbus_slave/include/pp/type_coerce.h

@@ -0,0 +1,32 @@
+#ifndef TYPE_COERCE_H
+#define TYPE_COERCE_H
+
+/**
+ * Structs for conversion between types,
+ * part of the TinyFrame utilities collection
+ * 
+ * (c) Ondřej Hruška, 2016-2017. MIT license.
+ * 
+ * This is a support header file for PayloadParser and PayloadBuilder.
+ */
+
+#include <stdint.h>
+#include <stddef.h>
+
+union conv8 {
+    uint8_t u8;
+    int8_t i8;
+};
+
+union conv16 {
+    uint16_t u16;
+    int16_t i16;
+};
+
+union conv32 {
+    uint32_t u32;
+    int32_t i32;
+    float f32;
+};
+
+#endif // TYPE_COERCE_H

components/modbus_slave/src/modbus.c

@@ -0,0 +1,475 @@
+#include <stdint.h>
+#include <stdbool.h>
+#include "pp/payload_parser.h"
+#include "pp/payload_builder.h"
+#include "modbus.h"
+
+/**
+ * Function to calculate MODBUS CRC.
+ *
+ * https://github.com/starnight/MODBUS-CRC/blob/master/modbuscrc.c
+ **/
+static uint16_t crc16_update(uint16_t crc, uint8_t a)
+{
+    int i;
+    crc ^= (uint16_t) a;
+    for (i = 0; i < 8; ++i) {
+        if (crc & 1) {
+            crc = (crc >> 1) ^ 0xA001;
+        } else {
+            crc = (crc >> 1);
+        }
+    }
+    return crc;
+}
+
+static inline uint16_t crc16_init()
+{
+    return 0xFFFF;
+}
+
+/**
+ * Handle a modbus request
+ */
+ModbusError_t mb_handleRequest(
+        ModbusSlave_t *ms,
+        const uint8_t *req,
+        size_t req_size,
+        uint8_t *resp,
+        size_t resp_capacity,
+        size_t *resp_size
+)
+{
+    uint16_t txn_id, protocol_id, ref;
+
+#ifdef MB_SUPPORT_FC22
+    uint16_t and_mask, or_mask;
+#endif
+
+#ifdef MB_SUPPORT_FC23
+    uint16_t ref2, count2;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02)
+    bool bvalue;
+#endif
+
+#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC04) \
+    || defined(MB_SUPPORT_FC16) || defined(MB_SUPPORT_FC22) || defined(MB_SUPPORT_FC23) \
+    || defined(MB_SUPPORT_FC05) || defined(MB_SUPPORT_FC06)
+    uint16_t value;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02) || defined(MB_SUPPORT_FC15)
+    uint8_t scratch, bytecount, bitcnt;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02) || defined(MB_SUPPORT_FC03) \
+    || defined(MB_SUPPORT_FC04) || defined(MB_SUPPORT_FC15) || defined(MB_SUPPORT_FC16) \
+    || defined(MB_SUPPORT_FC23)
+    uint16_t count;
+#endif
+
+    ModbusException_t exc = MB_EXCEPTION_OK;
+    size_t numbytes;
+    uint8_t fcx;
+
+    const size_t TCP_RESP_OVERHEAD = 8;
+    const size_t RTU_RESP_OVERHEAD = 4;
+    const size_t overhead = ms->proto == MB_PROTO_TCP ? TCP_RESP_OVERHEAD : RTU_RESP_OVERHEAD;
+    pb_mark_t resp_fc_mark = NULL, resp_len_mark = NULL, resp_pld_start_mark = NULL;
+
+    const bool tcp = ms->proto == MB_PROTO_TCP;
+    PayloadParser pp = pp_start_be(req, req_size, NULL);
+    PayloadBuilder pb = pb_start_be(resp, resp_capacity, NULL);
+
+    if (tcp) {
+        /* Parse header */
+        txn_id = pp_u16(&pp);
+        protocol_id = pp_u16(&pp);
+        pp_skip(&pp, 2); // msglen
+        if (protocol_id != 0) {
+            return MB_ERR_BADPROTO;
+        }
+    } else {
+        /* check  CRC */
+        uint16_t crc = crc16_init();
+        for (int pos = 0; pos < req_size /* size of CRC */; pos++) {
+            crc = crc16_update(crc, pp_u8(&pp));
+        }
+        if (crc != 0) {
+            return MB_ERR_CHECKSUM;
+        }
+        pp_rewind(&pp);
+    }
+
+    const uint8_t slave_id = pp_u8(&pp);
+
+    /* check addressing (don't check for TCP - UnitID is used e.g. for gateway target devices) */
+    if (!tcp && slave_id != ms->addr) {
+        return MB_ERR_NOTFORME;
+    }
+
+    fcx = pp_u8(&pp);
+
+    if (!pp_ok(&pp)) {
+        return MB_ERR_NEEDMORE;
+    }
+
+    /* start building the response */
+    if (tcp) {
+        pb_u16(&pb, txn_id);
+        pb_u16(&pb, protocol_id);
+        resp_len_mark = pb_save(&pb);
+        pb_u16(&pb, 0); // Placeholder for LEN
+    }
+    pb_u8(&pb, slave_id);
+    resp_fc_mark = pb_save(&pb);
+    pb_u8(&pb, fcx); // Exceptions will add 0x80 to this
+    resp_pld_start_mark = pb_save(&pb);
+
+    if (ms->startOfAccess) {
+        exc = ms->startOfAccess(ms, fcx, slave_id);
+        if (exc != 0) {
+            goto exception;
+        }
+    }
+
+    switch (fcx) {
+#ifdef MB_SUPPORT_FC05
+        case FC05_WRITE_SINGLE_COIL:
+            if (!ms->writeCoil) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+            ref = pp_u16(&pp);
+            value = pp_u16(&pp);
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (resp_capacity < 4 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            if (value == 0xFF00) {
+                exc = ms->writeCoil(ms, ref, true);
+            } else {
+                exc = ms->writeCoil(ms, ref, false);
+            }
+            if (exc != 0) {
+                goto exception;
+            }
+            pb_u16(&pb, ref);
+            pb_u16(&pb, value);
+            break;
+#endif
+
+#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02)
+        case FC01_READ_COILS:
+        case FC02_READ_DISCRETES:
+            /* check we have the needed function */
+#ifdef MB_SUPPORT_FC01
+            if (fcx == FC01_READ_COILS && !ms->readCoil) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+#endif
+#ifdef MB_SUPPORT_FC02
+            if (fcx == FC02_READ_DISCRETES && !ms->readDiscrete) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+#endif
+            ref = pp_u16(&pp);
+            count = pp_u16(&pp);
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            bytecount = (count + 7) / 8;
+            if (count > 255 * 8 || resp_capacity < bytecount + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            pb_u8(&pb, bytecount);
+            bitcnt = 0;
+            scratch = 0;
+            while (count-- > 0) {
+#ifdef MB_SUPPORT_FC01
+                if (fcx == FC01_READ_COILS) {
+                    exc = ms->readCoil(ms, ref++, &bvalue);
+                }
+#endif
+#ifdef MB_SUPPORT_FC02
+                if (fcx == FC02_READ_DISCRETES) {
+                    exc = ms->readDiscrete(ms, ref++, &bvalue);
+                }
+#endif
+                if (exc != 0) {
+                    goto exception;
+                }
+                scratch |= ((bvalue & 1) << bitcnt);
+                bitcnt++;
+                if (bitcnt == 8) {
+                    pb_u8(&pb, scratch);
+                    scratch = 0;
+                    bitcnt = 0;
+                }
+            }
+            if (bitcnt != 0) {
+                pb_u8(&pb, scratch);
+            }
+            break;
+#endif
+
+#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC04)
+        case FC03_READ_HOLDING_REGISTERS:
+        case FC04_READ_INPUT_REGISTERS:
+#ifdef MB_SUPPORT_FC03
+            /* check we have the needed function */
+            if (fcx == FC03_READ_HOLDING_REGISTERS && !ms->readHolding) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+#endif
+#ifdef MB_SUPPORT_FC04
+            if (fcx == FC04_READ_INPUT_REGISTERS && !ms->readInput) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+#endif
+            ref = pp_u16(&pp);
+            count = pp_u16(&pp);
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (count > 255 || resp_capacity < count * 2 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            pb_u8(&pb, count*2);
+            while (count-- > 0) {
+#ifdef MB_SUPPORT_FC03
+                if (fcx == FC03_READ_HOLDING_REGISTERS) {
+                    exc = ms->readHolding(ms, ref++, &value);
+                }
+#endif
+#ifdef MB_SUPPORT_FC04
+                if (fcx == FC04_READ_INPUT_REGISTERS) {
+                    exc = ms->readInput(ms, ref++, &value);
+                }
+#endif
+                if (exc != 0) {
+                    goto exception;
+                }
+                pb_u16(&pb, value);
+            }
+            break;
+#endif
+
+#ifdef MB_SUPPORT_FC06
+        case FC06_WRITE_SINGLE_REGISTER:
+            if (!ms->writeHolding) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+            ref = pp_u16(&pp);
+            value = pp_u16(&pp);
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (resp_capacity < 4 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            exc = ms->writeHolding(ms, ref, value);
+            if (exc != 0) {
+                goto exception;
+            }
+            pb_u16(&pb, ref);
+            pb_u16(&pb, value);
+            break;
+#endif
+
+#ifdef MB_SUPPORT_FC16
+        case FC16_WRITE_MULTIPLE_REGISTERS:
+            if (!ms->writeHolding) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+            ref = pp_u16(&pp);
+            count = pp_u16(&pp);
+            pp_skip(&pp, 1); // this is always count * 2
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (count > 255 || resp_capacity < 4 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            if (pp_remains(&pp) < count * 2 + (tcp ? 0 : 2)) {
+                return MB_ERR_NEEDMORE;
+            }
+            pb_u16(&pb, ref);
+            pb_u16(&pb, count);
+            while (count-- > 0) {
+                value = pp_u16(&pp);
+                exc = ms->writeHolding(ms, ref++, value);
+                if (exc != 0) {
+                    goto exception;
+                }
+            }
+            break;
+#endif
+
+#ifdef MB_SUPPORT_FC15
+        case FC15_WRITE_MULTIPLE_COILS:
+            if (!ms->writeCoil) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+            ref = pp_u16(&pp);
+            count = pp_u16(&pp);
+            bytecount = pp_u8(&pp);
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (count > 255 || resp_capacity < 4 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            if (pp_remains(&pp) < bytecount + (tcp ? 0 : 2)) {
+                return MB_ERR_NEEDMORE;
+            }
+            pb_u16(&pb, ref);
+            pb_u16(&pb, count);
+            bitcnt = 8;
+            scratch = 0;
+            while (count-- > 0) {
+                if (bitcnt == 8) {
+                    scratch = pp_u8(&pp);
+                    bitcnt = 0;
+                }
+                exc = ms->writeCoil(ms, ref++, (scratch >> bitcnt) & 1);
+                bitcnt++;
+                if (exc != 0) {
+                    goto exception;
+                }
+            }
+            break;
+#endif
+
+#ifdef MB_SUPPORT_FC22
+        case FC22_MASK_WRITE_REGISTER:
+            if (!ms->writeHolding || !ms->readHolding) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+            ref = pp_u16(&pp);
+            and_mask = pp_u16(&pp);
+            or_mask = pp_u16(&pp);
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (resp_capacity < 4 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            exc = ms->readHolding(ms, ref, &value);
+            if (exc != 0) {
+                goto exception;
+            }
+
+            value = (value & and_mask) | (or_mask & ~and_mask);
+            exc = ms->writeHolding(ms, ref, value);
+            if (exc != 0) {
+                goto exception;
+            }
+
+            pb_u16(&pb, ref);
+            pb_u16(&pb, and_mask);
+            pb_u16(&pb, or_mask);
+            break;
+#endif
+
+#ifdef MB_SUPPORT_FC23
+        case FC23_READ_WRITE_MULTIPLE_REGISTERS:
+            if (!ms->writeHolding || !ms->readHolding) {
+                exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+                goto exception;
+            }
+            // read
+            ref = pp_u16(&pp);
+            count = pp_u16(&pp);
+            // write
+            ref2 = pp_u16(&pp);
+            count2 = pp_u16(&pp);
+            pp_skip(&pp, 1); // qty of bytes
+            if (!pp_ok(&pp)) {
+                return MB_ERR_NEEDMORE;
+            }
+            if (count > 255 || count2 > 255 || resp_capacity < count * 2 + overhead) {
+                exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
+                goto exception;
+            }
+            if (pp_remains(&pp) < count2 * 2 + (tcp ? 0 : 2)) {
+                return MB_ERR_NEEDMORE;
+            }
+            pb_u8(&pb, 2 * count);
+            // First, write
+            while (count2-- > 0) {
+                value = pp_u16(&pp);
+                exc = ms->writeHolding(ms, ref2++, value);
+                if (exc != 0) {
+                    goto exception;
+                }
+            }
+            // Second, read
+            while (count-- > 0) {
+                exc = ms->readHolding(ms, ref++, &value);
+                if (exc != 0) {
+                    goto exception;
+                }
+                pb_u16(&pb, value);
+            }
+            break;
+#endif
+
+        default:
+            exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
+            goto exception;
+    }
+    goto checksum;
+
+    exception:;
+    *resp_fc_mark |= 0x80;
+    pb_restore(&pb, resp_pld_start_mark);
+    pb_u8(&pb, (uint8_t) exc);
+    goto checksum;
+
+    checksum:
+    numbytes = pb_length(&pb);
+    if (tcp) {
+        pb_restore(&pb, resp_len_mark);
+        pb_u16(&pb, numbytes - 6);
+        *resp_size = numbytes;
+    } else {
+        uint8_t *m = pb.start;
+        uint16_t crc = crc16_init();
+        *resp_size = numbytes + 2;
+        while (numbytes > 0) {
+            crc = crc16_update(crc, *m++);
+            numbytes--;
+        }
+        pb.bigendian = 0; // CRC is sent as little endian?
+        pb_u16(&pb, crc);
+    }
+
+    if (!pb_ok(&pb)) {
+        return MB_ERROR;
+    }
+
+    if (ms->endOfAccess) {
+        ms->endOfAccess(ms);
+    }
+    return MB_OK;
+}

components/modbus_slave/src/pp/payload_builder.c

@@ -0,0 +1,87 @@
+#include <string.h>
+#include "pp/payload_builder.h"
+
+#define pb_check_capacity(pb, needed)                                                   \
+    if ((pb)->current + (needed) > (pb)->end) {                                         \
+        if ((pb)->full_handler == NULL || !(pb)->full_handler((pb), (needed))) {        \
+            (pb)->ok = 0;                                                               \
+        }                                                                               \
+    }
+
+/** Write from a buffer */
+bool pb_buf(PayloadBuilder *pb, const uint8_t *buf, uint32_t len)
+{
+    pb_check_capacity(pb, len);
+    if (!pb->ok) { return false; }
+
+    memcpy(pb->current, buf, len);
+    pb->current += len;
+    return true;
+}
+
+/** Write s zero terminated string */
+bool pb_string(PayloadBuilder *pb, const char *str)
+{
+    uint32_t len = (uint32_t) strlen(str);
+    pb_check_capacity(pb, len + 1);
+    if (!pb->ok) {
+        return false;
+    }
+
+    memcpy(pb->current, str, len + 1);
+    pb->current += len + 1;
+    return true;
+}
+
+/** Write uint8_t to the buffer */
+bool pb_u8(PayloadBuilder *pb, uint8_t byte)
+{
+    pb_check_capacity(pb, 1);
+    if (!pb->ok) {
+        return false;
+    }
+
+    *pb->current++ = byte;
+    return true;
+}
+
+/** Write uint16_t to the buffer. */
+bool pb_u16(PayloadBuilder *pb, uint16_t word)
+{
+    pb_check_capacity(pb, 2);
+    if (!pb->ok) {
+        return false;
+    }
+
+    if (pb->bigendian) {
+        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
+        *pb->current++ = (uint8_t) (word & 0xFF);
+    } else {
+        *pb->current++ = (uint8_t) (word & 0xFF);
+        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
+    }
+    return true;
+}
+
+/** Write uint32_t to the buffer. */
+bool pb_u32(PayloadBuilder *pb, uint32_t word)
+{
+    pb_check_capacity(pb, 4);
+    if (!pb->ok) {
+        return false;
+    }
+
+    if (pb->bigendian) {
+        *pb->current++ = (uint8_t) ((word >> 24) & 0xFF);
+        *pb->current++ = (uint8_t) ((word >> 16) & 0xFF);
+        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
+        *pb->current++ = (uint8_t) (word & 0xFF);
+    } else {
+        *pb->current++ = (uint8_t) (word & 0xFF);
+        *pb->current++ = (uint8_t) ((word >> 8) & 0xFF);
+        *pb->current++ = (uint8_t) ((word >> 16) & 0xFF);
+        *pb->current++ = (uint8_t) ((word >> 24) & 0xFF);
+    }
+    return true;
+}
+

components/modbus_slave/src/pp/payload_parser.c

@@ -0,0 +1,104 @@
+#include "pp/payload_parser.h"
+
+#define pp_check_capacity(pp, needed)                                               \
+    if ((pp)->current + (needed) > (pp)->end) {                                     \
+        if ((pp)->empty_handler == NULL || !(pp)->empty_handler((pp), (needed))) {  \
+            (pp)->ok = 0;                                                           \
+        }                                                                           \
+    }
+
+uint8_t pp_u8(PayloadParser *pp)
+{
+    pp_check_capacity(pp, 1);
+    if (!pp->ok) {
+        return 0;
+    }
+
+    return *pp->current++;
+}
+
+uint16_t pp_u16(PayloadParser *pp)
+{
+    pp_check_capacity(pp, 2);
+    if (!pp->ok) {
+        return 0;
+    }
+
+    uint16_t x = 0;
+
+    if (pp->bigendian) {
+        x |= *pp->current++ << 8;
+        x |= *pp->current++;
+    } else {
+        x |= *pp->current++;
+        x |= *pp->current++ << 8;
+    }
+    return x;
+}
+
+uint32_t pp_u32(PayloadParser *pp)
+{
+    pp_check_capacity(pp, 4);
+    if (!pp->ok) {
+        return 0;
+    }
+
+    uint32_t x = 0;
+
+    if (pp->bigendian) {
+        x |= (uint32_t) (*pp->current++ << 24);
+        x |= (uint32_t) (*pp->current++ << 16);
+        x |= (uint32_t) (*pp->current++ << 8);
+        x |= *pp->current++;
+    } else {
+        x |= *pp->current++;
+        x |= (uint32_t) (*pp->current++ << 8);
+        x |= (uint32_t) (*pp->current++ << 16);
+        x |= (uint32_t) (*pp->current++ << 24);
+    }
+    return x;
+}
+
+const uint8_t *pp_tail(PayloadParser *pp, uint32_t *length)
+{
+    int32_t len = (int) (pp->end - pp->current);
+    if (!pp->ok || len <= 0) {
+        if (length != NULL) {
+            *length = 0;
+        }
+        return NULL;
+    }
+
+    if (length != NULL) {
+        *length = (uint32_t) len;
+    }
+
+    return pp->current;
+}
+
+/** Read a zstring */
+uint32_t pp_string(PayloadParser *pp, char *buffer, uint32_t maxlen)
+{
+    pp_check_capacity(pp, 1);
+    uint32_t len = 0;
+    while (len < maxlen - 1 && pp->current != pp->end) {
+        char c = *buffer++ = (char) *pp->current++;
+        if (c == 0) {
+            break;
+        }
+        len++;
+    }
+    *buffer = 0;
+    return len;
+}
+
+/** Read a buffer */
+uint32_t pp_buf(PayloadParser *pp, uint8_t *buffer, uint32_t maxlen)
+{
+    uint32_t len = 0;
+    while (len < maxlen && pp->current != pp->end) {
+        *buffer++ = *pp->current++;
+        len++;
+    }
+    return len;
+}

main/CMakeLists.txt

@@ -5,6 +5,7 @@ idf_component_register(SRCS
     sntp_cli.c
     utils.c
     wifi_conn.c
+    mbiface.c
     console/console_ioimpl.c
     console/console_server.c
     console/register_cmds.c

main/console/console_server.c

@@ -72,7 +72,7 @@ static void telnetsrv_handle(TcpdClient_t client, char *buf, int nbytes)
 /**
  * Socket read handler
  */
-esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
+static esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
 {
     char buf[64];
     int nbytes = read(sockfd, buf, sizeof(buf));

main/fanctl_main.c

@@ -19,6 +19,7 @@
 
 #include "console/register_cmds.h"
 #include "irblast.h"
+#include "mbiface.h"
 
 static const char *TAG = "main";
 
@@ -55,6 +56,8 @@ void app_main(void) {
     console_setup_uart_stdio();
     ESP_ERROR_CHECK(console_start_stdio(NULL, NULL));
 
+    mbiface_setup();
+
     telnetsrv_start(CONSOLE_TELNET_PORT);
     ESP_LOGI(TAG, "Startup finished, free heap = %u, cmds %"PRIu32, esp_get_free_heap_size(), console_count_commands());
 }

main/mbiface.c

@@ -0,0 +1,88 @@
+#include <esp_log.h>
+#include "mbiface.h"
+#include "socket_server.h"
+#include "tasks.h"
+#include "modbus.h"
+
+static const char * TAG = "mb";
+
+Tcpd_t g_mbifc_server = NULL;
+ModbusSlave_t g_modbus;
+static volatile uint16_t register2 = 0;
+
+/**
+ * Socket read handler
+ */
+static esp_err_t read_fn(Tcpd_t serv, TcpdClient_t client, int sockfd)
+{
+    uint8_t buf[1024];
+    uint8_t buf2[1024];
+    int nbytes = read(sockfd, buf, sizeof(buf));
+    if (nbytes <= 0) return ESP_FAIL;
+
+    size_t resp_len = 0;
+    ModbusError_t e = mb_handleRequest(&g_modbus, buf, nbytes, buf2, 1024, &resp_len);
+    if (e == 0) {
+        tcpd_send(client, buf2, (ssize_t) resp_len);
+    } else {
+        ESP_LOGE(TAG, "Error %d, closing socket", e);
+        tcpd_kick(client);
+    }
+
+    return ESP_OK;
+}
+
+ModbusException_t startOfAccess(ModbusSlave_t *pSlave, ModbusFunction_t function, uint8_t i) {
+    return 0;
+}
+
+void endOfAccess(ModbusSlave_t *ms) {
+    //
+}
+
+ModbusException_t rh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t *pValue) {
+    ESP_LOGD(TAG, "Read holding %d", ref);
+
+    if (ref == 1) {
+        *pValue = 1042; // device ID
+        return 0;
+    }
+    if (ref == 2) {
+        *pValue = register2;
+        return 0;
+    }
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+}
+
+ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t ref, uint16_t value) {
+    ESP_LOGD(TAG, "Write holding %d := %02x", ref, value);
+
+    if (ref == 2) {
+        register2 = value;
+        return 0;
+    }
+
+    return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
+}
+
+void mbiface_setup()
+{
+    ESP_LOGI(TAG, "initing MB iface");
+    g_modbus.proto = MB_PROTO_TCP;
+    g_modbus.addr = 1;
+    g_modbus.startOfAccess = startOfAccess;
+    g_modbus.endOfAccess = endOfAccess;
+    g_modbus.readHolding = rh;
+    g_modbus.writeHolding = wh;
+
+    tcpd_config_t server_config = TCPD_INIT_DEFAULT();
+    server_config.max_clients = 1;
+    server_config.task_prio = MBIFC_TASK_PRIO;
+    server_config.task_stack = MBIFC_TASK_STACK;
+    server_config.task_name = "MBIFC";
+    server_config.port = 502;
+    server_config.read_fn = read_fn;
+
+    ESP_ERROR_CHECK(tcpd_init(&server_config, &g_mbifc_server));
+}

main/mbiface.h

@@ -0,0 +1,16 @@
+/**
+ * Modbus interface
+ */
+
+#ifndef mbiface_H
+#define mbiface_H
+
+#include "socket_server.h"
+#include "modbus.h"
+
+extern Tcpd_t g_mbifc_server;
+extern ModbusSlave_t g_modbus;
+
+void mbiface_setup();
+
+#endif //mbiface_H

main/tasks.h

@@ -15,4 +15,7 @@
 #define TELNET_TASK_STACK  4096
 #define TELNET_TASK_PRIO   PRIO_LOW
 
+#define MBIFC_TASK_STACK  4096
+#define MBIFC_TASK_PRIO   PRIO_LOW
+
 #endif //CSPEMU_PRIORITIES_H