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

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