github/esphome
2
0
Fork 0
mirror of https://github.com/esphome/esphome.git synced 2026-10-06 02:51:29 +00:00
Code Issues Packages Projects Releases Wiki Activity

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

Browse Source 
Co-authored-by: J. Nick Koston <nick@koston.org>
This commit is contained in:
Josef Zweck
2026-08-07 14:17:11 -05:00
committed by GitHub
co-authored by J. Nick Koston
parent 2e1c517821
commit c24e61439b
7 changed files with 407 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esphome/components/modbus/modbus.cpp
+92 -17
View File
@@ -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);