From ea3ae6c8b470872d1c1cffa6cf278d3b044b9abd Mon Sep 17 00:00:00 2001 From: Martin Mares Date: Mon, 8 Jul 2019 14:56:12 +0200 Subject: [PATCH] ModBus: Implementation of all common commands --- test-modbus/modbus.c | 315 ++++++++++++++++++++++++++++++++++++++++++- test-modbus/modbus.h | 16 +++ 2 files changed, 324 insertions(+), 7 deletions(-) diff --git a/test-modbus/modbus.c b/test-modbus/modbus.c index 2e8320e..24a2589 100644 --- a/test-modbus/modbus.c +++ b/test-modbus/modbus.c @@ -23,6 +23,9 @@ static u16 rx_size; static byte rx_bad; static byte state; // STATE_xxx +static byte *rx_frame; +static byte *rx_frame_end; + static byte tx_buf[TX_BUFSIZE]; static u16 tx_size; static u16 tx_pos; @@ -259,11 +262,31 @@ static bool check_frame(void) return false; } + rx_frame = rx_buf + 1; + rx_frame_end = rx_frame + rx_size - 2; return true; } enum mb_function { + FUNC_READ_COILS = 0x01, + FUNC_READ_DISCRETE_INPUTS = 0x02, FUNC_READ_HOLDING_REGISTERS = 0x03, + FUNC_READ_INPUT_REGISTERS = 0x04, + FUNC_WRITE_SINGLE_COIL = 0x05, + FUNC_WRITE_SINGLE_REGISTER = 0x06, + FUNC_READ_EXCEPTION_STATUS = 0x07, + FUNC_DIAGNOSTICS = 0x08, + FUNC_GET_COMM_EVENT_COUNTER = 0x0b, + FUNC_GET_COMM_EVENT_LOG = 0x0c, + FUNC_WRITE_MULTIPLE_COILS = 0x0f, + FUNC_WRITE_MULTIPLE_REGISTERS = 0x10, + FUNC_REPORT_SLAVE_ID = 0x11, + FUNC_READ_FILE_RECORD = 0x14, + FUNC_WRITE_FILE_RECORD = 0x15, + FUNC_MASK_WRITE_REGISTER = 0x16, + FUNC_READ_WRITE_MULTIPLE_REGISTERS = 0x17, + FUNC_READ_FIFO_QUEUE = 0x18, + FUNC_ENCAPSULATED_INTERFACE_TRANSPORT = 0x2b, }; enum mb_error { @@ -272,6 +295,40 @@ enum mb_error { ERR_ILLEGAL_DATA_VALUE = 0x03, }; +static uint read_remains(void) +{ + return rx_frame_end - rx_frame; +} + +static byte read_byte(void) +{ + return *rx_frame++; +} + +static u16 read_u16(void) +{ + byte hi = *rx_frame++; + byte lo = *rx_frame++; + return (hi << 8) | lo; +} + +static void write_byte(byte v) +{ + tx_buf[tx_size++] = v; +} + +static void write_u16(u16 v) +{ + write_byte(v >> 8); + write_byte(v); +} + +static bool body_fits(uint body_len) +{ + // body_len excludes slave address, function code, and CRC + return (2 + body_len + 2 <= TX_BUFSIZE); +} + static void report_error(byte code) { // Discard the partially constructed body of the reply and rewrite the header @@ -280,9 +337,180 @@ static void report_error(byte code) tx_size = 3; } +static void func_read_bits(bool coils) +{ + if (read_remains() < 4) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + + uint bytes = (count+7) / 8; + if (!body_fits(1 + bytes)) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!(coils ? modbus_check_coil : modbus_check_discrete_input)(start + i)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + write_byte(bytes); + for (u16 i = 0; i < bytes; i++) { + byte b = 0; + for (byte j = 0; j < 8 && 8*i + j < count; j++) { + uint addr = start + 8*i + j; + if ((coils ? modbus_get_coil : modbus_get_discrete_input)(addr)) + b |= 1 << j; + } + write_byte(b); + } +} + +static void func_read_registers(byte holding) +{ + if (read_remains() < 4) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + + uint bytes = 2*count; + if (!body_fits(1 + bytes)) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!(holding ? modbus_check_holding_register : modbus_check_input_register)(start + i)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + // FIXME: Reporting of slave failures? + write_byte(bytes); + for (u16 i = 0; i < count; i++) + write_u16((holding ? modbus_get_holding_register : modbus_get_input_register)(start + i)); +} + +static void func_write_single_coil(void) +{ + if (read_remains() < 4) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 addr = read_u16(); + u16 value = read_u16(); + + if (!modbus_check_coil(addr)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + if (value != 0x0000 && value != 0xff00) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + modbus_set_coil(addr, value); +} + +static void func_write_single_register(void) +{ + if (read_remains() < 4) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 addr = read_u16(); + u16 value = read_u16(); + + if (!modbus_check_holding_register(addr)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + modbus_set_holding_register(addr, value); +} + +static void func_write_multiple_coils(void) +{ + if (read_remains() < 5) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + byte bytes = read_byte(); + + if (read_remains() < bytes || bytes != (count+7) / 8) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!modbus_check_coil(start + i)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + for (u16 i = 0; i < count; i++) + modbus_set_coil(start + i, rx_frame[i/8] & (1U << (i%8))); +} + +static void func_write_multiple_registers(void) +{ + if (read_remains() < 5) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 start = read_u16(); + u16 count = read_u16(); + byte bytes = read_byte(); + + if (read_remains() < bytes || bytes != 2*count) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < count; i++) + if (!modbus_check_holding_register(start + i)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + for (u16 i = 0; i < count; i++) + modbus_set_holding_register(start + i, read_u16()); +} + +static void func_mask_write_register(void) +{ + if (read_remains() < 6) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 addr = read_u16(); + u16 and_mask = read_u16(); + u16 or_mask = read_u16(); + + if (!modbus_check_holding_register(addr)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + u16 reg = modbus_get_holding_register(addr); + reg = (reg & and_mask) | (or_mask & ~and_mask); + modbus_set_holding_register(addr, reg); +} + +static void func_read_write_multiple_registers(void) +{ + if (read_remains() < 9) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + u16 read_start = read_u16(); + u16 read_count = read_u16(); + u16 write_start = read_u16(); + u16 write_count = read_u16(); + byte write_bytes = read_byte(); + + if (read_remains() < write_bytes || write_bytes != 2*write_count) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < read_count; i++) + if (!modbus_check_holding_register(read_start + i)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + for (u16 i = 0; i < write_count; i++) + if (!modbus_check_holding_register(write_start + i)) + return report_error(ERR_ILLEGAL_DATA_ADDRESS); + + byte read_bytes = 2*write_count; + if (!body_fits(1 + read_bytes)) + return report_error(ERR_ILLEGAL_DATA_VALUE); + + for (u16 i = 0; i < write_count; i++) + modbus_set_holding_register(write_start + i, read_u16()); + + write_byte(read_bytes); + for (u16 i = 0; i < read_count; i++) + modbus_get_holding_register(read_start + i); +} + static void process_frame(void) { - byte func = rx_buf[1]; + byte func = read_byte(); // Prepare reply frame tx_buf[0] = MB_OUR_ADDRESS; @@ -290,19 +518,42 @@ static void process_frame(void) tx_size = 2; switch (func) { + case FUNC_READ_COILS: + func_read_bits(true); + break; + case FUNC_READ_DISCRETE_INPUTS: + func_read_bits(false); + break; case FUNC_READ_HOLDING_REGISTERS: - tx_buf[tx_size++] = 2; - tx_buf[tx_size++] = 0x12; - tx_buf[tx_size++] = 0x34; + func_read_registers(true); + break; + case FUNC_READ_INPUT_REGISTERS: + func_read_registers(false); + break; + case FUNC_WRITE_SINGLE_COIL: + func_write_single_coil(); + break; + case FUNC_WRITE_SINGLE_REGISTER: + func_write_single_register(); + break; + case FUNC_WRITE_MULTIPLE_COILS: + func_write_multiple_coils(); + break; + case FUNC_WRITE_MULTIPLE_REGISTERS: + func_write_multiple_registers(); + break; + case FUNC_MASK_WRITE_REGISTER: + func_mask_write_register(); + break; + case FUNC_READ_WRITE_MULTIPLE_REGISTERS: + func_read_write_multiple_registers(); break; default: report_error(ERR_ILLEGAL_FUNCTION); } // Finish reply frame - u16 crc = crc16(tx_buf, tx_size); - tx_buf[tx_size++] = crc >> 8; - tx_buf[tx_size++] = crc; + write_u16(crc16(tx_buf, tx_size)); } void modbus_loop(void) @@ -332,3 +583,53 @@ void modbus_loop(void) rx_init(); } } + +/*** Callbacks ***/ + +bool modbus_check_discrete_input(u16 addr UNUSED) +{ + return false; +} + +bool modbus_get_discrete_input(u16 addr UNUSED) +{ + return false; +} + +bool modbus_check_coil(u16 addr UNUSED) +{ + return false; +} + +bool modbus_get_coil(u16 addr UNUSED) +{ + return false; +} + +void modbus_set_coil(u16 addr UNUSED, bool value UNUSED) +{ +} + +bool modbus_check_input_register(u16 addr UNUSED) +{ + return false; +} + +u16 modbus_get_input_register(u16 addr UNUSED) +{ + return 0; +} + +bool modbus_check_holding_register(u16 addr UNUSED) +{ + return (addr == 0); +} + +u16 modbus_get_holding_register(u16 addr UNUSED) +{ + return 0xbeef; +} + +void modbus_set_holding_register(u16 addr UNUSED, u16 value UNUSED) +{ +} diff --git a/test-modbus/modbus.h b/test-modbus/modbus.h index 1c2fe62..dd80a66 100644 --- a/test-modbus/modbus.h +++ b/test-modbus/modbus.h @@ -1,2 +1,18 @@ void modbus_init(void); void modbus_loop(void); + +// Callbacks + +bool modbus_check_discrete_input(u16 addr); +bool modbus_get_discrete_input(u16 addr); + +bool modbus_check_coil(u16 addr); +bool modbus_get_coil(u16 addr); +void modbus_set_coil(u16 addr, bool value); + +bool modbus_check_input_register(u16 addr); +u16 modbus_get_input_register(u16 addr); + +bool modbus_check_holding_register(u16 addr); +u16 modbus_get_holding_register(u16 addr); +void modbus_set_holding_register(u16 addr, u16 value); -- 2.39.2