From e3b13ccdd1fbf77fcc4d25b03bae67128a2031fd Mon Sep 17 00:00:00 2001 From: maxlemo Date: Wed, 2 Aug 2023 11:11:14 -0400 Subject: [PATCH] adds fc23 server code --- nanomodbus.c | 138 ++++++++++++++++++++++++++++++++++++++- tests/nanomodbus_tests.c | 32 +++++++++ 2 files changed, 169 insertions(+), 1 deletion(-) diff --git a/nanomodbus.c b/nanomodbus.c index a4956ef..053b30c 100644 --- a/nanomodbus.c +++ b/nanomodbus.c @@ -1129,6 +1129,138 @@ static nmbs_error handle_write_multiple_registers(nmbs_t* nmbs) { } #endif +#ifndef NMBS_SERVER_READ_WRITE_REGISTERS_DISABLED +static nmbs_error handle_read_write_registers(nmbs_t* nmbs) +{ + nmbs_error err = recv(nmbs, 9); + if(err != NMBS_ERROR_NONE) + return err; + uint16_t read_address = get_2(nmbs); + uint16_t read_quantity = get_2(nmbs); + uint16_t write_address = get_2(nmbs); + uint16_t write_quantity = get_2(nmbs); + + uint8_t byte_count_write = get_1(nmbs); + + NMBS_DEBUG_PRINT("ra %d\trq %d\t wa %d\t wq %d\t b %d\tregs ", + read_address, + read_quantity, + write_address, + write_quantity, + byte_count_write); + + err = recv(nmbs, byte_count_write); + if(err != NMBS_ERROR_NONE) + return err; + + uint16_t registers[0x007B]; + for(int i = 0; i < byte_count_write / 2; i++) + { + registers[i] = get_2(nmbs); + NMBS_DEBUG_PRINT("%d ", registers[i]); + } + + err = recv_msg_footer(nmbs); + if(err != NMBS_ERROR_NONE) + return err; + + if(!nmbs->msg.ignored) + { + { + // write handling BEFORE read handling (see fig 27 at + // https://modbus.org/docs/Modbus_Application_Protocol_V1_1b.pdf) + if(write_quantity < 1 || write_quantity > 0x007B) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if((uint32_t)write_address + (uint32_t)write_quantity > ((uint32_t)0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if(byte_count_write == 0) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if(byte_count_write != write_quantity * 2) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if(nmbs->callbacks.write_multiple_registers) + { + err = nmbs->callbacks.write_multiple_registers(write_address, + write_quantity, + registers, + nmbs->msg.unit_id, + nmbs->platform.arg); + if(err != NMBS_ERROR_NONE) + { + if(nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + } + else + { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + { + // read handling + if(read_quantity < 1 || read_quantity > 125) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_VALUE); + + if((uint32_t)read_address + (uint32_t)read_quantity > ((uint32_t)0xFFFF) + 1) + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_DATA_ADDRESS); + + if(nmbs->callbacks.read_holding_registers) + { + uint16_t regs[125] = {0}; + err = nmbs->callbacks.read_holding_registers(read_address, + read_quantity, + regs, + nmbs->msg.unit_id, + nmbs->platform.arg); + if(err != NMBS_ERROR_NONE) + { + if(nmbs_error_is_exception(err)) + return send_exception_msg(nmbs, err); + + return send_exception_msg(nmbs, NMBS_EXCEPTION_SERVER_DEVICE_FAILURE); + } + + if(!nmbs->msg.broadcast) + { + uint8_t regs_bytes = read_quantity * 2; + put_res_header(nmbs, 1 + regs_bytes); + + put_1(nmbs, regs_bytes); + + NMBS_DEBUG_PRINT("b %d\t", regs_bytes); + + NMBS_DEBUG_PRINT("regs "); + for(int i = 0; i < read_quantity; i++) + { + put_2(nmbs, regs[i]); + NMBS_DEBUG_PRINT("%d ", regs[i]); + } + + err = send_msg(nmbs); + if(err != NMBS_ERROR_NONE) + return err; + } + } + else + { + return send_exception_msg(nmbs, NMBS_EXCEPTION_ILLEGAL_FUNCTION); + } + } + } + else + { + return recv_write_multiple_registers_res(nmbs, write_address, write_quantity); + } + + return NMBS_ERROR_NONE; +} +#endif + #ifndef NMBS_SERVER_READ_FILE_RECORD_DISABLED static nmbs_error handle_read_file_record(nmbs_t* nmbs) { nmbs_error err = recv(nmbs, 1); @@ -1406,7 +1538,11 @@ static nmbs_error handle_req_fc(nmbs_t* nmbs) { err = handle_write_file_record(nmbs); break; #endif - +#ifndef NMBS_SERVER_READ_WRITE_REGISTERS + case 23: + err = handle_read_write_registers(nmbs); + break; +#endif default: err = NMBS_EXCEPTION_ILLEGAL_FUNCTION; } diff --git a/tests/nanomodbus_tests.c b/tests/nanomodbus_tests.c index 885871e..0b620da 100644 --- a/tests/nanomodbus_tests.c +++ b/tests/nanomodbus_tests.c @@ -968,6 +968,37 @@ void test_fc21(nmbs_transport transport) { stop_client_and_server(); } +void test_fc23(nmbs_transport transport) { + const uint8_t fc = 16; + uint8_t raw_res[260]; + uint16_t registers[125]; + uint16_t registers_write[125] = {42}; + nmbs_callbacks callbacks_empty = {0}; + + start_client_and_server(transport, &callbacks_empty); + + should("return NMBS_EXCEPTION_ILLEGAL_FUNCTION when callback is not registered server-side"); + expect(nmbs_read_write_registers(&CLIENT, 0, 1, registers, 0, 1, registers_write) == NMBS_EXCEPTION_ILLEGAL_FUNCTION); + + stop_client_and_server(); + + start_client_and_server(transport, &(nmbs_callbacks){ + .read_holding_registers = read_registers, + .write_multiple_registers = write_registers}); + + should("immediately return NMBS_ERROR_INVALID_ARGUMENT when calling with quantity 0"); + expect(nmbs_read_write_registers(&CLIENT, 1, 0, registers, 1, 0, registers_write) == NMBS_ERROR_INVALID_ARGUMENT); + + should("immediately return NMBS_ERROR_INVALID_ARGUMENT when calling with quantity > 0x007B"); + expect(nmbs_read_write_registers(&CLIENT, 1, 0x007C, registers, 1, 0x007C, registers_write) == NMBS_ERROR_INVALID_ARGUMENT); + + should("immediately return NMBS_ERROR_INVALID_ARGUMENT when calling with address + quantity > 0xFFFF + 1"); + expect(nmbs_read_write_registers(&CLIENT, 0xFFFF, 2, registers, 0xFFFF, 2, registers_write) == NMBS_ERROR_INVALID_ARGUMENT); + + // TODO add actual tests + + stop_client_and_server(); +} nmbs_transport transports[2] = {NMBS_TRANSPORT_RTU, NMBS_TRANSPORT_TCP}; const char* transports_str[2] = {"RTU", "TCP"}; @@ -1007,5 +1038,6 @@ int main(int argc, char* argv[]) { for_transports(test_fc21, "send and receive FC 21 (0x15) Write File Record"); + for_transports(test_fc23, "send and receive FC 23 (0x17) Read/Write Multiple Registers"); return 0; }