diff --git a/esphome/components/modbus/modbus.cpp b/esphome/components/modbus/modbus.cpp index c2efa93fae..6350f43ef6 100644 --- a/esphome/components/modbus/modbus.cpp +++ b/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)) { - data_offset = 2; - data_len = 4; - } - - // 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; + if (this->role == ModbusRole::SERVER) { + if (function_code == 0x1 || function_code == 0x3 || function_code == 0x4 || function_code == 0x6) { + data_offset = 2; + data_len = 4; + } else if (function_code == 0x10) { + if (at < 6) { + return true; + } + 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)) { - 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); + continue; } - 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; diff --git a/esphome/components/modbus/modbus.h b/esphome/components/modbus/modbus.h index aebdbccc78..ec35612690 100644 --- a/esphome/components/modbus/modbus.h +++ b/esphome/components/modbus/modbus.h @@ -59,6 +59,7 @@ class ModbusDevice { virtual void on_modbus_data(const std::vector &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 &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); diff --git a/esphome/components/modbus_controller/__init__.py b/esphome/components/modbus_controller/__init__.py index 8079b824b0..5ab82f5e17 100644 --- a/esphome/components/modbus_controller/__init__.py +++ b/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, []): diff --git a/esphome/components/modbus_controller/modbus_controller.cpp b/esphome/components/modbus_controller/modbus_controller.cpp index 81e9ccf0a6..0f3ddf920d 100644 --- a/esphome/components/modbus_controller/modbus_controller.cpp +++ b/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 &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 &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 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_), diff --git a/esphome/components/modbus_controller/modbus_controller.h b/esphome/components/modbus_controller/modbus_controller.h index 11d27c4025..a86ad1ccb5 100644 --- a/esphome/components/modbus_controller/modbus_controller.h +++ b/esphome/components/modbus_controller/modbus_controller.h @@ -258,6 +258,7 @@ class SensorItem { class ServerRegister { using ReadLambda = std::function; + using WriteLambda = std::function; public: ServerRegister(uint16_t address, SensorValueType value_type, uint8_t register_count) { @@ -277,6 +278,17 @@ class ServerRegister { }; } + template + void set_write_lambda(const std::function &&user_write_lambda) { + this->write_lambda = [this, user_write_lambda](int64_t number) { + if constexpr (std::is_same_v) { + float float_value = bit_cast(static_cast(number)); + return user_write_lambda(this->address, float_value); + } + return user_write_lambda(this->address, static_cast(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 &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 &data); /// default delegate called by process_modbus_data when a response for a write response has retrieved from the diff --git a/tests/components/modbus_controller/common.yaml b/tests/components/modbus_controller/common.yaml index 7fa9f8dae3..7d342ee353 100644 --- a/tests/components/modbus_controller/common.yaml +++ b/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