[modbus_server] Simplify server response handling (#12376)

Co-authored-by: Claude Opus 4.8 <noreply@anthropic.com> Co-authored-by: J. Nick Koston <nick@home-assistant.io> Co-authored-by: J. Nick Koston <nick@koston.org>
2026-07-11 09:25:34 +00:00 · 2026-06-28 12:41:47 -07:00
parent 2f32c88ae5
commit 4ebecf514a
10 changed files with 476 additions and 226 deletions
@@ -124,7 +124,6 @@ async def register_modbus_client_device(var, config):

 async def register_modbus_server_device(var, config):
    parent = await cg.get_variable(config[CONF_MODBUS_ID])
-    cg.add(var.set_parent(parent))
    cg.add(var.set_address(config[CONF_ADDRESS]))
    cg.add(parent.register_device(var))

@@ -258,7 +258,7 @@ bool ModbusServerHub::parse_modbus_client_frame_() {
  std::memcpy(data, this->rx_buffer_.data() + data_offset, data_len);
  this->clear_rx_buffer_(LOG_STR("parse succeeded"), false, frame_length);

-  this->process_modbus_client_frame_(address, function_code, data, data_len);
+  this->process_modbus_client_frame_(address, function_code, data);

  return true;
 }
@@ -321,10 +321,8 @@ void ModbusClientHub::process_modbus_server_frame(uint8_t address, uint8_t funct
 }

 void ModbusServerHub::process_modbus_server_frame(uint8_t address, uint8_t function_code, const uint8_t *, uint16_t) {
-  for (auto *device : this->devices_) {
-    if (device->address_ == address) {
-      ESP_LOGE(TAG, "Unexpected response from address %" PRIu8 ", which is mapped to this device.", address);
-    }
+  if (this->find_device_(address) != nullptr) {
+    ESP_LOGE(TAG, "Unexpected response from address %" PRIu8 ", which is mapped to this device.", address);
  }

  if (this->expecting_peer_response_ == address) {
@@ -338,31 +336,124 @@ void ModbusServerHub::process_modbus_server_frame(uint8_t address, uint8_t funct
  this->expecting_peer_response_ = 0;
 }

-void ModbusServerHub::process_modbus_client_frame_(uint8_t address, uint8_t function_code, const uint8_t *data,
-                                                   uint16_t len) {
-  bool found = false;
-
+ModbusServerDevice *ModbusServerHub::find_device_(uint8_t address) {
  for (auto *device : this->devices_) {
-    if (device->address_ == address) {
-      found = true;
-
-      if (static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::READ_HOLDING_REGISTERS ||
-          static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::READ_INPUT_REGISTERS) {
-        device->on_modbus_read_registers(function_code, helpers::get_data<uint16_t>(data, 0),
-                                         helpers::get_data<uint16_t>(data, 2));
-      } else if (static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::WRITE_SINGLE_REGISTER ||
-                 static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::WRITE_MULTIPLE_REGISTERS) {
-        device->on_modbus_write_registers(function_code, std::vector<uint8_t>(data, data + len));
-      } else {
-        ESP_LOGW(TAG, "Unsupported function code %" PRIu8, function_code);
-        device->send_error(function_code, ModbusExceptionCode::ILLEGAL_FUNCTION);
-      }
+    if (device->get_address() == address) {
+      return device;
    }
  }
+  return nullptr;
+}

-  if (!found) {
+bool ModbusServerHub::check_register_range_(uint8_t address, uint8_t function_code, uint16_t start_address,
+                                            uint16_t number_of_registers) {
+  if ((uint32_t) start_address + number_of_registers > 0x10000u) {
+    ESP_LOGW(TAG, "Register address out of range - start: %" PRIu16 " num: %" PRIu16, start_address,
+             number_of_registers);
+    this->send_exception_(address, function_code, ModbusExceptionCode::ILLEGAL_DATA_ADDRESS);
+    return false;
+  }
+  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) {
    this->expecting_peer_response_ = address;
    ESP_LOGV(TAG, "Request to peer %" PRIu8 " received", address);
+    return;
+  }
+
+  ServerResponseStatus status;
+  uint8_t response_buffer[modbus::MAX_RAW_SIZE];
+  const uint8_t *response_data = response_buffer;
+  uint16_t response_len = 0;
+
+  switch (static_cast<ModbusFunctionCode>(function_code)) {
+    case ModbusFunctionCode::READ_HOLDING_REGISTERS:
+    case ModbusFunctionCode::READ_INPUT_REGISTERS: {
+      // PDU data: start address(2) + quantity(2).
+      uint16_t start_address = helpers::get_data<uint16_t>(data, 0);
+      uint16_t number_of_registers = helpers::get_data<uint16_t>(data, 2);
+      if (number_of_registers == 0 || number_of_registers > MAX_NUM_OF_REGISTERS_TO_READ) {
+        ESP_LOGW(TAG, "Invalid number of registers %" PRIu16, number_of_registers);
+        this->send_exception_(address, function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
+        return;
+      }
+      if (!this->check_register_range_(address, function_code, start_address, number_of_registers)) {
+        return;
+      }
+      RegisterValues registers;
+      if (static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::READ_HOLDING_REGISTERS) {
+        status = device->on_modbus_read_holding_registers(start_address, number_of_registers, registers);
+      } else {
+        status = device->on_modbus_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());
+        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, ModbusExceptionCode::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 ModbusFunctionCode::WRITE_SINGLE_REGISTER:
+    case ModbusFunctionCode::WRITE_MULTIPLE_REGISTERS: {
+      // PDU data: start address(2) [+ quantity(2) + byte count(1)] + register values.
+      // A single-register write always targets one register; for a multiple-register write the
+      // quantity is in the frame and its byte count must equal quantity * 2. The register values are
+      // assembled into registers below so the handler doesn't have to know the request framing.
+      uint16_t start_address = helpers::get_data<uint16_t>(data, 0);
+      uint16_t number_of_registers = 1;
+      uint16_t values_offset = 2;  // single write: values follow the 2-byte start address
+      if (static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::WRITE_MULTIPLE_REGISTERS) {
+        number_of_registers = helpers::get_data<uint16_t>(data, 2);
+        uint8_t number_of_bytes = helpers::get_data<uint8_t>(data, 4);
+        values_offset = 5;  // multiple write: values follow start address(2) + quantity(2) + byte count(1)
+        if (number_of_registers == 0 || number_of_registers > MAX_NUM_OF_REGISTERS_TO_WRITE ||
+            number_of_registers * 2 != number_of_bytes) {
+          ESP_LOGW(TAG, "Invalid number of registers %" PRIu16 " or bytes %" PRIu8, number_of_registers,
+                   number_of_bytes);
+          this->send_exception_(address, function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
+          return;
+        }
+        if (!this->check_register_range_(address, function_code, start_address, number_of_registers)) {
+          return;
+        }
+      }
+      // Assemble the register values (host byte order) so the handler never sees wire framing.
+      RegisterValues registers;
+      for (uint16_t i = 0; i < number_of_registers; i++) {
+        registers.push_back(helpers::get_data<uint16_t>(data, values_offset + i * 2));
+      }
+      status = device->on_modbus_write_registers(start_address, registers);
+      response_data = data;  // echo the request header per Modbus 6.6, 6.12
+      response_len = 4;
+      break;
+    }
+    default:
+      ESP_LOGW(TAG, "Unsupported function code %" PRIu8, function_code);
+      this->send_exception_(address, function_code, ModbusExceptionCode::ILLEGAL_FUNCTION);
+      return;
+  }
+  if (status.has_value()) {
+    this->send_exception_(address, function_code, status.value());
+  } else {
+    this->send_response_(address, function_code, response_data, response_len);
  }
 }

@@ -455,17 +546,27 @@ float Modbus::get_setup_priority() const {
  return setup_priority::BUS - 1.0f;
 }

-void ModbusServerHub::send(uint8_t address, uint8_t function_code, const std::vector<uint8_t> &payload) {
-  const uint16_t len = static_cast<uint16_t>(2 + payload.size());
-  if (len > MAX_RAW_SIZE) {
-    ESP_LOGE(TAG, "Server send frame too large (%" PRIu16 " bytes)", len);
+void ModbusServerHub::send_response_(uint8_t address, uint8_t function_code, const uint8_t *payload,
+                                     uint16_t payload_len) {
+  // Build the raw frame (address + function code + payload) in a stack buffer; it's consumed
+  // immediately by send_raw_ and a full raw frame never exceeds MAX_RAW_SIZE.
+  if (payload_len + 2 > MAX_RAW_SIZE) {
+    ESP_LOGE(TAG, "Server response too large (%" PRIu16 " bytes)", static_cast<uint16_t>(payload_len + 2));
    return;
  }
  uint8_t raw_frame[MAX_RAW_SIZE];
  raw_frame[0] = address;
  raw_frame[1] = function_code;
-  std::memcpy(raw_frame + 2, payload.data(), payload.size());
-  this->send_raw_(raw_frame, len);
+  std::memcpy(raw_frame + 2, payload, payload_len);
+  this->send_raw_(raw_frame, payload_len + 2);
+}
+
+void ModbusServerHub::send_exception_(uint8_t address, uint8_t function_code, ModbusExceptionCode exception_code) {
+  uint8_t raw_frame[3];
+  raw_frame[0] = address;
+  raw_frame[1] = function_code | FUNCTION_CODE_EXCEPTION_MASK;
+  raw_frame[2] = static_cast<uint8_t>(exception_code);
+  this->send_raw_(raw_frame, 3);
 }

 // Raw send for client: pushes to tx queue. Everything except the CRC must be contained in payload.
@@ -130,22 +130,22 @@ class ModbusServerHub : public Modbus {
 public:
  ModbusServerHub() = default;
  void dump_config() override;
-  void send(uint8_t address, uint8_t function_code, const std::vector<uint8_t> &payload);
-  ESPDEPRECATED("Use ModbusServerDevice::send_raw instead. Removed in 2026.10.0", "2026.4.0")
-  void send_raw(const std::vector<uint8_t> &payload) {
-    this->send_raw_(payload.data(), static_cast<uint16_t>(payload.size()));
-  };
  void register_device(ModbusServerDevice *device) { this->devices_.push_back(device); }

 protected:
-  friend class ModbusServerDevice;
-
  void parse_modbus_frames() override;
  bool parse_modbus_client_frame_();
  // Parsers need to handle standard (ModbusFunctionCode) and custom (uint8_t) function codes, so we use uint8_t here.
  void process_modbus_server_frame(uint8_t address, uint8_t function_code, const uint8_t *data, uint16_t len) override;
-  void process_modbus_client_frame_(uint8_t address, uint8_t function_code, const uint8_t *data, uint16_t len);
+  void process_modbus_client_frame_(uint8_t address, uint8_t function_code, const uint8_t *data);
+  ModbusServerDevice *find_device_(uint8_t address);
+  // Returns true if [start_address, start_address + number_of_registers) fits in the 16-bit address space.
+  // 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);
  void send_raw_(const uint8_t *payload, uint16_t len);
+  void send_exception_(uint8_t address, uint8_t function_code, ModbusExceptionCode exception_code);
+  void send_response_(uint8_t address, uint8_t function_code, const uint8_t *payload, uint16_t payload_len);
  uint8_t expecting_peer_response_{0};
  std::vector<ModbusServerDevice *> devices_;

@@ -200,35 +200,41 @@ class ModbusClientDevice {
 // This is for compatibility with external components using the former class name
 using ModbusDevice = ModbusClientDevice;

+// Result of a server register handler: std::nullopt means success, otherwise the Modbus exception code to return.
+using ServerResponseStatus = std::optional<ModbusExceptionCode>;
+// 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:
-  ModbusServerDevice() = default;
-  ModbusServerDevice(ModbusServerHub *parent, uint8_t address) : parent_(parent), address_(address) {}
  virtual ~ModbusServerDevice() = default;
+  ModbusServerDevice() = default;
+  // Polymorphic base: non-copyable and non-movable to prevent slicing (Rule of Five).
  ModbusServerDevice(const ModbusServerDevice &) = delete;
  ModbusServerDevice &operator=(const ModbusServerDevice &) = delete;
  ModbusServerDevice(ModbusServerDevice &&) = delete;
  ModbusServerDevice &operator=(ModbusServerDevice &&) = delete;
-  void set_parent(ModbusServerHub *parent) { this->parent_ = parent; }
  void set_address(uint8_t address) { this->address_ = address; }
-  virtual void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers){};
-  virtual void on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data){};
-  void send(uint8_t function, const std::vector<uint8_t> &payload) {
-    this->parent_->send(this->address_, function, payload);
-  }
-  void send_raw(const std::vector<uint8_t> &payload) {
-    this->parent_->send_raw_(payload.data(), static_cast<uint16_t>(payload.size()));
-  }
-  void send_error(uint8_t function_code, ModbusExceptionCode exception_code) {
-    uint8_t error_response[3] = {this->address_, uint8_t(function_code | FUNCTION_CODE_EXCEPTION_MASK),
-                                 static_cast<uint8_t>(exception_code)};
-    this->parent_->send_raw_(error_response, 3);
-  }
+  uint8_t get_address() const { return this->address_; }
+  virtual ServerResponseStatus on_modbus_read_registers(uint16_t start_address, uint16_t number_of_registers,
+                                                        RegisterValues &registers) {
+    return ModbusExceptionCode::ILLEGAL_FUNCTION;
+  };
+  virtual ServerResponseStatus on_modbus_read_input_registers(uint16_t start_address, uint16_t number_of_registers,
+                                                              RegisterValues &registers) {
+    return this->on_modbus_read_registers(start_address, number_of_registers, registers);
+  };
+  virtual ServerResponseStatus on_modbus_read_holding_registers(uint16_t start_address, uint16_t number_of_registers,
+                                                                RegisterValues &registers) {
+    return this->on_modbus_read_registers(start_address, number_of_registers, registers);
+  };
+  virtual ServerResponseStatus on_modbus_write_registers(uint16_t start_address, const RegisterValues &registers) {
+    return ModbusExceptionCode::ILLEGAL_FUNCTION;
+  };

 protected:
-  friend ModbusServerHub;
-
-  ModbusServerHub *parent_{nullptr};
  uint8_t address_{0};
 };

@@ -82,7 +82,7 @@ static constexpr uint8_t MAX_NUM_OF_REGISTERS_TO_READ = 125;  // 0x7D
 // Smallest possible frame is 4 bytes (custom function with no data): address(1) + function(1) + CRC(2)
 static constexpr uint16_t MIN_FRAME_SIZE = 4;
 static constexpr uint16_t MAX_PDU_SIZE = 253;  // Max PDU size is 256 - address(1) - CRC(2) = 253
-static constexpr uint16_t MAX_RAW_SIZE = 254;  // Max RAW size is 255 - CRC(2) = 254
+static constexpr uint16_t MAX_RAW_SIZE = 254;  // Max RAW size is 256 - CRC(2) = 254
 static constexpr uint16_t MAX_FRAME_SIZE = 256;
 /// End of Modbus definitions
 }  // namespace esphome::modbus
@@ -101,53 +101,19 @@ static size_t required_payload_size(SensorValueType sensor_value_type) {
  }
 }

-void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type) {
-  switch (value_type) {
-    case SensorValueType::U_WORD:
-    case SensorValueType::S_WORD:
-      data.push_back(value & 0xFFFF);
-      break;
-    case SensorValueType::U_DWORD:
-    case SensorValueType::S_DWORD:
-    case SensorValueType::FP32:
-      data.push_back((value & 0xFFFF0000) >> 16);
-      data.push_back(value & 0xFFFF);
-      break;
-    case SensorValueType::U_DWORD_R:
-    case SensorValueType::S_DWORD_R:
-    case SensorValueType::FP32_R:
-      data.push_back(value & 0xFFFF);
-      data.push_back((value & 0xFFFF0000) >> 16);
-      break;
-    case SensorValueType::U_QWORD:
-    case SensorValueType::S_QWORD:
-      data.push_back((value & 0xFFFF000000000000) >> 48);
-      data.push_back((value & 0xFFFF00000000) >> 32);
-      data.push_back((value & 0xFFFF0000) >> 16);
-      data.push_back(value & 0xFFFF);
-      break;
-    case SensorValueType::U_QWORD_R:
-    case SensorValueType::S_QWORD_R:
-      data.push_back(value & 0xFFFF);
-      data.push_back((value & 0xFFFF0000) >> 16);
-      data.push_back((value & 0xFFFF00000000) >> 32);
-      data.push_back((value & 0xFFFF000000000000) >> 48);
-      break;
-    default:
-      ESP_LOGE(TAG, "Invalid data type for modbus number to payload conversion: %d", static_cast<uint16_t>(value_type));
-      break;
-  }
+void log_unsupported_value_type(SensorValueType value_type) {
+  ESP_LOGE(TAG, "Invalid data type for modbus number to payload conversion: %d", static_cast<uint16_t>(value_type));
 }

-int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
+int64_t payload_to_number(const uint8_t *data, size_t size, SensorValueType sensor_value_type, uint8_t offset,
                          uint32_t bitmask, bool *error_return) {
  int64_t value = 0;  // int64_t because it can hold signed and unsigned 32 bits

  // Validate offset against the buffer for all types, including RAW/unsupported, so
  // a malformed or misconfigured frame still produces an error log.
-  if (static_cast<size_t>(offset) > data.size()) {
+  if (static_cast<size_t>(offset) > size) {
    ESP_LOGE(TAG, "not enough data for value type=%u offset=%u size=%zu", static_cast<unsigned int>(sensor_value_type),
-             static_cast<unsigned int>(offset), data.size());
+             static_cast<unsigned int>(offset), size);
    if (error_return)
      *error_return = true;
    return value;
@@ -158,10 +124,9 @@ int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sens
    return value;
  }

-  if (data.size() - offset < required_size) {
+  if (size - offset < required_size) {
    ESP_LOGE(TAG, "not enough data for value type=%u offset=%u size=%zu required=%zu",
-             static_cast<unsigned int>(sensor_value_type), static_cast<unsigned int>(offset), data.size(),
-             required_size);
+             static_cast<unsigned int>(sensor_value_type), static_cast<unsigned int>(offset), size, required_size);
    if (error_return)
      *error_return = true;
    return value;
@@ -214,6 +179,31 @@ int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sens
  return value;
 }

+int64_t registers_to_number(const uint16_t *registers, size_t count, SensorValueType sensor_value_type,
+                            bool *error_return) {
+  const size_t required_size = required_payload_size(sensor_value_type);
+  if (required_size == 0) {
+    return 0;  // RAW/unsupported: nothing to read
+  }
+  const size_t required_words = required_size / 2;
+  if (required_words > count) {
+    ESP_LOGE(TAG, "not enough registers for value type=%u count=%zu required=%zu",
+             static_cast<unsigned int>(sensor_value_type), count, required_words);
+    if (error_return)
+      *error_return = true;
+    return 0;
+  }
+  // Serialize the needed words back to big-endian bytes and reuse the audited byte decoder so the
+  // sign-extension behaviour stays identical to the wire path.
+  uint8_t bytes[8];  // at most 4 registers (QWORD)
+  for (size_t i = 0; i < required_words; i++) {
+    uint16_t reg = registers[i];
+    bytes[i * 2] = static_cast<uint8_t>(reg >> 8);
+    bytes[i * 2 + 1] = static_cast<uint8_t>(reg & 0xFF);
+  }
+  return payload_to_number(bytes, required_size, sensor_value_type, 0, 0xFFFFFFFF, error_return);
+}
+
 StaticVector<uint8_t, MAX_PDU_SIZE> create_client_pdu(ModbusFunctionCode function_code, uint16_t start_address,
                                                      uint16_t number_of_entities, const uint8_t *values,
                                                      size_t values_len) {
@@ -224,24 +224,77 @@ template<typename N> N mask_and_shift_by_rightbit(N data, uint32_t mask) {
  return 0;
 }

-/** Convert float value to vector<uint16_t> suitable for sending
- * @param data target for payload
- * @param value float value to convert
- * @param value_type defines if 16/32 or FP32 is used
- * @return vector containing the modbus register words in correct order
- */
-void number_to_payload(std::vector<uint16_t> &data, int64_t value, SensorValueType value_type);
+// Logs an error for an unsupported value type. Defined in the .cpp so logging stays out of headers.
+void log_unsupported_value_type(SensorValueType value_type);

-/** Convert vector<uint8_t> response payload to number.
+/** Append the Modbus register words for value to data.
+ * Works with any container exposing push_back(uint16_t) (e.g. std::vector or StaticVector).
+ */
+template<typename Container> void number_to_payload(Container &data, int64_t value, SensorValueType value_type) {
+  switch (value_type) {
+    case SensorValueType::U_WORD:
+    case SensorValueType::S_WORD:
+      data.push_back(value & 0xFFFF);
+      break;
+    case SensorValueType::U_DWORD:
+    case SensorValueType::S_DWORD:
+    case SensorValueType::FP32:
+      data.push_back((value & 0xFFFF0000) >> 16);
+      data.push_back(value & 0xFFFF);
+      break;
+    case SensorValueType::U_DWORD_R:
+    case SensorValueType::S_DWORD_R:
+    case SensorValueType::FP32_R:
+      data.push_back(value & 0xFFFF);
+      data.push_back((value & 0xFFFF0000) >> 16);
+      break;
+    case SensorValueType::U_QWORD:
+    case SensorValueType::S_QWORD:
+      data.push_back((value & 0xFFFF000000000000) >> 48);
+      data.push_back((value & 0xFFFF00000000) >> 32);
+      data.push_back((value & 0xFFFF0000) >> 16);
+      data.push_back(value & 0xFFFF);
+      break;
+    case SensorValueType::U_QWORD_R:
+    case SensorValueType::S_QWORD_R:
+      data.push_back(value & 0xFFFF);
+      data.push_back((value & 0xFFFF0000) >> 16);
+      data.push_back((value & 0xFFFF00000000) >> 32);
+      data.push_back((value & 0xFFFF000000000000) >> 48);
+      break;
+    default:
+      log_unsupported_value_type(value_type);
+      break;
+  }
+}
+
+/** Convert a raw response payload to a number.
 * @param data payload with the data to convert
+ * @param size number of bytes available in data
 * @param sensor_value_type defines if 16/32/64 bits or FP32 is used
 * @param offset offset to the data in data
 * @param bitmask bitmask used for masking and shifting
 * @return 64-bit number of the payload
 */
-int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
+int64_t payload_to_number(const uint8_t *data, size_t size, SensorValueType sensor_value_type, uint8_t offset,
                          uint32_t bitmask, bool *error_return = nullptr);

+/** Convert vector<uint8_t> response payload to number. */
+inline int64_t payload_to_number(const std::vector<uint8_t> &data, SensorValueType sensor_value_type, uint8_t offset,
+                                 uint32_t bitmask, bool *error_return = nullptr) {
+  return payload_to_number(data.data(), data.size(), sensor_value_type, offset, bitmask, error_return);
+}
+
+/** Reconstruct a number from register words (host byte order). Inverse of number_to_payload.
+ * Decodes the value at the start of the given span; advance the pointer to read successive values.
+ * @param registers register values in host byte order
+ * @param count number of registers available in registers
+ * @param sensor_value_type defines if 16/32/64 bits or FP32 is used
+ * @return 64-bit number of the registers
+ */
+int64_t registers_to_number(const uint16_t *registers, size_t count, SensorValueType sensor_value_type,
+                            bool *error_return = nullptr);
+
 /** Create a modbus clinet pdu for reading/writing single/multiple coils/register/inputs.
 * @param function_code the modbus function code to use. One of:
 * READ_COILS
@@ -3,26 +3,18 @@
 #include "esphome/core/log.h"

 namespace esphome::modbus_server {
-using modbus::ModbusFunctionCode;
 using modbus::ModbusExceptionCode;
-using modbus::helpers::payload_to_number;
+using modbus::helpers::registers_to_number;

 static const char *const TAG = "modbus_server";

-void ModbusServer::on_modbus_read_registers(uint8_t function_code, uint16_t start_address,
-                                            uint16_t number_of_registers) {
+modbus::ServerResponseStatus ModbusServer::on_modbus_read_registers(uint16_t start_address,
+                                                                    uint16_t number_of_registers,
+                                                                    modbus::RegisterValues &registers) {
  ESP_LOGV(TAG,
-           "Received read holding/input registers for device 0x%X. FC: 0x%X. Start address: 0x%X. Number of registers: "
-           "0x%X.",
-           this->address_, function_code, start_address, number_of_registers);
+           "Received read holding/input registers for device 0x%X. Start address: 0x%X. Number of registers: 0x%X.",
+           this->address_, start_address, number_of_registers);

-  if (number_of_registers == 0 || number_of_registers > modbus::MAX_NUM_OF_REGISTERS_TO_READ) {
-    ESP_LOGW(TAG, "Invalid number of registers %" PRIu16 ". Sending exception response.", number_of_registers);
-    this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_ADDRESS);
-    return;
-  }
-
-  std::vector<uint16_t> sixteen_bit_response;
  for (uint16_t current_address = start_address; current_address < start_address + number_of_registers;) {
    bool found = false;
    for (auto *server_register : this->server_registers_) {
@@ -36,10 +28,7 @@ void ModbusServer::on_modbus_read_registers(uint8_t function_code, uint16_t star
                 server_register->address, static_cast<size_t>(server_register->value_type),
                 server_register->register_count, server_register->format_value(value, value_buf, sizeof(value_buf)));

-        std::vector<uint16_t> payload;
-        payload.reserve(server_register->register_count * 2);
-        modbus::helpers::number_to_payload(payload, value, server_register->value_type);
-        sixteen_bit_response.insert(sixteen_bit_response.end(), payload.cbegin(), payload.cend());
+        modbus::helpers::number_to_payload(registers, value, server_register->value_type);
        current_address += server_register->register_count;
        found = true;
        break;
@@ -53,92 +42,37 @@ void ModbusServer::on_modbus_read_registers(uint8_t function_code, uint16_t star
                 "Could not match any register to address 0x%02X, but default allowed. "
                 "Returning default value: %" PRIu16 ".",
                 current_address, this->server_courtesy_response_.register_value);
-        sixteen_bit_response.push_back(this->server_courtesy_response_.register_value);
+        registers.push_back(this->server_courtesy_response_.register_value);
        current_address += 1;  // Just increment by 1, as the default response is a single register
      } else {
        ESP_LOGW(TAG,
                 "Could not match any register to address 0x%02X and default not allowed. Sending exception response.",
                 current_address);
-        this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_ADDRESS);
-        return;
+        return ModbusExceptionCode::ILLEGAL_DATA_ADDRESS;
      }
    }
  }

-  std::vector<uint8_t> response;
-  if (number_of_registers != sixteen_bit_response.size())
-    ESP_LOGW(TAG, "Response size not matched to request register count.");
-  response.push_back(sixteen_bit_response.size() * 2);  // actual byte count
-  for (auto v : sixteen_bit_response) {
-    auto decoded_value = decode_value(v);
-    response.push_back(decoded_value[0]);
-    response.push_back(decoded_value[1]);
-  }
-  this->send(function_code, response);
+  return {};
 }

-void ModbusServer::on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data) {
-  uint16_t number_of_registers;
-  uint16_t payload_offset;
+modbus::ServerResponseStatus ModbusServer::on_modbus_write_registers(uint16_t start_address,
+                                                                     const modbus::RegisterValues &registers) {
+  // registers holds the values to write in host byte order; its size is the register count.
+  ESP_LOGV(TAG, "Received write registers for device 0x%X. Start address: 0x%X. Number of registers: 0x%zX.",
+           this->address_, start_address, registers.size());

-  if (static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::WRITE_MULTIPLE_REGISTERS) {
-    if (data.size() < 5) {
-      ESP_LOGW(TAG, "Write multiple registers data too short (%zu bytes)", data.size());
-      this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
-      return;
-    }
-    number_of_registers = uint16_t(data[3]) | (uint16_t(data[2]) << 8);
-    if (number_of_registers == 0 || number_of_registers > modbus::MAX_NUM_OF_REGISTERS_TO_WRITE) {
-      ESP_LOGW(TAG, "Invalid number of registers %" PRIu16 ". Sending exception response.", number_of_registers);
-      this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
-      return;
-    }
-    uint16_t payload_size = data[4];
-    if (payload_size != number_of_registers * 2) {
-      ESP_LOGW(TAG,
-               "Payload size of %" PRIu16 " bytes is not 2 times the number of registers (%" PRIu16
-               "). Sending exception response.",
-               payload_size, number_of_registers);
-      this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
-      return;
-    }
-    if (data.size() < 5 + payload_size) {
-      ESP_LOGW(TAG, "Write multiple registers payload truncated (%zu bytes, expected %u)", data.size(),
-               5 + payload_size);
-      this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
-      return;
-    }
-    payload_offset = 5;
-  } else if (static_cast<ModbusFunctionCode>(function_code) == ModbusFunctionCode::WRITE_SINGLE_REGISTER) {
-    if (data.size() < 4) {
-      ESP_LOGW(TAG, "Write single register data too short (%zu bytes)", data.size());
-      this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_VALUE);
-      return;
-    }
-    number_of_registers = 1;
-    payload_offset = 2;
-  } else {
-    ESP_LOGW(TAG, "Invalid function code 0x%X. Sending exception response.", function_code);
-    this->send_error(function_code, ModbusExceptionCode::ILLEGAL_FUNCTION);
-    return;
-  }
-
-  uint16_t start_address = uint16_t(data[1]) | (uint16_t(data[0]) << 8);
-  ESP_LOGD(TAG,
-           "Received write holding registers for device 0x%X. FC: 0x%X. Start address: 0x%X. Number of registers: "
-           "0x%X.",
-           this->address_, function_code, start_address, number_of_registers);
-
-  auto for_each_register = [this, start_address, number_of_registers, payload_offset](
-                               const std::function<bool(ServerRegister *, uint16_t offset)> &callback) -> bool {
-    uint16_t offset = payload_offset;
-    for (uint16_t current_address = start_address; current_address < start_address + number_of_registers;) {
+  auto for_each_register =
+      [this, start_address,
+       &registers](const std::function<bool(ServerRegister *, uint16_t register_offset)> &callback) -> bool {
+    uint16_t register_offset = 0;
+    for (uint32_t current_address = start_address; current_address < start_address + registers.size();) {
      bool ok = false;
      for (auto *server_register : this->server_registers_) {
        if (server_register->address == current_address) {
-          ok = callback(server_register, offset);
+          ok = callback(server_register, register_offset);
          current_address += server_register->register_count;
-          offset += server_register->register_count * sizeof(uint16_t);
+          register_offset += server_register->register_count;
          break;
        }
      }
@@ -150,36 +84,41 @@ void ModbusServer::on_modbus_write_registers(uint8_t function_code, const std::v
    return true;
  };

-  // check all registers are writable before writing to any of them:
-  if (!for_each_register([](ServerRegister *server_register, uint16_t offset) -> bool {
-        return server_register->write_lambda != nullptr;
-      })) {
-    ESP_LOGW(TAG, "Invalid register address. Sending exception response.");
-    this->send_error(function_code, ModbusExceptionCode::ILLEGAL_DATA_ADDRESS);
-    return;
-  }
-
-  // Actually write to the registers:
-  if (!for_each_register([&data](ServerRegister *server_register, uint16_t offset) {
-        bool error = false;
-        int64_t number = payload_to_number(data, server_register->value_type, offset, 0xFFFFFFFF, &error);
-        if (error) {
-          return false;
-        } else {
-          return server_register->write_lambda(number);
+  // Pre-flight: every targeted register must be writable AND have its full value present in the request,
+  // so we never apply a partial write before discovering a problem. The commit pass below re-runs
+  // registers_to_number rather than caching the decoded values: using the same function for the check and
+  // the write keeps a single source of truth for the decode bound, independent of how register_count was set.
+  ModbusExceptionCode precheck = ModbusExceptionCode::ILLEGAL_DATA_ADDRESS;  // unmatched or unwritable register
+  if (!for_each_register([&precheck, &registers](ServerRegister *server_register, uint16_t register_offset) -> bool {
+        if (server_register->write_lambda == nullptr) {
+          return false;  // unwritable -> ILLEGAL_DATA_ADDRESS
        }
+        bool error = false;
+        registers_to_number(registers.data() + register_offset, registers.size() - register_offset,
+                            server_register->value_type, &error);
+        if (error) {
+          precheck = ModbusExceptionCode::ILLEGAL_DATA_VALUE;  // request doesn't supply the full value
+          return false;
+        }
+        return true;
      })) {
-    ESP_LOGW(TAG, "Could not write all registers. Sending exception response.");
-    this->send_error(function_code, ModbusExceptionCode::SERVICE_DEVICE_FAILURE);
-    return;
+    ESP_LOGW(TAG, "Write request rejected before applying any register. Sending exception response.");
+    return precheck;
  }

-  std::vector<uint8_t> response;
-  response.reserve(6);
-  response.push_back(this->address_);
-  response.push_back(function_code);
-  response.insert(response.end(), data.begin(), data.begin() + 4);
-  this->send_raw(response);
+  // Commit: every value is known writable and decodable, so the only failure now is a user write callback
+  // rejecting the value at runtime -- which cannot be rolled back.
+  if (!for_each_register([&registers](ServerRegister *server_register, uint16_t register_offset) {
+        int64_t number = registers_to_number(registers.data() + register_offset, registers.size() - register_offset,
+                                             server_register->value_type);
+        return server_register->write_lambda(number);
+      })) {
+    ESP_LOGW(TAG, "A register write callback failed mid-sequence; earlier writes were already applied.");
+    return ModbusExceptionCode::SERVICE_DEVICE_FAILURE;
+  }
+
+  // Success: the caller builds the write response (an echo of the request header).
+  return {};
 }

 void ModbusServer::dump_config() {
@@ -98,9 +98,11 @@ class ModbusServer : public Component, public modbus::ModbusServerDevice {
  /// Registers a server register with the controller. Called by esphomes code generator
  void add_server_register(ServerRegister *server_register) { server_registers_.push_back(server_register); }
  /// called when a modbus request (function code 0x03 or 0x04) was parsed without errors
-  void on_modbus_read_registers(uint8_t function_code, uint16_t start_address, uint16_t number_of_registers) final;
+  modbus::ServerResponseStatus on_modbus_read_registers(uint16_t start_address, uint16_t number_of_registers,
+                                                        modbus::RegisterValues &registers) final;
  /// called when a modbus request (function code 0x06 or 0x10) was parsed without errors
-  void on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data) final;
+  modbus::ServerResponseStatus on_modbus_write_registers(uint16_t start_address,
+                                                         const modbus::RegisterValues &registers) final;
  /// Called by esphome generated code to set the server courtesy response object
  void set_server_courtesy_response(const ServerCourtesyResponse &server_courtesy_response) {
    this->server_courtesy_response_ = server_courtesy_response;
@@ -194,4 +194,40 @@ TEST(ModbusHelpersTest, PayloadToNumberDecodesValidWord) {
  EXPECT_EQ(payload_to_number(data, SensorValueType::U_WORD, 0, 0xFFFFFFFF), 0x1234);
 }

+// --- registers_to_number ---------------------------------------------------
+// Register words are host byte order; results must match the byte-based payload_to_number.
+
+TEST(ModbusHelpersTest, RegistersToNumberDecodesWord) {
+  const uint16_t registers[] = {0x1234};
+  EXPECT_EQ(registers_to_number(registers, 1, SensorValueType::U_WORD), 0x1234);
+}
+
+TEST(ModbusHelpersTest, RegistersToNumberDecodesDwordHighWordFirst) {
+  const uint16_t registers[] = {0x1234, 0x5678};
+  EXPECT_EQ(registers_to_number(registers, 2, SensorValueType::U_DWORD), 0x12345678);
+}
+
+TEST(ModbusHelpersTest, RegistersToNumberDecodesAtSpanStart) {
+  // The function decodes the value at the start of the span; the caller advances the pointer.
+  const uint16_t registers[] = {0xAAAA, 0x1234};
+  EXPECT_EQ(registers_to_number(registers + 1, 1, SensorValueType::U_WORD), 0x1234);
+}
+
+TEST(ModbusHelpersTest, RegistersToNumberMatchesPayloadToNumber) {
+  // Same value via both decoders: registers (host order) vs big-endian bytes.
+  const uint16_t registers[] = {0x8001, 0x0002};
+  const std::vector<uint8_t> bytes{0x80, 0x01, 0x00, 0x02};
+  for (auto value_type : {SensorValueType::S_DWORD, SensorValueType::U_DWORD, SensorValueType::S_DWORD_R}) {
+    EXPECT_EQ(registers_to_number(registers, 2, value_type), payload_to_number(bytes, value_type, 0, 0xFFFFFFFF))
+        << "value_type=" << static_cast<int>(value_type);
+  }
+}
+
+TEST(ModbusHelpersTest, RegistersToNumberRejectsTruncatedMultiRegisterValue) {
+  const uint16_t registers[] = {0x1234};
+  bool error = false;
+  EXPECT_EQ(registers_to_number(registers, 1, SensorValueType::U_DWORD, &error), 0);
+  EXPECT_TRUE(error);
+}
+
 }  // namespace esphome::modbus::helpers
@@ -0,0 +1,124 @@
+#include <gtest/gtest.h>
+
+#include "esphome/components/modbus_server/modbus_server.h"
+
+namespace esphome::modbus_server {
+
+using modbus::ModbusExceptionCode;
+using modbus::RegisterValues;
+
+namespace {
+
+RegisterValues make_registers(std::initializer_list<uint16_t> values) {
+  RegisterValues registers;
+  for (uint16_t value : values)
+    registers.push_back(value);
+  return registers;
+}
+
+}  // namespace
+
+// A single writable WORD register is applied and the handler reports success (nullopt).
+TEST(ModbusServerWrite, SingleWordSucceeds) {
+  ModbusServer server;
+  int64_t written = -1;
+  ServerRegister reg(0x0000, SensorValueType::U_WORD, 1);
+  reg.write_lambda = [&written](int64_t value) {
+    written = value;
+    return true;
+  };
+  server.add_server_register(&reg);
+
+  auto status = server.on_modbus_write_registers(0x0000, make_registers({0x1234}));
+  EXPECT_FALSE(status.has_value());  // nullopt == success
+  EXPECT_EQ(written, 0x1234);
+}
+
+// A multi-register value is decoded high word first and applied as a single number.
+TEST(ModbusServerWrite, DwordSucceeds) {
+  ModbusServer server;
+  int64_t written = -1;
+  ServerRegister reg(0x0000, SensorValueType::U_DWORD, 2);
+  reg.write_lambda = [&written](int64_t value) {
+    written = value;
+    return true;
+  };
+  server.add_server_register(&reg);
+
+  auto status = server.on_modbus_write_registers(0x0000, make_registers({0x1234, 0x5678}));
+  EXPECT_FALSE(status.has_value());
+  EXPECT_EQ(written, 0x12345678);
+}
+
+// Regression: a request that under-supplies a multi-register value is rejected before any
+// write_lambda runs, so no register is partially written.
+TEST(ModbusServerWrite, UnderSuppliedValueAppliesNothing) {
+  ModbusServer server;
+  bool word_written = false;
+  ServerRegister word_reg(0x0000, SensorValueType::U_WORD, 1);
+  word_reg.write_lambda = [&word_written](int64_t) {
+    word_written = true;
+    return true;
+  };
+  bool dword_written = false;
+  ServerRegister dword_reg(0x0001, SensorValueType::U_DWORD, 2);  // needs two registers
+  dword_reg.write_lambda = [&dword_written](int64_t) {
+    dword_written = true;
+    return true;
+  };
+  server.add_server_register(&word_reg);
+  server.add_server_register(&dword_reg);
+
+  // Two words supplied: one for the WORD at 0x0000, but only one of the two the DWORD at 0x0001 needs.
+  auto status = server.on_modbus_write_registers(0x0000, make_registers({0x1111, 0x2222}));
+  ASSERT_TRUE(status.has_value());
+  if (status.has_value())
+    EXPECT_EQ(status.value(), ModbusExceptionCode::ILLEGAL_DATA_VALUE);
+  EXPECT_FALSE(word_written);  // the writable WORD must NOT have been applied
+  EXPECT_FALSE(dword_written);
+}
+
+// A read-only register (no write_lambda) yields ILLEGAL_DATA_ADDRESS and applies nothing.
+TEST(ModbusServerWrite, UnwritableRegisterRejected) {
+  ModbusServer server;
+  ServerRegister read_only(0x0000, SensorValueType::U_WORD, 1);  // no write_lambda set
+  server.add_server_register(&read_only);
+
+  auto status = server.on_modbus_write_registers(0x0000, make_registers({0x1234}));
+  ASSERT_TRUE(status.has_value());
+  if (status.has_value())
+    EXPECT_EQ(status.value(), ModbusExceptionCode::ILLEGAL_DATA_ADDRESS);
+}
+
+// An address with no registered register yields ILLEGAL_DATA_ADDRESS.
+TEST(ModbusServerWrite, UnmatchedAddressRejected) {
+  ModbusServer server;
+  auto status = server.on_modbus_write_registers(0x0005, make_registers({0x1234}));
+  ASSERT_TRUE(status.has_value());
+  if (status.has_value())
+    EXPECT_EQ(status.value(), ModbusExceptionCode::ILLEGAL_DATA_ADDRESS);
+}
+
+// A write_lambda failing at runtime is the one non-atomic case: the earlier register is already
+// applied, and the handler reports SERVICE_DEVICE_FAILURE.
+TEST(ModbusServerWrite, CallbackFailureIsServiceDeviceFailure) {
+  ModbusServer server;
+  bool first_written = false;
+  ServerRegister first(0x0000, SensorValueType::U_WORD, 1);
+  first.write_lambda = [&first_written](int64_t) {
+    first_written = true;
+    return true;
+  };
+  ServerRegister second(0x0001, SensorValueType::U_WORD, 1);
+  second.write_lambda = [](int64_t) { return false; };  // rejects at runtime
+  server.add_server_register(&first);
+  server.add_server_register(&second);
+
+  auto status = server.on_modbus_write_registers(0x0000, make_registers({0xAAAA, 0xBBBB}));
+  ASSERT_TRUE(status.has_value());
+  if (status.has_value())
+    EXPECT_EQ(status.value(), ModbusExceptionCode::SERVICE_DEVICE_FAILURE);
+  EXPECT_TRUE(first_written);  // pre-validation passed, so the first write applied before the failure
+}
+
+}  // namespace esphome::modbus_server