diff --git a/src/main.c b/src/main.c index dafab9b..d9fdf0e 100644 --- a/src/main.c +++ b/src/main.c @@ -1,8 +1,191 @@ #include #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() { - 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; } diff --git a/src/modbus.c b/src/modbus.c index cb2b6ad..fef189d 100644 --- a/src/modbus.c +++ b/src/modbus.c @@ -51,8 +51,8 @@ ModbusError_t mb_handleRequest( pb_mark_t resp_fc_mark, resp_len_mark, resp_pld_start_mark; const bool tcp = ms->proto == MB_PROTO_TCP; - PayloadParser pp = pp_start_le(req, req_size, NULL); - PayloadBuilder pb = pb_start_le(resp, resp_capacity, NULL); + PayloadParser pp = pp_start_be(req, req_size, NULL); + PayloadBuilder pb = pb_start_be(resp, resp_capacity, NULL); if (tcp) { /* Parse header */ @@ -66,10 +66,10 @@ ModbusError_t mb_handleRequest( msglen = req_size; /* check CRC */ 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)); } - if (crc != pp_u16(&pp)) { + if (crc != 0) { return MB_ERR_CHECKSUM; } pp_rewind(&pp); @@ -137,16 +137,12 @@ ModbusError_t mb_handleRequest( case FC01_READ_COILS: case FC02_READ_DISCRETES: /* check we have the needed function */ - if (fcx == FC01_READ_COILS) { - if (!ms->readCoil) { - exc = MB_EXCEPTION_ILLEGAL_FUNCTION; - goto exception; - } - } else { - if (!ms->readDiscrete) { - exc = MB_EXCEPTION_ILLEGAL_FUNCTION; - goto exception; - } + if (fcx == FC01_READ_COILS && !ms->readCoil) { + exc = MB_EXCEPTION_ILLEGAL_FUNCTION; + goto exception; + } else if (!ms->readDiscrete) { + exc = MB_EXCEPTION_ILLEGAL_FUNCTION; + goto exception; } ref = pp_u16(&pp); count = pp_u16(&pp); @@ -170,7 +166,7 @@ ModbusError_t mb_handleRequest( if (exc != 0) { goto exception; } - scratch = (scratch << 1) | bvalue; + scratch |= ((bvalue & 1) << bitcnt); bitcnt++; if (bitcnt == 8) { pb_u8(&pb, scratch); @@ -178,21 +174,20 @@ ModbusError_t mb_handleRequest( bitcnt = 0; } } + if (bitcnt != 0) { + pb_u8(&pb, scratch); + } break; case FC03_READ_HOLDING_REGISTERS: case FC04_READ_INPUT_REGISTERS: /* check we have the needed function */ - if (fcx == FC03_READ_HOLDING_REGISTERS) { - if (!ms->readHolding) { - exc = MB_EXCEPTION_ILLEGAL_FUNCTION; - goto exception; - } - } else { - if (!ms->readInput) { - exc = MB_EXCEPTION_ILLEGAL_FUNCTION; - goto exception; - } + if (fcx == FC03_READ_HOLDING_REGISTERS && !ms->readHolding) { + exc = MB_EXCEPTION_ILLEGAL_FUNCTION; + goto exception; + } else if (!ms->readInput) { + exc = MB_EXCEPTION_ILLEGAL_FUNCTION; + goto exception; } ref = pp_u16(&pp); @@ -204,6 +199,7 @@ ModbusError_t mb_handleRequest( exc = MB_EXCEPTION_ILLEGAL_DATA_ADDRESS; // TODO? goto exception; } + pb_u8(&pb, count*2); while (count-- > 0) { if (fcx == FC03_READ_HOLDING_REGISTERS) { exc = ms->readHolding(ms, ref++, &value); @@ -257,6 +253,8 @@ ModbusError_t mb_handleRequest( 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); @@ -264,8 +262,6 @@ ModbusError_t mb_handleRequest( goto exception; } } - pb_u16(&pb, ref); - pb_u16(&pb, count); break; case FC15_WRITE_MULTIPLE_COILS: @@ -286,6 +282,8 @@ ModbusError_t mb_handleRequest( 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) { @@ -299,8 +297,6 @@ ModbusError_t mb_handleRequest( goto exception; } } - pb_u16(&pb, ref); - pb_u16(&pb, count); break; case FC22_MASK_WRITE_REGISTER: @@ -356,16 +352,16 @@ ModbusError_t mb_handleRequest( if (pp_remains(&pp) < count2 * 2 + (tcp ? 0 : 2)) { return MB_ERR_NEEDMORE; } + pb_u8(&pb, 2 * count); // First, write - while (count-- > 0) { + while (count2-- > 0) { value = pp_u16(&pp); - exc = ms->writeHolding(ms, ref++, value); + exc = ms->writeHolding(ms, ref2++, value); if (exc != 0) { goto exception; } } // Second, read - pb_u8(&pb, 2 * count); while (count-- > 0) { exc = ms->readHolding(ms, ref++, &value); if (exc != 0) { @@ -393,17 +389,22 @@ ModbusError_t mb_handleRequest( pb_mark_t end = pb_save(&pb); 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); - numbytes += 2; } - *resp_size = numbytes; + + if (!pb_ok(&pb)) { + return MB_ERROR; + } if (ms->endOfAccess) { ms->endOfAccess(ms); diff --git a/src/modbus.h b/src/modbus.h index 0ac8dcd..4549176 100644 --- a/src/modbus.h +++ b/src/modbus.h @@ -34,10 +34,10 @@ typedef enum ModbusFunction { typedef enum ModbusError { MB_OK = 0, MB_ERROR = 1, - MB_ERR_NOTFORME, - MB_ERR_NEEDMORE, - MB_ERR_CHECKSUM, - MB_ERR_BADPROTO, + MB_ERR_NOTFORME = 2, + MB_ERR_NEEDMORE = 3, + MB_ERR_CHECKSUM = 4, + MB_ERR_BADPROTO = 5, } ModbusError_t; typedef enum ModbusProtocol { @@ -51,14 +51,14 @@ struct ModbusSlave { uint8_t addr; ModbusProtocol_t proto; 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); - enum ModbusException (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value); - enum ModbusException (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value); - enum ModbusException (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value); - enum ModbusException (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value); - enum ModbusException (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value); - enum ModbusException (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value); + ModbusException_t (*readCoil)(ModbusSlave_t *ms, uint16_t reference, bool *value); + ModbusException_t (*readDiscrete)(ModbusSlave_t *ms, uint16_t reference, bool *value); + ModbusException_t (*readHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value); + ModbusException_t (*readInput)(ModbusSlave_t *ms, uint16_t reference, uint16_t *value); + ModbusException_t (*writeCoil)(ModbusSlave_t *ms, uint16_t reference, bool value); + ModbusException_t (*writeHolding)(ModbusSlave_t *ms, uint16_t reference, uint16_t value); }; ModbusError_t mb_handleRequest( diff --git a/src/pp/payload_builder.h b/src/pp/payload_builder.h index bc11b33..2612a4a 100644 --- a/src/pp/payload_builder.h +++ b/src/pp/payload_builder.h @@ -61,6 +61,9 @@ struct PayloadBuilder_ { // --- 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)