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

fixes

Browse Source
master
Ondřej Hruška 2 years ago
parent 81f6c8f2e9
commit ab646c3a47
4 changed files with 233 additions and 46 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. 185
      src/main.c
  2. 69
      src/modbus.c
  3. 22
      src/modbus.h
  4. 3
      src/pp/payload_builder.h

185
src/main.c
Unescape View File

@ -1,8 +1,191 @@
#include <stdio.h> #include <stdio.h>
#include "modbus.h" #include "modbus.h"
/* https://gist.github.com/ccbrown/9722406 */
void hexdump(const void *data, size_t size)
{
char ascii[17];
size_t i, j;
ascii[16] = '\0';
for (i = 0; i < size; ++i) {
printf("%02X ", ((unsigned char *) data)[i]);
const uint8_t c = ((unsigned char *) data)[i];
if (c >= ' ' && c <= '~') {
ascii[i % 16] = (char) c;
} else {
ascii[i % 16] = '.';
}
if ((i + 1) % 8 == 0 || i + 1 == size) {
printf(" ");
if ((i + 1) % 16 == 0) {
printf("| %s \n", ascii);
} else if (i + 1 == size) {
ascii[(i + 1) % 16] = '\0';
if ((i + 1) % 16 <= 8) {
printf(" ");
}
for (j = (i + 1) % 16; j < 16; ++j) {
printf(" ");
}
printf("| %s \n", ascii);
}
}
}
}
static ssize_t parsehex(const char *s, uint8_t *out, size_t cap)
{
uint8_t buf = 0;
uint8_t v;
bool first = true;
char c;
size_t sz = 0;
while (0 != (c = *s++)) {
if (c >= '0' && c <= '9') {
v = c - '0';
} else if (c >= 'a' && c <= 'f') {
v = 10 + (c - 'a');
} else if (c >= 'A' && c <= 'F') {
v = 10 + (c - 'A');
} else {
continue;
}
if (first) {
buf |= (v & 15) << 4;
first = false;
} else {
buf |= (v & 15);
if (cap == 0) {
return -1;
}
out[sz++] = buf;
buf = 0;
first = true;
cap--;
}
}
return (ssize_t) sz;
}
ModbusException_t soa(ModbusSlave_t *ms, ModbusFunction_t fcx, uint8_t slave_id) {
printf("Start of access: fc%02d, slave %d\n", fcx, slave_id);
return 0;
}
void eoa(ModbusSlave_t *ms) {
printf("End of access\n");
}
ModbusException_t rh(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
printf("rh %d\n", i);
switch (i) {
case 107:
*out = 0xAE41;
break;
case 108:
*out = 0x5652;
break;
case 109:
*out = 0x4340;
break;
default:
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}
return 0;
}
ModbusException_t ri(ModbusSlave_t *ms, uint16_t i, uint16_t *out) {
printf("ri %d\n", i);
switch (i) {
case 8:
*out = 0xa;
break;
default:
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}
return 0;
}
ModbusException_t rc(ModbusSlave_t *ms, uint16_t i, bool *out) {
uint8_t store[] = {
0b11001101,
0b01101011,
0b10110010,
0b00001110,
0b00011011,
};
if (i >= 19 && i <= 55) {
int pos = i-19;
*out = 0 != (store[pos/8] & (1 << (pos%8)));
} else {
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}
return 0;
}
ModbusException_t rd(ModbusSlave_t *ms, uint16_t i, bool *out) {
uint8_t store[] = {
0b10101100,
0b11011011,
0b00110101,
};
if (i >= 196 && i <= 217) {
int pos = i - 196;
*out = 0 != (store[pos/8] & (1 << (pos%8)));
} else {
return MB_EXCEPTION_ILLEGAL_DATA_ADDRESS;
}
return 0;
}
ModbusException_t wc(ModbusSlave_t *pSlave, uint16_t i, bool b) {
printf("Write coil %d <- %d\n", i, b);
return 0;
}
ModbusException_t wh(ModbusSlave_t *pSlave, uint16_t i, uint16_t i1) {
printf("Write reg %d <- %d\n", i, i1);
return 0;
}
int main() int main()
{ {
printf("Hello, World!\n"); #define buflen 1024
uint8_t buf[buflen];
uint8_t buf2[buflen];
// 11 03 006B 0003 7687
// 11 04 0008 0001 B298
// 11 01 0013 0025 0E84
// 11 02 00C4 0016 BAA9
// 11 05 00AC FF00 4E8B
// 11 06 0001 0003 9A9B
// 11 0F 0013 000A 02 CD01 BF0B
// 11 10 0001 0002 04 000A 0102 C6F0
ssize_t pldlen = parsehex("11 03 006B 0003 7687", buf, buflen);
ModbusSlave_t ms = {
.addr = 17,
.proto = MB_PROTO_RTU,
.startOfAccess = soa,
.endOfAccess = eoa,
.readHolding = rh,
.readInput = ri,
.readCoil = rc,
.readDiscrete = rd,
.writeCoil = wc,
.writeHolding = wh,
};
printf("Req:\n");
hexdump(buf, pldlen);
size_t respsize;
ModbusError_t e = mb_handleRequest(&ms, buf, pldlen, buf2, buflen, &respsize);
if (e) {
printf("Err %d\n", e);
} else {
printf("Resp:\n");
hexdump(buf2, respsize);
}
return 0; return 0;
} }

69
src/modbus.c
Unescape View File

@ -51,8 +51,8 @@ ModbusError_t mb_handleRequest(
pb_mark_t resp_fc_mark, resp_len_mark, resp_pld_start_mark; pb_mark_t resp_fc_mark, resp_len_mark, resp_pld_start_mark;
const bool tcp = ms->proto == MB_PROTO_TCP; const bool tcp = ms->proto == MB_PROTO_TCP;
PayloadParser pp = pp_start_le(req, req_size, NULL); PayloadParser pp = pp_start_be(req, req_size, NULL);
PayloadBuilder pb = pb_start_le(resp, resp_capacity, NULL); PayloadBuilder pb = pb_start_be(resp, resp_capacity, NULL);
if (tcp) { if (tcp) {
/* Parse header */ /* Parse header */
@ -66,10 +66,10 @@ ModbusError_t mb_handleRequest(
msglen = req_size; msglen = req_size;
/* check CRC */ /* check CRC */
uint16_t crc = crc16_init(); uint16_t crc = crc16_init();
for (int pos = 0; pos < req_size - 2 /* size of CRC */; pos++) { for (int pos = 0; pos < req_size /* size of CRC */; pos++) {
crc = crc16_update(crc, pp_u8(&pp)); crc = crc16_update(crc, pp_u8(&pp));
} }
if (crc != pp_u16(&pp)) { if (crc != 0) {
return MB_ERR_CHECKSUM; return MB_ERR_CHECKSUM;
} }
pp_rewind(&pp); pp_rewind(&pp);
@ -137,16 +137,12 @@ ModbusError_t mb_handleRequest(
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 */
if (fcx == FC01_READ_COILS) { if (fcx == FC01_READ_COILS && !ms->readCoil) {
if (!ms->readCoil) { exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; goto exception;
goto exception; } else if (!ms->readDiscrete) {
} exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
} else { goto exception;
if (!ms->readDiscrete) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
} }
ref = pp_u16(&pp); ref = pp_u16(&pp);
count = pp_u16(&pp); count = pp_u16(&pp);
@ -170,7 +166,7 @@ ModbusError_t mb_handleRequest(
if (exc != 0) { if (exc != 0) {
goto exception; goto exception;
} }
scratch = (scratch << 1) | bvalue; scratch |= ((bvalue & 1) << bitcnt);
bitcnt++; bitcnt++;
if (bitcnt == 8) { if (bitcnt == 8) {
pb_u8(&pb, scratch); pb_u8(&pb, scratch);
@ -178,21 +174,20 @@ ModbusError_t mb_handleRequest(
bitcnt = 0; bitcnt = 0;
} }
} }
if (bitcnt != 0) {
pb_u8(&pb, scratch);
}
break; break;
case FC03_READ_HOLDING_REGISTERS: case FC03_READ_HOLDING_REGISTERS:
case FC04_READ_INPUT_REGISTERS: case FC04_READ_INPUT_REGISTERS:
/* check we have the needed function */ /* check we have the needed function */
if (fcx == FC03_READ_HOLDING_REGISTERS) { if (fcx == FC03_READ_HOLDING_REGISTERS && !ms->readHolding) {
if (!ms->readHolding) { exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
exc = MB_EXCEPTION_ILLEGAL_FUNCTION; goto exception;
goto exception; } else if (!ms->readInput) {
} exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
} else { goto exception;
if (!ms->readInput) {
exc = MB_EXCEPTION_ILLEGAL_FUNCTION;
goto exception;
}
} }
ref = pp_u16(&pp); ref = pp_u16(&pp);
@ -204,6 +199,7 @@ ModbusError_t mb_handleRequest(
exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO? exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO?
goto exception; goto exception;
} }
pb_u8(&pb, count*2);
while (count-- > 0) { while (count-- > 0) {
if (fcx == FC03_READ_HOLDING_REGISTERS) { if (fcx == FC03_READ_HOLDING_REGISTERS) {
exc = ms->readHolding(ms, ref++, &value); exc = ms->readHolding(ms, ref++, &value);
@ -257,6 +253,8 @@ ModbusError_t mb_handleRequest(
if (pp_remains(&pp) < count * 2 + (tcp ? 0 : 2)) { if (pp_remains(&pp) < count * 2 + (tcp ? 0 : 2)) {
return MB_ERR_NEEDMORE; return MB_ERR_NEEDMORE;
} }
pb_u16(&pb, ref);
pb_u16(&pb, count);
while (count-- > 0) { while (count-- > 0) {
value = pp_u16(&pp); value = pp_u16(&pp);
exc = ms->writeHolding(ms, ref++, value); exc = ms->writeHolding(ms, ref++, value);
@ -264,8 +262,6 @@ ModbusError_t mb_handleRequest(
goto exception; goto exception;
} }
} }
pb_u16(&pb, ref);
pb_u16(&pb, count);
break; break;
case FC15_WRITE_MULTIPLE_COILS: case FC15_WRITE_MULTIPLE_COILS:
@ -286,6 +282,8 @@ ModbusError_t mb_handleRequest(
if (pp_remains(&pp) < bytecount + (tcp ? 0 : 2)) { if (pp_remains(&pp) < bytecount + (tcp ? 0 : 2)) {
return MB_ERR_NEEDMORE; return MB_ERR_NEEDMORE;
} }
pb_u16(&pb, ref);
pb_u16(&pb, count);
bitcnt = 8; bitcnt = 8;
scratch = 0; scratch = 0;
while (count-- > 0) { while (count-- > 0) {
@ -299,8 +297,6 @@ ModbusError_t mb_handleRequest(
goto exception; goto exception;
} }
} }
pb_u16(&pb, ref);
pb_u16(&pb, count);
break; break;
case FC22_MASK_WRITE_REGISTER: case FC22_MASK_WRITE_REGISTER:
@ -356,16 +352,16 @@ ModbusError_t mb_handleRequest(
if (pp_remains(&pp) < count2 * 2 + (tcp ? 0 : 2)) { if (pp_remains(&pp) < count2 * 2 + (tcp ? 0 : 2)) {
return MB_ERR_NEEDMORE; return MB_ERR_NEEDMORE;
} }
pb_u8(&pb, 2 * count);
// First, write // First, write
while (count-- > 0) { while (count2-- > 0) {
value = pp_u16(&pp); value = pp_u16(&pp);
exc = ms->writeHolding(ms, ref++, value); exc = ms->writeHolding(ms, ref2++, value);
if (exc != 0) { if (exc != 0) {
goto exception; goto exception;
} }
} }
// Second, read // Second, read
pb_u8(&pb, 2 * count);
while (count-- > 0) { while (count-- > 0) {
exc = ms->readHolding(ms, ref++, &value); exc = ms->readHolding(ms, ref++, &value);
if (exc != 0) { if (exc != 0) {
@ -393,17 +389,22 @@ ModbusError_t mb_handleRequest(
pb_mark_t end = pb_save(&pb); 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;
} else { } else {
uint8_t *m = pb.start; uint8_t *m = pb.start;
uint16_t crc = crc16_init(); uint16_t crc = crc16_init();
*resp_size = numbytes + 2;
while (numbytes > 0) { while (numbytes > 0) {
crc = crc16_update(crc, *m++); crc = crc16_update(crc, *m++);
numbytes--; numbytes--;
} }
pb.bigendian = 0; // CRC is sent as little endian?
pb_u16(&pb, crc); pb_u16(&pb, crc);
numbytes += 2;
} }
*resp_size = numbytes;
if (!pb_ok(&pb)) {
return MB_ERROR;
}
if (ms->endOfAccess) { if (ms->endOfAccess) {
ms->endOfAccess(ms); ms->endOfAccess(ms);

22
src/modbus.h
Unescape View File

@ -34,10 +34,10 @@ typedef enum ModbusFunction {
typedef enum ModbusError { typedef enum ModbusError {
MB_OK = 0, MB_OK = 0,
MB_ERROR = 1, MB_ERROR = 1,
MB_ERR_NOTFORME, MB_ERR_NOTFORME = 2,
MB_ERR_NEEDMORE, MB_ERR_NEEDMORE = 3,
MB_ERR_CHECKSUM, MB_ERR_CHECKSUM = 4,
MB_ERR_BADPROTO, MB_ERR_BADPROTO = 5,
} ModbusError_t; } ModbusError_t;
typedef enum ModbusProtocol { typedef enum ModbusProtocol {
@ -51,14 +51,14 @@ struct ModbusSlave {
uint8_t addr; uint8_t addr;
ModbusProtocol_t proto; ModbusProtocol_t proto;
void *userData; void *userData;
enum ModbusException (*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);
enum ModbusException (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value); ModbusException_t (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value);
enum ModbusException (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value); ModbusException_t (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value);
enum ModbusException (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value); ModbusException_t (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
enum ModbusException (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value); ModbusException_t (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value);
enum ModbusException (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value); ModbusException_t (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value);
enum ModbusException (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value); ModbusException_t (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value);
}; };
ModbusError_t mb_handleRequest( ModbusError_t mb_handleRequest(

3
src/pp/payload_builder.h
Unescape View File

@ -61,6 +61,9 @@ struct PayloadBuilder_ {
// --- utilities --- // --- utilities ---
/** Returns true if the parser is still in OK state (not overrun) */
#define pb_ok(pb) ((pb)->ok)
/** Get already used bytes count */ /** Get already used bytes count */
#define pb_length(pb) ((pb)->current - (pb)->start) #define pb_length(pb) ((pb)->current - (pb)->start)

Write Preview
Loading…
Cancel
Save