adds fc23 server code

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

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