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

2022-08-19 23:56:16 +02:00
parent c83042658e
commit 3efa1b6665
16 changed files with 1220 additions and 1 deletions
@@ -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
@@ -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
@@ -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
@@ -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