electro
/
esp-recuperator
forked from electro/esp-irblaster
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

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

Browse Source
master
Ondřej Hruška 2 years ago
parent c83042658e
commit 3efa1b6665
16 changed files with 1220 additions and 1 deletions
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Show Stats Download Patch File Download Diff File
  1. 7
      components/modbus_slave/CMakeLists.txt
  2. 1
      components/modbus_slave/README.txt
  3. 3
      components/modbus_slave/component.mk
  4. 103
      components/modbus_slave/include/modbus.h
  5. 129
      components/modbus_slave/include/pp/payload_builder.h
  6. 167
      components/modbus_slave/include/pp/payload_parser.h
  7. 32
      components/modbus_slave/include/pp/type_coerce.h
  8. 475
      components/modbus_slave/src/modbus.c
  9. 87
      components/modbus_slave/src/pp/payload_builder.c
  10. 104
      components/modbus_slave/src/pp/payload_parser.c
  11. 1
      main/CMakeLists.txt
  12. 2
      main/console/console_server.c
  13. 3
      main/fanctl_main.c
  14. 88
      main/mbiface.c
  15. 16
      main/mbiface.h
  16. 3
      main/tasks.h

7
components/modbus_slave/CMakeLists.txt
Unescape View File

@ -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()

1
components/modbus_slave/README.txt
Unescape View File

@ -0,0 +1 @@
modbus slave

3
components/modbus_slave/component.mk
Unescape View File

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

103
components/modbus_slave/include/modbus.h
Unescape View File

@ -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

129
components/modbus_slave/include/pp/payload_builder.h
Unescape View File

@ -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

167
components/modbus_slave/include/pp/payload_parser.h
Unescape View File

@ -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

32
components/modbus_slave/include/pp/type_coerce.h
Unescape View File

@ -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

475
components/modbus_slave/src/modbus.c
Unescape View File

@ -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;
}

87
components/modbus_slave/src/pp/payload_builder.c
Unescape View File

@ -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;
}

104
components/modbus_slave/src/pp/payload_parser.c
Unescape View File

@ -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;
}

1
main/CMakeLists.txt
Unescape View File

@ -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

2
main/console/console_server.c
Unescape View File

@ -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));

3
main/fanctl_main.c
Unescape View File

@ -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());
}

88
main/mbiface.c
Unescape View File

@ -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));
}

16
main/mbiface.h
Unescape View File

@ -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

3
main/tasks.h
Unescape View File

@ -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

Write Preview
Loading…
Cancel
Save