judsonupchurch/nanoMODBUS
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

adds fc23 server code

Browse Source
This commit is contained in:
maxlemo 2023-08-02 11:11:14 -04:00
parent 5991219763
commit e3b13ccdd1
2 changed files with 169 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
nanomodbus.c
View File

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

32
tests/nanomodbus_tests.c
View File

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