[modbus] Add server support for read/write multiple registers (0x17) (#17357)

Co-authored-by: J. Nick Koston <nick@koston.org>

esphome/components/modbus/modbus.cpp

@@ -376,6 +376,50 @@ bool ModbusServerHub::check_register_range_(uint8_t address, uint8_t function_co
   return true;
 }
 
+bool ModbusServerHub::build_or_reject_read_response_(uint8_t address, uint8_t function_code, ResponseStatus status,
+                                                     uint16_t number_of_registers, const RegisterValues &registers,
+                                                     std::span<uint8_t> response_buffer, uint16_t &response_len) {
+  // A handler that returns an exception leaves registers partially filled, so check the exception
+  // first and forward it before validating the register count on the success path.
+  if (status.has_value()) {
+    this->send_exception_(address, function_code, status.value());
+    return false;
+  }
+
+  if (registers.size() != number_of_registers) {
+    ESP_LOGE(TAG, "Incorrect response %" PRIu16 " requested, %zu returned", number_of_registers, registers.size());
+    this->send_exception_(address, function_code, ExceptionCode::SERVICE_DEVICE_FAILURE);
+    return false;
+  }
+
+  // The byte count is a single byte, so the count must stay within the protocol read limit; above it the
+  // static_cast<uint8_t>(number_of_registers * 2) below would silently truncate the byte count.
+  if (number_of_registers > MAX_NUM_OF_REGISTERS_TO_READ) {
+    ESP_LOGE(TAG, "Read response of %" PRIu16 " registers exceeds the limit of %" PRIu16, number_of_registers,
+             MAX_NUM_OF_REGISTERS_TO_READ);
+    this->send_exception_(address, function_code, ExceptionCode::SERVICE_DEVICE_FAILURE);
+    return false;
+  }
+
+  // Byte count(1) + two bytes per register. Checked here rather than at the call sites so the bound travels with
+  // the write itself: a future caller starting at a non-zero response_len, or passing a smaller buffer, is
+  // rejected instead of overrunning it before send_response_'s size guard can fire.
+  const size_t required = static_cast<size_t>(response_len) + 1 + static_cast<size_t>(number_of_registers) * 2;
+  if (required > response_buffer.size()) {
+    ESP_LOGE(TAG, "Read response needs %zu bytes but only %zu are available", required, response_buffer.size());
+    this->send_exception_(address, function_code, ExceptionCode::SERVICE_DEVICE_FAILURE);
+    return false;
+  }
+
+  response_buffer[response_len++] = static_cast<uint8_t>(number_of_registers * 2);  // actual byte count
+  for (auto r : registers) {
+    auto register_bytes = decode_value(r);
+    response_buffer[response_len++] = register_bytes[0];
+    response_buffer[response_len++] = register_bytes[1];
+  }
+  return true;
+}
+
 void ModbusServerHub::process_modbus_client_frame_(uint8_t address, uint8_t function_code, const uint8_t *data) {
   ModbusServerDevice *device = this->find_device_(address);
   if (device == nullptr) {
@@ -410,25 +454,10 @@ void ModbusServerHub::process_modbus_client_frame_(uint8_t address, uint8_t func
         status = device->on_read_input_registers(start_address, number_of_registers, registers);
       }
 
-      // A handler that returns an exception leaves registers partially filled, so check the exception
-      // first and forward it before validating the register count on the success path.
-      if (status.has_value()) {
-        this->send_exception_(address, function_code, status.value());
+      if (!this->build_or_reject_read_response_(address, function_code, status, number_of_registers, registers,
+                                                response_buffer, response_len)) {
         return;
       }
-
-      if (registers.size() != number_of_registers) {
-        ESP_LOGE(TAG, "Incorrect response %" PRIu16 " requested, %zu returned", number_of_registers, registers.size());
-        this->send_exception_(address, function_code, ExceptionCode::SERVICE_DEVICE_FAILURE);
-        return;
-      }
-
-      response_buffer[response_len++] = static_cast<uint8_t>(number_of_registers * 2);  // actual byte count
-      for (auto r : registers) {
-        auto register_bytes = decode_value(r);
-        response_buffer[response_len++] = register_bytes[0];
-        response_buffer[response_len++] = register_bytes[1];
-      }
       break;
     }
     case FunctionCode::WRITE_SINGLE_REGISTER:
@@ -465,6 +494,52 @@ void ModbusServerHub::process_modbus_client_frame_(uint8_t address, uint8_t func
       response_len = 4;
       break;
     }
+    case FunctionCode::READ_WRITE_MULTIPLE_REGISTERS: {
+      // PDU data: read start address(2) + read quantity(2) + write start address(2) + write quantity(2) +
+      // write byte count(1) + write register values. Per Modbus 6.17 the write is performed before the read.
+      uint16_t read_start_address = helpers::get_data<uint16_t>(data, 0);
+      uint16_t number_of_registers = helpers::get_data<uint16_t>(data, 2);
+      uint16_t write_start_address = helpers::get_data<uint16_t>(data, 4);
+      uint16_t number_of_write_registers = helpers::get_data<uint16_t>(data, 6);
+      uint8_t number_of_bytes = helpers::get_data<uint8_t>(data, 8);
+      if (number_of_registers == 0 || number_of_registers > MAX_NUM_OF_REGISTERS_TO_READ ||
+          number_of_write_registers == 0 || number_of_write_registers > MAX_NUM_OF_REGISTERS_TO_WRITE_RW ||
+          number_of_write_registers * 2 != number_of_bytes) {
+        ESP_LOGW(TAG, "Invalid number of registers (read %" PRIu16 ", write %" PRIu16 ") or bytes %" PRIu8,
+                 number_of_registers, number_of_write_registers, number_of_bytes);
+        this->send_exception_(address, function_code, ExceptionCode::ILLEGAL_DATA_VALUE);
+        return;
+      }
+      if (!this->check_register_range_(address, function_code, read_start_address, number_of_registers) ||
+          !this->check_register_range_(address, function_code, write_start_address, number_of_write_registers)) {
+        return;
+      }
+      // Perform the write first (Modbus 6.17). Scoped so the write values are off the stack before the read
+      // values are allocated, keeping only one RegisterValues buffer live at a time.
+      {
+        // Assemble the written register values (host byte order); they follow the 9-byte request header.
+        RegisterValues write_registers;
+        for (uint16_t i = 0; i < number_of_write_registers; i++) {
+          write_registers.push_back(helpers::get_data<uint16_t>(data, 9 + i * 2));
+        }
+        // Dispatch to the standalone write and read handlers so any device implementing those supports 0x17
+        // without a dedicated handler; a device that maps registers by address reconstructs the read response
+        // from the values it just stored.
+        status = device->on_write_registers(write_start_address, write_registers);
+      }
+      if (status.has_value()) {
+        this->send_exception_(address, function_code, status.value());
+        return;
+      }
+      RegisterValues registers;
+      status = device->on_read_holding_registers(read_start_address, number_of_registers, registers);
+
+      if (!this->build_or_reject_read_response_(address, function_code, status, number_of_registers, registers,
+                                                response_buffer, response_len)) {
+        return;
+      }
+      break;
+    }
     default:
       ESP_LOGW(TAG, "Unsupported function code %" PRIu8, function_code);
       this->send_exception_(address, function_code, ExceptionCode::ILLEGAL_FUNCTION);

esphome/components/modbus/modbus.h

@@ -312,6 +312,14 @@ class ModbusClientHub : public Modbus {
   std::deque<ModbusDeviceCommand> tx_buffer_;
 };
 
+// Transaction status: std::nullopt on success, otherwise a Modbus exception code
+using ResponseStatus = std::optional<ExceptionCode>;
+
+// Register values exchanged with server handlers, in host byte order. Sized at the larger of the two protocol
+// maxima (read = 125 / 0x7D, write = 123 / 0x7B); the per-direction count limit is enforced by the hub, not by
+// the capacity of this type.
+using RegisterValues = StaticVector<uint16_t, MAX_NUM_OF_REGISTERS_TO_READ>;
+
 class ModbusServerHub : public Modbus {
  public:
   ModbusServerHub() = default;
@@ -328,6 +336,15 @@ class ModbusServerHub : public Modbus {
   // On failure, logs and sends an ILLEGAL_DATA_ADDRESS exception to the client.
   bool check_register_range_(uint8_t address, uint8_t function_code, uint16_t start_address,
                              uint16_t number_of_registers);
+
+  // Builds the body of a register read response (byte count followed by the big-endian register values) into
+  // response_buffer. Shared by every function code that answers with register values, so the read reply stays
+  // identical across them. Returns false once an exception has been sent: the one the handler reported via
+  // status, or SERVICE_DEVICE_FAILURE if it returned the wrong number of registers, the count exceeds the
+  // protocol read limit, or the body does not fit.
+  bool build_or_reject_read_response_(uint8_t address, uint8_t function_code, ResponseStatus status,
+                                      uint16_t number_of_registers, const RegisterValues &registers,
+                                      std::span<uint8_t> response_buffer, uint16_t &response_len);
   void send_raw_(const uint8_t *payload, uint16_t len);
   void send_exception_(uint8_t address, uint8_t function_code, ExceptionCode exception_code);
   void send_response_(uint8_t address, uint8_t function_code, const uint8_t *payload, uint16_t payload_len);
@@ -340,9 +357,6 @@ class ModbusServerHub : public Modbus {
   uint16_t deferred_payload_len_{0};
 };
 
-// Transaction status: std::nullopt on success, otherwise a Modbus exception code
-using ResponseStatus = std::optional<ExceptionCode>;
-
 /// Callback contract. Each accepted request ends in exactly ONE terminal: on_response() (data),
 /// on_error() (exception), on_no_response() (timeout/interruption), or on_not_sent() (dropped by
 /// clear_tx_queue_for_address before transmission). A request refused at send_pdu() (false return)
@@ -563,11 +577,6 @@ class ESPDEPRECATED("Subclass ModbusClientDevice and override on_response()/on_e
   }
 };
 
-// Register values exchanged with server handlers, in host byte order. Sized at the larger of the two protocol
-// maxima (read = 125 / 0x7D, write = 123 / 0x7B); the per-direction count limit is enforced by the hub, not by
-// the capacity of this type.
-using RegisterValues = StaticVector<uint16_t, MAX_NUM_OF_REGISTERS_TO_READ>;
-
 class ModbusServerDevice {
  public:
   virtual ~ModbusServerDevice() = default;

esphome/components/modbus/modbus_definitions.h

@@ -33,12 +33,12 @@ enum class FunctionCode : uint8_t {
   GET_COMM_EVENT_LOG = 0x0C,      // not implemented
   WRITE_MULTIPLE_COILS = 0x0F,
   WRITE_MULTIPLE_REGISTERS = 0x10,
-  REPORT_SERVER_ID = 0x11,               // not implemented
-  READ_FILE_RECORD = 0x14,               // not implemented
-  WRITE_FILE_RECORD = 0x15,              // not implemented
-  MASK_WRITE_REGISTER = 0x16,            // not implemented
-  READ_WRITE_MULTIPLE_REGISTERS = 0x17,  // not implemented
-  READ_FIFO_QUEUE = 0x18,                // not implemented
+  REPORT_SERVER_ID = 0x11,     // not implemented
+  READ_FILE_RECORD = 0x14,     // not implemented
+  WRITE_FILE_RECORD = 0x15,    // not implemented
+  MASK_WRITE_REGISTER = 0x16,  // not implemented
+  READ_WRITE_MULTIPLE_REGISTERS = 0x17,
+  READ_FIFO_QUEUE = 0x18,  // not implemented
 };
 
 // Remove before 2027.2.0

tests/components/modbus/modbus_helpers_test.cpp

@@ -92,6 +92,18 @@ TEST(ModbusClientFrameLength, ReadWriteMultipleByteCountCappedAtSpecLimit) {
   EXPECT_EQ(client_pdu_length(pdu, sizeof(pdu)), 10 + MAX_NUM_OF_REGISTERS_TO_WRITE_RW * 2);
 }
 
+TEST(ModbusClientFrameLength, ReadWriteMultipleUsesByteCount) {
+  // read start(2) + read qty(2) + write start(2) + write qty(2) + byte count(1) then data
+  const uint8_t frame[] = {0x01, 0x17, 0x9C, 0xB9, 0x00, 0x02, 0x9C, 0x41, 0x00, 0x02, 0x04, 0xAA, 0xBB, 0xCC, 0xDD};
+  EXPECT_EQ(client_frame_length(frame, sizeof(frame)), 13 + 4);
+}
+
+TEST(ModbusClientFrameLength, ReadWriteMultipleMissingByteCount) {
+  // header present up to the write quantity but the byte count byte (frame[10]) is absent
+  const uint8_t frame[] = {0x01, 0x17, 0x9C, 0xB9, 0x00, 0x02, 0x9C, 0x41, 0x00, 0x02};
+  EXPECT_EQ(client_frame_length(frame, sizeof(frame)), 13);
+}
+
 TEST(ModbusClientFrameLength, WriteMultipleMissingByteCount) {
   const uint8_t frame[] = {0x01, 0x10, 0x00, 0x00, 0x00, 0x02};
   EXPECT_EQ(client_frame_length(frame, sizeof(frame)), 9);

tests/integration/fixtures/uart_mock_modbus_server_read_write.yaml

@@ -0,0 +1,106 @@
+esphome:
+  name: uart-mock-modbus-srv-rw
+
+host:
+api:
+logger:
+  level: VERBOSE
+
+external_components:
+  - source:
+      type: local
+      path: EXTERNAL_COMPONENT_PATH
+
+# Dummy uart entry to satisfy modbus's DEPENDENCIES = ["uart"]
+# The actual UART bus used is the uart_mock component below
+uart:
+  baud_rate: 115200
+  port: /dev/null
+
+uart_mock:
+  - id: virtual_uart_dev
+    baud_rate: 9600
+    rx_full_threshold: 120
+    rx_timeout: 2
+    auto_start: false
+    debug:
+    injections:
+      # FC 0x17 Read/Write Multiple Registers on device 1:
+      #   write reg 0x0001 = 0x1234 (qty 1), then read regs 0x0001..0x0002 (qty 2).
+      # Per Modbus 6.17 the write is performed before the read, so reg 0x0001 must
+      # read back the just-written 0x1234 in the same request.
+      - delay: 100ms
+        inject_rx:
+          [0x01, 0x17, 0x00, 0x01, 0x00, 0x02, 0x00, 0x01, 0x00, 0x01, 0x02, 0x12, 0x34, 0x49, 0xD8]
+      # FC 0x17: write reg 0x0003 = 0x5678 (qty 1), then read reg 0x0003 (qty 1) -
+      # a write and read targeting a different register block.
+      - delay: 100ms
+        inject_rx:
+          [0x01, 0x17, 0x00, 0x03, 0x00, 0x01, 0x00, 0x03, 0x00, 0x01, 0x02, 0x56, 0x78, 0x9B, 0x10]
+
+globals:
+  - id: stored_1
+    type: uint16_t
+    initial_value: "0"
+  - id: stored_3
+    type: uint16_t
+    initial_value: "0"
+
+modbus:
+  uart_id: virtual_uart_dev
+  role: server
+
+modbus_server:
+  - address: 1
+    registers:
+      # Writable + readable register backed by a global. The read publishes what it
+      # returns so the test can confirm the write half ran before the read half.
+      - address: 0x01
+        value_type: U_WORD
+        read_lambda: |-
+          id(rw_read_1).publish_state(id(stored_1));
+          return id(stored_1);
+        write_lambda: |-
+          id(stored_1) = x;
+          id(rw_write_1).publish_state(x);
+          return true;
+      # Read-only register, read together with 0x01 by the first request's 2-register read.
+      - address: 0x02
+        value_type: U_WORD
+        read_lambda: |-
+          id(rw_read_2).publish_state(0x00AA);
+          return 0x00AA;
+      # Second writable + readable register, targeted by the second request.
+      - address: 0x03
+        value_type: U_WORD
+        read_lambda: |-
+          id(rw_read_3).publish_state(id(stored_3));
+          return id(stored_3);
+        write_lambda: |-
+          id(stored_3) = x;
+          id(rw_write_3).publish_state(x);
+          return true;
+
+sensor:
+  - platform: template
+    name: "rw_write_1"
+    id: rw_write_1
+  - platform: template
+    name: "rw_read_1"
+    id: rw_read_1
+  - platform: template
+    name: "rw_read_2"
+    id: rw_read_2
+  - platform: template
+    name: "rw_write_3"
+    id: rw_write_3
+  - platform: template
+    name: "rw_read_3"
+    id: rw_read_3
+
+button:
+  - platform: template
+    name: "Start Scenario"
+    id: start_scenario_btn
+    on_press:
+      - lambda: "id(virtual_uart_dev).start_scenario();"

tests/integration/fixtures/uart_mock_modbus_server_read_write_invalid.yaml

@@ -0,0 +1,81 @@
+esphome:
+  name: uart-mock-modbus-srv-rw-inv
+
+host:
+api:
+logger:
+  level: VERBOSE
+
+external_components:
+  - source:
+      type: local
+      path: EXTERNAL_COMPONENT_PATH
+
+# Dummy uart entry to satisfy modbus's DEPENDENCIES = ["uart"]
+# The actual UART bus used is the uart_mock component below
+uart:
+  baud_rate: 115200
+  port: /dev/null
+
+uart_mock:
+  - id: virtual_uart_dev
+    baud_rate: 9600
+    rx_full_threshold: 120
+    rx_timeout: 2
+    auto_start: false
+    debug:
+    injections:
+      # Malformed FC 0x17 Read/Write Multiple Registers, otherwise well formed (valid CRC): write
+      # quantity 2 but byte count 2 (2 registers need 4 bytes), i.e. byte count != 2x write quantity.
+      # The hub must reject it (ILLEGAL_DATA_VALUE) before touching any register.
+      - delay: 100ms
+        inject_rx:
+          [0x01, 0x17, 0x00, 0x01, 0x00, 0x01, 0x00, 0x01, 0x00, 0x02, 0x02, 0x12, 0x34, 0x09, 0x89]
+      # A valid FC 0x03 read of reg 0x0A injected afterwards. Its read_lambda fires the "probe"
+      # sensor, which (because injections run in order) signals the malformed frame was processed.
+      - delay: 100ms
+        inject_rx: [0x01, 0x03, 0x00, 0x0A, 0x00, 0x01, 0xA4, 0x08]
+
+modbus:
+  uart_id: virtual_uart_dev
+  role: server
+
+modbus_server:
+  - address: 1
+    registers:
+      # The malformed request's write half spans 0x01-0x02. Both are registered so modbus_server's
+      # address pre-flight cannot reject the frame on its own: if the hub wrongly accepted it, these
+      # write_lambdas would fire the "write_seen" sensor.
+      - address: 0x01
+        value_type: U_WORD
+        read_lambda: return 0;
+        write_lambda: |-
+          id(write_seen).publish_state(1);
+          return true;
+      - address: 0x02
+        value_type: U_WORD
+        read_lambda: return 0;
+        write_lambda: |-
+          id(write_seen).publish_state(1);
+          return true;
+      # Processing probe: a valid read of this register fires after the malformed frame.
+      - address: 0x0A
+        value_type: U_WORD
+        read_lambda: |-
+          id(probe).publish_state(1);
+          return 1;
+
+sensor:
+  - platform: template
+    name: "write_seen"
+    id: write_seen
+  - platform: template
+    name: "probe"
+    id: probe
+
+button:
+  - platform: template
+    name: "Start Scenario"
+    id: start_scenario_btn
+    on_press:
+      - lambda: "id(virtual_uart_dev).start_scenario();"

tests/integration/test_uart_mock_modbus.py

@@ -203,6 +203,99 @@ async def test_uart_mock_modbus_server(
         _assert_no_modbus_errors(error_log_lines, warning_log_lines)
 
 
+@pytest.mark.asyncio
+async def test_uart_mock_modbus_server_read_write(
+    yaml_config: str,
+    run_compiled: RunCompiledFunction,
+    api_client_connected: APIClientConnectedFactory,
+) -> None:
+    """Test modbus server FC 0x17 (read/write multiple registers).
+
+    Injects raw 0x17 request frames and checks the round-trip through the
+    server's read_lambda/write_lambda, independent of how the hub dispatches
+    0x17 internally:
+      * one request writes reg 0x01 then reads regs 0x01+0x02 -- reg 0x01 reads
+        back the just-written value (the write happens before the read per
+        Modbus 6.17), and the second register is returned by the same
+        multi-register read;
+      * a second request writes and reads a different register block.
+    """
+
+    line_callback, error_log_lines, warning_log_lines = _make_modbus_line_callback()
+
+    tracker = SensorTracker(
+        ["rw_write_1", "rw_read_1", "rw_read_2", "rw_write_3", "rw_read_3"]
+    )
+    futures = tracker.expect_all(
+        {
+            "rw_write_1": 4660,  # 0x1234 written to reg 0x0001
+            "rw_read_1": 4660,  # reg 0x0001 reads back the just-written value
+            "rw_read_2": 170,  # 0x00AA read from reg 0x0002 in the same request
+            "rw_write_3": 22136,  # 0x5678 written to reg 0x0003
+            "rw_read_3": 22136,  # reg 0x0003 reads back the just-written value
+        }
+    )
+
+    async with (
+        run_compiled(yaml_config, line_callback=line_callback),
+        api_client_connected() as client,
+    ):
+        await tracker.setup_and_start_scenario(client)
+        await tracker.await_all(futures)
+        _assert_no_modbus_errors(error_log_lines, warning_log_lines)
+
+
+@pytest.mark.asyncio
+async def test_uart_mock_modbus_server_read_write_invalid(
+    yaml_config: str,
+    run_compiled: RunCompiledFunction,
+    api_client_connected: APIClientConnectedFactory,
+) -> None:
+    """Test modbus server FC 0x17 invalid-frame handling.
+
+    Injects a well-formed (valid CRC) 0x17 request whose write byte count (2)
+    does not match 2x the write quantity (2 registers need 4 bytes), so the hub
+    must reject it with ILLEGAL_DATA_VALUE before touching any register. A valid
+    read is injected right after as a processing marker.
+
+    The invalid frame is verified via bus-level signals rather than the reply
+    frame on the wire: the mock UART cannot observe the server's TX reliably on
+    the host platform (the server's transmission is gated by a millis()-based tx
+    delay), so instead we assert the request is rejected exactly once and never
+    applied to a register.
+    """
+
+    line_callback, error_log_lines, warning_log_lines = _make_modbus_line_callback()
+
+    tracker = SensorTracker(["write_seen", "probe"])
+    probe_seen = tracker.expect("probe", 1)
+
+    async with (
+        run_compiled(yaml_config, line_callback=line_callback),
+        api_client_connected() as client,
+    ):
+        await tracker.setup_and_start_scenario(client)
+        # The probe read is injected after the malformed frame, so once it fires
+        # the malformed frame has already been processed.
+        await tracker.await_change(probe_seen, "probe")
+
+    # Exactly one bus-level rejection for the malformed frame (no cascade)...
+    invalid_warnings = [
+        line for line in warning_log_lines if "Invalid number of registers" in line
+    ]
+    assert len(invalid_warnings) == 1, (
+        "Expected exactly one invalid-frame rejection, got warnings:\n"
+        + "\n".join(warning_log_lines)
+    )
+    assert len(error_log_lines) == 0, (
+        "Expected no modbus errors, but got:\n" + "\n".join(error_log_lines)
+    )
+    # ...and the rejected write is never applied to the target register.
+    assert not tracker.sensor_states["write_seen"], (
+        f"malformed 0x17 must not write, but write_seen fired: {tracker.sensor_states['write_seen']}"
+    )
+
+
 @pytest.mark.asyncio
 async def test_uart_mock_modbus_server_controller(
     yaml_config: str,