master
Ondřej Hruška 2 years ago
parent 81f6c8f2e9
commit ab646c3a47
  1. 185
      src/main.c
  2. 69
      src/modbus.c
  3. 22
      src/modbus.h
  4. 3
      src/pp/payload_builder.h

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

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

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

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

Loading…
Cancel
Save