From 0897a34fae9ec1d9cb172d5fec2fa7376e4580f7 Mon Sep 17 00:00:00 2001
From: babayaga <cgoflyn@gmail.com>
Date: Sun, 25 May 2025 10:10:01 +0200
Subject: [PATCH] mb tcp fix

---
 src/App.cpp              |  2 +-
 src/modbus/ModbusTCP.cpp | 35 +++++++++++++++++++++++------------
 2 files changed, 24 insertions(+), 13 deletions(-)

diff --git a/src/App.cpp b/src/App.cpp
index cb62e197..1f444153 100644
--- a/src/App.cpp
+++ b/src/App.cpp
@@ -135,7 +135,7 @@ short App::info()
     return E_OK;
 }
 
-Component *App::byId(ushort id) 
+Component *App::byId(ushort id)
 {
     short s = components.size();
     for (short i = 0; i < s; i++)
diff --git a/src/modbus/ModbusTCP.cpp b/src/modbus/ModbusTCP.cpp
index 280329c4..093f7671 100644
--- a/src/modbus/ModbusTCP.cpp
+++ b/src/modbus/ModbusTCP.cpp
@@ -117,13 +117,14 @@ short ModbusTCP::setup()
     // READ_HOLDING_REGISTERS worker (FC=03)
     auto readHregsWorker = [this](ModbusMessage request) -> ModbusMessage
     {
+        Log.verboseln("ModbusTCP: Read HReg worker called");
         ModbusMessage response;
         uint16_t address = 0;
         uint16_t count = 0;
         request.get(2, address);
         request.get(4, count);
         if (count == 0 || count > 125)
-        { 
+        {
             Log.warningln("ModbusTCP: Read HReg worker called for %d registers starting at %d - Invalid count %d", count, address);
             response.setError(request.getServerID(), request.getFunctionCode(), Modbus::ILLEGAL_DATA_VALUE);
             return response;
@@ -133,12 +134,23 @@ short ModbusTCP::setup()
         {
             uint16_t currentAddr = address + i;
             MB_Registers *reg = this->findMappingForAddress(currentAddr, FN_READ_HOLD_REGISTER);
-            uint16_t regValue = 0; // Default to 0
-            Component *target = owner->byId(reg->componentId);
-            if (target)
+            if (reg)
             {
-                regValue = (uint16_t)target->mb_tcp_read(reg);
-                response.add(regValue);
+                uint16_t regValue = 0;
+                Component *target = owner->byId(reg->componentId);
+                if (target)
+                {
+                    regValue = (uint16_t)target->mb_tcp_read(reg);
+                    response.add(regValue);
+                }
+                else
+                {
+                    //Log.warningln("ModbusTCP: Read HReg worker called for %d registers starting at %d - Component ID %d not found", count, address, reg->componentId);
+                    response.setError(request.getServerID(), request.getFunctionCode(), Modbus::ILLEGAL_DATA_ADDRESS);
+                }
+            }else{
+                //Log.warningln("ModbusTCP: Read HReg worker called for %d registers starting at %d - No mapping found", count, address);
+                response.setError(request.getServerID(), request.getFunctionCode(), Modbus::ILLEGAL_DATA_ADDRESS);
             }
         }
         return response;
@@ -180,7 +192,7 @@ short ModbusTCP::setup()
         short result = target->mb_tcp_write(reg, value ? 1 : 0);
         if (result == E_OK)
         {
-            response = request; // Echo request on success
+            response = request;
         }
         else
         {
@@ -221,11 +233,10 @@ short ModbusTCP::setup()
             response.setError(request.getServerID(), request.getFunctionCode(), Modbus::SERVER_DEVICE_FAILURE); // Internal error
             return response;
         }
-
         short result = target->mb_tcp_write(reg, (short)value);
         if (result == E_OK)
         {
-            response = request; // Echo request on success
+            response = request;
         }
         else
         {
@@ -268,7 +279,7 @@ short ModbusTCP::setup()
                 {
                     Log.errorln("ModbusTCP: Write Multi Coil FC=15 Addr=%d Count=%d - Failed to read data byte at index %d.", address, count, dataIndex - 1);
                     response.setError(request.getServerID(), request.getFunctionCode(), Modbus::SERVER_DEVICE_FAILURE); // Or ILLEGAL_DATA_VALUE
-                    return response;                                                                                  // Abort on data read error
+                    return response;                                                                                    // Abort on data read error
                 }
             }
 
@@ -344,7 +355,7 @@ short ModbusTCP::setup()
         }
 
         bool anyWriteFailed = false; // Track actual component write failures
-        uint8_t dataIndex = 7; // Start of register data in request
+        uint8_t dataIndex = 7;       // Start of register data in request
 
         for (uint16_t i = 0; i < count; ++i)
         {
@@ -355,7 +366,7 @@ short ModbusTCP::setup()
             {
                 Log.errorln("ModbusTCP: Write Multi HReg FC=16 Addr=%d Count=%d - Failed to read data value at index %d.", address, count, dataIndex);
                 response.setError(request.getServerID(), request.getFunctionCode(), Modbus::SERVER_DEVICE_FAILURE); // Or ILLEGAL_DATA_VALUE
-                return response;                                                                                  // Abort on data read error
+                return response;                                                                                    // Abort on data read error
             }
             dataIndex += 2;