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

improvements

Browse Source
master
Ondřej Hruška 2 years ago
parent ab646c3a47
commit c1b6566cda
6 changed files with 108 additions and 16 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. 84
      src/modbus.c
  2. 32
      src/modbus.h
  3. 2
      src/pp/payload_builder.c
  4. 2
      src/pp/payload_builder.h
  5. 2
      src/pp/payload_parser.c
  6. 2
      src/pp/payload_parser.h

84
src/modbus.c
Unescape View File

@ -40,15 +40,44 @@ ModbusError_t mb_handleRequest(
size_t *resp_size 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; 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; ModbusException_t exc = MB_EXCEPTION_OK;
size_t numbytes; size_t numbytes;
uint8_t fcx, bytecount, bitcnt, scratch; uint8_t fcx;
const size_t TCP_RESP_OVERHEAD = 8; const size_t TCP_RESP_OVERHEAD = 8;
const size_t RTU_RESP_OVERHEAD = 4; const size_t RTU_RESP_OVERHEAD = 4;
const size_t overhead = ms->proto == MB_PROTO_TCP ? TCP_RESP_OVERHEAD : RTU_RESP_OVERHEAD; 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; const bool tcp = ms->proto == MB_PROTO_TCP;
PayloadParser pp = pp_start_be(req, req_size, NULL); PayloadParser pp = pp_start_be(req, req_size, NULL);
@ -58,12 +87,11 @@ ModbusError_t mb_handleRequest(
/* Parse header */ /* Parse header */
txn_id = pp_u16(&pp); txn_id = pp_u16(&pp);
protocol_id = pp_u16(&pp); protocol_id = pp_u16(&pp);
msglen = pp_u16(&pp); pp_skip(&pp, 2); // msglen
if (protocol_id != 0) { if (protocol_id != 0) {
return MB_ERR_BADPROTO; return MB_ERR_BADPROTO;
} }
} else { } else {
msglen = req_size;
/* check CRC */ /* check CRC */
uint16_t crc = crc16_init(); uint16_t crc = crc16_init();
for (int pos = 0; pos < req_size /* size of CRC */; pos++) { for (int pos = 0; pos < req_size /* size of CRC */; pos++) {
@ -108,6 +136,7 @@ ModbusError_t mb_handleRequest(
} }
switch (fcx) { switch (fcx) {
#ifdef MB_SUPPORT_FC05
case FC05_WRITE_SINGLE_COIL: case FC05_WRITE_SINGLE_COIL:
if (!ms->writeCoil) { if (!ms->writeCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -133,17 +162,24 @@ ModbusError_t mb_handleRequest(
pb_u16(&pb, ref); pb_u16(&pb, ref);
pb_u16(&pb, value); pb_u16(&pb, value);
break; break;
#endif
#if defined(MB_SUPPORT_FC01) || defined(MB_SUPPORT_FC02)
case FC01_READ_COILS: case FC01_READ_COILS:
case FC02_READ_DISCRETES: case FC02_READ_DISCRETES:
/* check we have the needed function */ /* check we have the needed function */
#ifdef MB_SUPPORT_FC01
if (fcx == FC01_READ_COILS && !ms->readCoil) { if (fcx == FC01_READ_COILS && !ms->readCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception; goto exception;
} else if (!ms->readDiscrete) { }
#endif
#ifdef MB_SUPPORT_FC02
if (fcx == FC02_READ_DISCRETES && !ms->readDiscrete) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception; goto exception;
} }
#endif
ref = pp_u16(&pp); ref = pp_u16(&pp);
count = pp_u16(&pp); count = pp_u16(&pp);
if (!pp_ok(&pp)) { if (!pp_ok(&pp)) {
@ -158,11 +194,16 @@ ModbusError_t mb_handleRequest(
bitcnt = 0; bitcnt = 0;
scratch = 0; scratch = 0;
while (count-- > 0) { while (count-- > 0) {
#ifdef MB_SUPPORT_FC01
if (fcx == FC01_READ_COILS) { if (fcx == FC01_READ_COILS) {
exc = ms->readCoil(ms, ref++, &bvalue); exc = ms->readCoil(ms, ref++, &bvalue);
} else { }
#endif
#ifdef MB_SUPPORT_FC02
if (fcx == FC02_READ_DISCRETES) {
exc = ms->readDiscrete(ms, ref++, &bvalue); exc = ms->readDiscrete(ms, ref++, &bvalue);
} }
#endif
if (exc != 0) { if (exc != 0) {
goto exception; goto exception;
} }
@ -178,18 +219,24 @@ ModbusError_t mb_handleRequest(
pb_u8(&pb, scratch); pb_u8(&pb, scratch);
} }
break; break;
#endif
#if defined(MB_SUPPORT_FC03) || defined(MB_SUPPORT_FC04)
case FC03_READ_HOLDING_REGISTERS: case FC03_READ_HOLDING_REGISTERS:
case FC04_READ_INPUT_REGISTERS: case FC04_READ_INPUT_REGISTERS:
#ifdef MB_SUPPORT_FC03
/* check we have the needed function */ /* check we have the needed function */
if (fcx == FC03_READ_HOLDING_REGISTERS && !ms->readHolding) { if (fcx == FC03_READ_HOLDING_REGISTERS && !ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception; goto exception;
} else if (!ms->readInput) { }
#endif
#ifdef MB_SUPPORT_FC04
if (fcx == FC04_READ_INPUT_REGISTERS && !ms->readInput) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception; goto exception;
} }
#endif
ref = pp_u16(&pp); ref = pp_u16(&pp);
count = pp_u16(&pp); count = pp_u16(&pp);
if (!pp_ok(&pp)) { if (!pp_ok(&pp)) {
@ -201,18 +248,25 @@ ModbusError_t mb_handleRequest(
} }
pb_u8(&pb, count*2); pb_u8(&pb, count*2);
while (count-- > 0) { while (count-- > 0) {
#ifdef MB_SUPPORT_FC03
if (fcx == FC03_READ_HOLDING_REGISTERS) { if (fcx == FC03_READ_HOLDING_REGISTERS) {
exc = ms->readHolding(ms, ref++, &value); exc = ms->readHolding(ms, ref++, &value);
} else { }
#endif
#ifdef MB_SUPPORT_FC04
if (fcx == FC04_READ_INPUT_REGISTERS) {
exc = ms->readInput(ms, ref++, &value); exc = ms->readInput(ms, ref++, &value);
} }
#endif
if (exc != 0) { if (exc != 0) {
goto exception; goto exception;
} }
pb_u16(&pb, value); pb_u16(&pb, value);
} }
break; break;
#endif
#ifdef MB_SUPPORT_FC06
case FC06_WRITE_SINGLE_REGISTER: case FC06_WRITE_SINGLE_REGISTER:
if (!ms->writeHolding) { if (!ms->writeHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -234,7 +288,9 @@ ModbusError_t mb_handleRequest(
pb_u16(&pb, ref); pb_u16(&pb, ref);
pb_u16(&pb, value); pb_u16(&pb, value);
break; break;
#endif
#ifdef MB_SUPPORT_FC16
case FC16_WRITE_MULTIPLE_REGISTERS: case FC16_WRITE_MULTIPLE_REGISTERS:
if (!ms->writeHolding) { if (!ms->writeHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -263,7 +319,9 @@ ModbusError_t mb_handleRequest(
} }
} }
break; break;
#endif
#ifdef MB_SUPPORT_FC15
case FC15_WRITE_MULTIPLE_COILS: case FC15_WRITE_MULTIPLE_COILS:
if (!ms->writeCoil) { if (!ms->writeCoil) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -298,7 +356,9 @@ ModbusError_t mb_handleRequest(
} }
} }
break; break;
#endif
#ifdef MB_SUPPORT_FC22
case FC22_MASK_WRITE_REGISTER: case FC22_MASK_WRITE_REGISTER:
if (!ms->writeHolding || !ms->readHolding) { if (!ms->writeHolding || !ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -329,7 +389,9 @@ ModbusError_t mb_handleRequest(
pb_u16(&pb, and_mask); pb_u16(&pb, and_mask);
pb_u16(&pb, or_mask); pb_u16(&pb, or_mask);
break; break;
#endif
#ifdef MB_SUPPORT_FC23
case FC23_READ_WRITE_MULTIPLE_REGISTERS: case FC23_READ_WRITE_MULTIPLE_REGISTERS:
if (!ms->writeHolding || !ms->readHolding) { if (!ms->writeHolding || !ms->readHolding) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -370,6 +432,7 @@ ModbusError_t mb_handleRequest(
pb_u16(&pb, value); pb_u16(&pb, value);
} }
break; break;
#endif
default: default:
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
@ -386,7 +449,6 @@ ModbusError_t mb_handleRequest(
checksum: checksum:
numbytes = pb_length(&pb); numbytes = pb_length(&pb);
if (tcp) { if (tcp) {
pb_mark_t end = pb_save(&pb);
pb_restore(&pb, resp_len_mark); pb_restore(&pb, resp_len_mark);
pb_u16(&pb, numbytes - 6); pb_u16(&pb, numbytes - 6);
*resp_size = numbytes; *resp_size = numbytes;

32
src/modbus.h
Unescape View File

@ -1,5 +1,5 @@
/** /**
* TODO file description * Modbus slave
*/ */
#ifndef MODBUS_H #ifndef MODBUS_H
@ -9,6 +9,18 @@
#include <stdbool.h> #include <stdbool.h>
#include <sys/types.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 { typedef enum ModbusException {
MB_EXCEPTION_OK = 0, MB_EXCEPTION_OK = 0,
MB_EXCEPTION_ILLEGAL_FUNCTION = 1, MB_EXCEPTION_ILLEGAL_FUNCTION = 1,
@ -53,12 +65,30 @@ struct ModbusSlave {
void *userData; void *userData;
ModbusException_t (*startOfAccess)(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id); ModbusException_t (*startOfAccess)(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id);
void (*endOfAccess)(ModbusSlave_t *ms); void (*endOfAccess)(ModbusSlave_t *ms);
#ifdef MB_SUPPORT_FC01
ModbusException_t (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value); 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); 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); 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); 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); 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); ModbusException_t (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value);
#endif
}; };
ModbusError_t mb_handleRequest( ModbusError_t mb_handleRequest(

2
src/pp/payload_builder.c
Unescape View File

@ -1,5 +1,5 @@
#include <string.h> #include <string.h>
#include "payload_builder.h" #include "pp/payload_builder.h"
#define pb_check_capacity(pb, needed) \ #define pb_check_capacity(pb, needed) \
if ((pb)->current + (needed) > (pb)->end) { \ if ((pb)->current + (needed) > (pb)->end) { \

2
src/pp/payload_builder.h
Unescape View File

@ -4,7 +4,7 @@
/** /**
* PayloadBuilder, part of the TinyFrame utilities collection * 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 * The builder supports big and little endian which is selected when
* initializing it or by accessing the bigendian struct field. * initializing it or by accessing the bigendian struct field.

2
src/pp/payload_parser.c
Unescape View File

@ -1,4 +1,4 @@
#include "payload_parser.h" #include "pp/payload_parser.h"
#define pp_check_capacity(pp, needed) \ #define pp_check_capacity(pp, needed) \
if ((pp)->current + (needed) > (pp)->end) { \ if ((pp)->current + (needed) > (pp)->end) { \

2
src/pp/payload_parser.h
Unescape View File

@ -4,7 +4,7 @@
/** /**
* PayloadParser, part of the TinyFrame utilities collection * 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). * This module helps you with parsing payloads (not only from TinyFrame).
* *

Write Preview
Loading…
Cancel
Save