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_server] Simplify server response handling (#12376)

Browse Source 
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>
This commit is contained in:
Bonne Eggleston
2026-06-28 19:41:47 +00:00
committed by GitHub
co-authored by Claude Opus 4.8 J. Nick Koston J. Nick Koston
parent 2f32c88ae5
commit 4ebecf514a
10 changed files with 476 additions and 226 deletions
Download Patch File Download Diff File Expand all files Collapse all files
esphome/components/modbus/modbus.cpp
+131 -30
View File
@@ -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.