[modbus] Modbus server role: write holding registers (#9156)

esphome/components/modbus/modbus.cpp

@@ -90,15 +90,24 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) {
 
   } else {
     // data starts at 2 and length is 4 for read registers commands
-    if (this->role == ModbusRole::SERVER && (function_code == 0x1 || function_code == 0x3 || function_code == 0x4)) {
+    if (this->role == ModbusRole::SERVER) {
-      data_offset = 2;
+      if (function_code == 0x1 || function_code == 0x3 || function_code == 0x4 || function_code == 0x6) {
-      data_len = 4;
+        data_offset = 2;
-    }
+        data_len = 4;
-
+      } else if (function_code == 0x10) {
-    // the response for write command mirrors the requests and data starts at offset 2 instead of 3 for read commands
+        if (at < 6) {
-    if (function_code == 0x5 || function_code == 0x06 || function_code == 0xF || function_code == 0x10) {
+          return true;
-      data_offset = 2;
+        }
-      data_len = 4;
+        data_offset = 2;
+        // starting address (2 bytes) + quantity of registers (2 bytes) + byte count itself (1 byte) + actual byte count
+        data_len = 2 + 2 + 1 + raw[6];
+      }
+    } else {
+      // the response for write command mirrors the requests and data starts at offset 2 instead of 3 for read commands
+      if (function_code == 0x5 || function_code == 0x06 || function_code == 0xF || function_code == 0x10) {
+        data_offset = 2;
+        data_len = 4;
+      }
     }
 
     // Error ( msb indicates error )
@@ -132,6 +141,7 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) {
   bool found = false;
   for (auto *device : this->devices_) {
     if (device->address_ == address) {
+      found = true;
       // Is it an error response?
       if ((function_code & 0x80) == 0x80) {
         ESP_LOGD(TAG, "Modbus error function code: 0x%X exception: %d", function_code, raw[2]);
@@ -141,13 +151,21 @@ bool Modbus::parse_modbus_byte_(uint8_t byte) {
           // Ignore modbus exception not related to a pending command
           ESP_LOGD(TAG, "Ignoring Modbus error - not expecting a response");
         }
-      } else if (this->role == ModbusRole::SERVER && (function_code == 0x3 || function_code == 0x4)) {
+        continue;
-        device->on_modbus_read_registers(function_code, uint16_t(data[1]) | (uint16_t(data[0]) << 8),
-                                         uint16_t(data[3]) | (uint16_t(data[2]) << 8));
-      } else {
-        device->on_modbus_data(data);
       }
-      found = true;
+      if (this->role == ModbusRole::SERVER) {
+        if (function_code == 0x3 || function_code == 0x4) {
+          device->on_modbus_read_registers(function_code, uint16_t(data[1]) | (uint16_t(data[0]) << 8),
+                                           uint16_t(data[3]) | (uint16_t(data[2]) << 8));
+          continue;
+        }
+        if (function_code == 0x6 || function_code == 0x10) {
+          device->on_modbus_write_registers(function_code, data);
+          continue;
+        }
+      }
+      // fallthrough for other function codes
+      device->on_modbus_data(data);
     }
   }
   waiting_for_response = 0;

esphome/components/modbus/modbus.h

@@ -59,6 +59,7 @@ class ModbusDevice {
   virtual void on_modbus_data(const std::vector<uint8_t> &data) = 0;
   virtual void on_modbus_error(uint8_t function_code, uint8_t exception_code) {}
   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, uint16_t start_address, uint16_t number_of_entities, uint8_t payload_len = 0,
             const uint8_t *payload = nullptr) {
     this->parent_->send(this->address_, function, start_address, number_of_entities, payload_len, payload);

esphome/components/modbus_controller/__init__.py

@@ -39,6 +39,7 @@ CODEOWNERS = ["@martgras"]
 AUTO_LOAD = ["modbus"]
 
 CONF_READ_LAMBDA = "read_lambda"
+CONF_WRITE_LAMBDA = "write_lambda"
 CONF_SERVER_REGISTERS = "server_registers"
 MULTI_CONF = True
 
@@ -148,6 +149,7 @@ ModbusServerRegisterSchema = cv.Schema(
         cv.Required(CONF_ADDRESS): cv.positive_int,
         cv.Optional(CONF_VALUE_TYPE, default="U_WORD"): cv.enum(SENSOR_VALUE_TYPE),
         cv.Required(CONF_READ_LAMBDA): cv.returning_lambda,
+        cv.Optional(CONF_WRITE_LAMBDA): cv.returning_lambda,
     }
 )
 
@@ -318,6 +320,17 @@ async def to_code(config):
                     ),
                 )
             )
+            if CONF_WRITE_LAMBDA in server_register:
+                cg.add(
+                    server_register_var.set_write_lambda(
+                        cg.TemplateArguments(cpp_type),
+                        await cg.process_lambda(
+                            server_register[CONF_WRITE_LAMBDA],
+                            parameters=[(cg.uint16, "address"), (cpp_type, "x")],
+                            return_type=cg.bool_,
+                        ),
+                    )
+                )
             cg.add(var.add_server_register(server_register_var))
     await register_modbus_device(var, config)
     for conf in config.get(CONF_ON_COMMAND_SENT, []):

esphome/components/modbus_controller/modbus_controller.cpp

@@ -152,6 +152,86 @@ void ModbusController::on_modbus_read_registers(uint8_t function_code, uint16_t
   this->send(function_code, start_address, number_of_registers, response.size(), response.data());
 }
 
+void ModbusController::on_modbus_write_registers(uint8_t function_code, const std::vector<uint8_t> &data) {
+  uint16_t number_of_registers;
+  uint16_t payload_offset;
+
+  if (function_code == 0x10) {
+    number_of_registers = uint16_t(data[3]) | (uint16_t(data[2]) << 8);
+    if (number_of_registers == 0 || number_of_registers > 0x7B) {
+      ESP_LOGW(TAG, "Invalid number of registers %d. Sending exception response.", number_of_registers);
+      send_error(function_code, 3);
+      return;
+    }
+    uint16_t payload_size = data[4];
+    if (payload_size != number_of_registers * 2) {
+      ESP_LOGW(TAG, "Payload size of %d bytes is not 2 times the number of registers (%d). Sending exception response.",
+               payload_size, number_of_registers);
+      send_error(function_code, 3);
+      return;
+    }
+    payload_offset = 5;
+  } else if (function_code == 0x06) {
+    number_of_registers = 1;
+    payload_offset = 2;
+  } else {
+    ESP_LOGW(TAG, "Invalid function code 0x%X. Sending exception response.", function_code);
+    send_error(function_code, 1);
+    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;) {
+      bool ok = false;
+      for (auto *server_register : this->server_registers_) {
+        if (server_register->address == current_address) {
+          ok = callback(server_register, offset);
+          current_address += server_register->register_count;
+          offset += server_register->register_count * sizeof(uint16_t);
+          break;
+        }
+      }
+
+      if (!ok) {
+        return false;
+      }
+    }
+    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;
+      })) {
+    send_error(function_code, 1);
+    return;
+  }
+
+  // Actually write to the registers:
+  if (!for_each_register([&data](ServerRegister *server_register, uint16_t offset) {
+        int64_t number = payload_to_number(data, server_register->value_type, offset, 0xFFFFFFFF);
+        return server_register->write_lambda(number);
+      })) {
+    send_error(function_code, 4);
+    return;
+  }
+
+  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);
+}
+
 SensorSet ModbusController::find_sensors_(ModbusRegisterType register_type, uint16_t start_address) const {
   auto reg_it = std::find_if(
       std::begin(this->register_ranges_), std::end(this->register_ranges_),

esphome/components/modbus_controller/modbus_controller.h

@@ -258,6 +258,7 @@ class SensorItem {
 
 class ServerRegister {
   using ReadLambda = std::function<int64_t()>;
+  using WriteLambda = std::function<bool(int64_t value)>;
 
  public:
   ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count) {
@@ -277,6 +278,17 @@ class ServerRegister {
     };
   }
 
+  template<typename T>
+  void set_write_lambda(const std::function<bool(uint16_t address, const T v)> &&user_write_lambda) {
+    this->write_lambda = [this, user_write_lambda](int64_t number) {
+      if constexpr (std::is_same_v<T, float>) {
+        float float_value = bit_cast<float>(static_cast<uint32_t>(number));
+        return user_write_lambda(this->address, float_value);
+      }
+      return user_write_lambda(this->address, static_cast<T>(number));
+    };
+  }
+
   // Formats a raw value into a string representation based on the value type for debugging
   std::string format_value(int64_t value) const {
     switch (this->value_type) {
@@ -304,6 +316,7 @@ class ServerRegister {
   SensorValueType value_type{SensorValueType::RAW};
   uint8_t register_count{0};
   ReadLambda read_lambda;
+  WriteLambda write_lambda;
 };
 
 // ModbusController::create_register_ranges_ tries to optimize register range
@@ -485,6 +498,8 @@ class ModbusController : public PollingComponent, public modbus::ModbusDevice {
   void on_modbus_error(uint8_t function_code, uint8_t exception_code) override;
   /// 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;
+  /// 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;
   /// default delegate called by process_modbus_data when a response has retrieved from the incoming queue
   void on_register_data(ModbusRegisterType register_type, uint16_t start_address, const std::vector<uint8_t> &data);
   /// default delegate called by process_modbus_data when a response for a write response has retrieved from the

tests/components/modbus_controller/common.yaml

@@ -33,7 +33,18 @@ modbus_controller:
         read_lambda: |-
           return 42.3;
     max_cmd_retries: 0
-
+  - id: modbus_controller3
+    address: 0x3
+    modbus_id: mod_bus2
+    server_registers:
+      - address: 0x0009
+        value_type: S_DWORD
+        read_lambda: |-
+          return 31;
+        write_lambda: |-
+          printf("address=%d, value=%d", x);
+          return true;
+    max_cmd_retries: 0
 binary_sensor:
   - platform: modbus_controller
     modbus_controller_id: modbus_controller1