electro/modbus-slave
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

improvements

Browse Source
This commit is contained in:
MightyPork
2022-08-19 23:14:16 +02:00
parent ab646c3a47
commit c1b6566cda
6 changed files with 108 additions and 16 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modbus.c
+73 -11
View File
@@ -40,15 +40,44 @@ ModbusError_t mb_handleRequest(
size_t *resp_size
)
{
uint16_t txn_id, msglen, protocol_id, ref, count, ref2, count2, value, and_mask, or_mask;
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, bytecount, bitcnt, scratch;
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, resp_len_mark, resp_pld_start_mark;
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);
@@ -58,12 +87,11 @@ ModbusError_t mb_handleRequest(
/* Parse header */
txn_id = pp_u16(&pp);
protocol_id = pp_u16(&pp);
msglen = pp_u16(&pp);
pp_skip(&pp, 2); // msglen
if (protocol_id != 0) {
return MB_ERR_BADPROTO;
}
} else {
msglen = req_size;
/* check CRC */
uint16_t crc = crc16_init();
for (int pos = 0; pos < req_size /* size of CRC */; pos++) {
@@ -108,6 +136,7 @@ ModbusError_t mb_handleRequest(
}
switch (fcx) {
#ifdef MB_SUPPORT_FC05
case FC05_WRITE_SINGLE_COIL:
if (!ms->writeCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -133,17 +162,24 @@ ModbusError_t mb_handleRequest(
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;
} else if (!ms->readDiscrete) {
}
#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)) {
@@ -158,11 +194,16 @@ ModbusError_t mb_handleRequest(
bitcnt = 0;
scratch = 0;
while (count-- > 0) {
#ifdef MB_SUPPORT_FC01
if (fcx == FC01_READ_COILS) {
exc = ms->readCoil(ms, ref++, &bvalue);
} else {
}
#endif
#ifdef MB_SUPPORT_FC02
if (fcx == FC02_READ_DISCRETES) {
exc = ms->readDiscrete(ms, ref++, &bvalue);
}
#endif
if (exc != 0) {
goto exception;
}
@@ -178,18 +219,24 @@ ModbusError_t mb_handleRequest(
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;
} else if (!ms->readInput) {
}
#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)) {
@@ -201,18 +248,25 @@ ModbusError_t mb_handleRequest(
}
pb_u8(&pb, count*2);
while (count-- > 0) {
#ifdef MB_SUPPORT_FC03
if (fcx == FC03_READ_HOLDING_REGISTERS) {
exc = ms->readHolding(ms, ref++, &value);
} else {
}
#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;
@@ -234,7 +288,9 @@ ModbusError_t mb_handleRequest(
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;
@@ -263,7 +319,9 @@ ModbusError_t mb_handleRequest(
}
}
break;
#endif
#ifdef MB_SUPPORT_FC15
case FC15_WRITE_MULTIPLE_COILS:
if (!ms->writeCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -298,7 +356,9 @@ ModbusError_t mb_handleRequest(
}
}
break;
#endif
#ifdef MB_SUPPORT_FC22
case FC22_MASK_WRITE_REGISTER:
if (!ms->writeHolding || !ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -329,7 +389,9 @@ ModbusError_t mb_handleRequest(
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;
@@ -370,6 +432,7 @@ ModbusError_t mb_handleRequest(
pb_u16(&pb, value);
}
break;
#endif
default:
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@@ -386,7 +449,6 @@ ModbusError_t mb_handleRequest(
checksum:
numbytes = pb_length(&pb);
if (tcp) {
pb_mark_t end = pb_save(&pb);
pb_restore(&pb, resp_len_mark);
pb_u16(&pb, numbytes - 6);
*resp_size = numbytes;
src/modbus.h
+31 -1
View File
@@ -1,5 +1,5 @@
/**
* TODO file description
* Modbus slave
*/
#ifndef MODBUS_H
@@ -9,6 +9,18 @@
#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,
@@ -53,12 +65,30 @@ struct ModbusSlave {
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(
src/pp/payload_builder.c
+1 -1
View File
@@ -1,5 +1,5 @@
#include <string.h>
#include "payload_builder.h"
#include "pp/payload_builder.h"
#define pb_check_capacity(pb, needed) \
if ((pb)->current + (needed) > (pb)->end) { \
src/pp/payload_builder.h
+1 -1
View File
@@ -4,7 +4,7 @@
/**
* PayloadBuilder, part of the TinyFrame utilities collection
*
* (c) Ondřej Hruška, 2014-2017. MIT license.
* (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.
src/pp/payload_parser.c
+1 -1
View File
@@ -1,4 +1,4 @@
#include "payload_parser.h"
#include "pp/payload_parser.h"
#define pp_check_capacity(pp, needed) \
if ((pp)->current + (needed) > (pp)->end) { \
src/pp/payload_parser.h
+1 -1
View File
@@ -4,7 +4,7 @@
/**
* PayloadParser, part of the TinyFrame utilities collection
*
* (c) Ondřej Hruška, 2016-2017. MIT license.
* (c) Ondřej Hruška, 2016-2022. MIT license.
*
* This module helps you with parsing payloads (not only from TinyFrame).
*