diff --git a/components/modbus_slave/CMakeLists.txt b/components/modbus_slave/CMakeLists.txt new file mode 100644 index 0000000..6c51679 --- /dev/null +++ b/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() diff --git a/components/modbus_slave/README.txt b/components/modbus_slave/README.txt new file mode 100644 index 0000000..06131ee --- /dev/null +++ b/components/modbus_slave/README.txt @@ -0,0 +1 @@ +modbus slave diff --git a/components/modbus_slave/component.mk b/components/modbus_slave/component.mk new file mode 100644 index 0000000..87ae05a --- /dev/null +++ b/components/modbus_slave/component.mk @@ -0,0 +1,3 @@ + +COMPONENT_SRCDIRS := src +COMPONENT_ADD_INCLUDEDIRS := include diff --git a/components/modbus_slave/include/modbus.h b/components/modbus_slave/include/modbus.h new file mode 100644 index 0000000..9258f57 --- /dev/null +++ b/components/modbus_slave/include/modbus.h @@ -0,0 +1,103 @@ +/** + * Modbus slave + */ + +#ifndef MODBUS_H +#define MODBUS_H + +#include +#include +#include + +// 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 diff --git a/components/modbus_slave/include/pp/payload_builder.h b/components/modbus_slave/include/pp/payload_builder.h new file mode 100644 index 0000000..fd4297b --- /dev/null +++ b/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 +#include +#include +#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 diff --git a/components/modbus_slave/include/pp/payload_parser.h b/components/modbus_slave/include/pp/payload_parser.h new file mode 100644 index 0000000..ebd8908 --- /dev/null +++ b/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 +#include +#include +#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 diff --git a/components/modbus_slave/include/pp/type_coerce.h b/components/modbus_slave/include/pp/type_coerce.h new file mode 100644 index 0000000..2e4d0c6 --- /dev/null +++ b/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 +#include + +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 diff --git a/components/modbus_slave/src/modbus.c b/components/modbus_slave/src/modbus.c new file mode 100644 index 0000000..ef20165 --- /dev/null +++ b/components/modbus_slave/src/modbus.c @@ -0,0 +1,475 @@ +#include +#include +#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; +} diff --git a/components/modbus_slave/src/pp/payload_builder.c b/components/modbus_slave/src/pp/payload_builder.c new file mode 100644 index 0000000..14afc9e --- /dev/null +++ b/components/modbus_slave/src/pp/payload_builder.c @@ -0,0 +1,87 @@ +#include +#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; +} + diff --git a/components/modbus_slave/src/pp/payload_parser.c b/components/modbus_slave/src/pp/payload_parser.c new file mode 100644 index 0000000..68be4f9 --- /dev/null +++ b/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; +} diff --git a/main/CMakeLists.txt b/main/CMakeLists.txt index bf7d008..426a1a4 100644 --- a/main/CMakeLists.txt +++ b/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 diff --git a/main/console/console_server.c b/main/console/console_server.c index 61f227e..35f588f 100644 --- a/main/console/console_server.c +++ b/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)); diff --git a/main/fanctl_main.c b/main/fanctl_main.c index 83737ac..d158452 100644 --- a/main/fanctl_main.c +++ b/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()); } diff --git a/main/mbiface.c b/main/mbiface.c new file mode 100644 index 0000000..0672c7b --- /dev/null +++ b/main/mbiface.c @@ -0,0 +1,88 @@ +#include +#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)); +} diff --git a/main/mbiface.h b/main/mbiface.h new file mode 100644 index 0000000..eb7697f --- /dev/null +++ b/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 diff --git a/main/tasks.h b/main/tasks.h index 1021c05..e43d242 100644 --- a/main/tasks.h +++ b/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