update eModbus to 1.7.2, #2254

lib/eModbus/library.json

@@ -1,6 +1,6 @@
 {
   "name": "eModbus",
-  "version": "1.7.0",
+  "version": "1.7.2",
   "keywords": "Arduino, ESP32, Modbus, RTU, ASCII, ModbusASCII, ModbusRTU, ModbusTCP",
   "description": "ModbusRTU, ModbusASCII and ModbusTCP functions for ESP32",
   "homepage": "https://emodbus.github.io",
@@ -23,6 +23,25 @@
     "url": "https://github.com/eModbus/eModbus",
     "branch": "master"
   },
+  "dependencies": [
+    {
+      "owner": "me-no-dev",
+      "name": "AsyncTCP",
+      "version": "*",
+      "platforms": ["espressif32"]
+    },
+    {
+      "owner": "me-no-dev",
+      "name": "ESPAsyncTCP",
+      "version": "*",
+      "platforms": ["espressif8266"]
+    },
+    {
+      "name": "Ethernet",
+      "version": "https://github.com/arduino-libraries/Ethernet.git",
+      "platforms": ["espressif32"]
+    }
+  ],
   "export": {
     "include":
     [

lib/eModbus/library.properties

@@ -1,5 +1,5 @@
 name=eModbus
-version=1.7.0
+version=1.7.2
 author=bertmelis,Miq1 <miq1@gmx.de>
 maintainer=Miq1 <miq1@gmx.de>
 sentence=eModbus provides Modbus RTU, ASCII and TCP functions for ESP32.

lib/eModbus/src/ModbusBridgeETH.h

@@ -0,0 +1,21 @@
+// =================================================================================================
+// eModbus: Copyright 2024 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_BRIDGE_ETHERNET_H
+#define _MODBUS_BRIDGE_ETHERNET_H
+#include "options.h"
+#if HAS_ETHERNET == 1
+#include <ETH.h>
+#include <SPI.h>
+
+#undef SERVER_END
+#define SERVER_END // NIL for Ethernet
+
+#include "ModbusServerTCPtemp.h"
+#include "ModbusBridgeTemp.h"
+
+using ModbusBridgeEthernet = ModbusBridge<ModbusServerTCP<WiFiServer, WiFiClient>>;
+#endif
+
+#endif

lib/eModbus/src/ModbusBridgeTemp.h

@@ -29,8 +29,8 @@ public:
   ModbusBridge();
 
   // Constructors for the RTU variant. Parameters as are for ModbusServerRTU
-  ModbusBridge(HardwareSerial& serial, uint32_t timeout, int rtsPin = -1);
+  ModbusBridge(uint32_t timeout, int rtsPin = -1);
-  ModbusBridge(HardwareSerial& serial, uint32_t timeout, RTScallback rts);
+  ModbusBridge(uint32_t timeout, RTScallback rts);
 
   // Destructor
   ~ModbusBridge();
@@ -43,6 +43,12 @@ public:
 
   // Block a function code (respond with ILLEGAL_FUNCTION error)
   bool denyFunctionCode(uint8_t aliasID, uint8_t functionCode);
+  
+  // Add/remove request/response filters
+  bool addRequestFilter(uint8_t aliasID, MBSworker rF);
+  bool removeRequestFilter(uint8_t aliasID);
+  bool addResponseFilter(uint8_t aliasID, MBSworker rF);
+  bool removeResponseFilter(uint8_t aliasID);
 
 protected:
   // ServerData holds all data necessary to address a single server
@@ -52,6 +58,8 @@ protected:
     ServerType serverType;        // TCP_SERVER or RTU_SERVER
     IPAddress host;               // TCP: host IP address, else 0.0.0.0
     uint16_t port;                // TCP: host port number, else 0
+    MBSworker requestFilter;      // optional filter requests before forwarding them
+    MBSworker responseFilter;     // optional filter responses before forwarding them
 
     // RTU constructor
     ServerData(uint8_t sid, ModbusClient *c) :
@@ -59,7 +67,9 @@ protected:
       client(c),
       serverType(RTU_SERVER),
       host(IPAddress(0, 0, 0, 0)),
-      port(0) {}
+      port(0),
+      requestFilter(nullptr),
+      responseFilter(nullptr) {}
     
     // TCP constructor
     ServerData(uint8_t sid, ModbusClient *c, IPAddress h, uint16_t p) :
@@ -67,7 +77,9 @@ protected:
       client(c),
       serverType(TCP_SERVER),
       host(h),
-      port(p) {}
+      port(p),
+      requestFilter(nullptr),
+      responseFilter(nullptr) {}
   };
 
   // Default worker functions
@@ -85,13 +97,13 @@ ModbusBridge<SERVERCLASS>::ModbusBridge() :
 
 // Constructors for RTU variant
 template<typename SERVERCLASS>
-ModbusBridge<SERVERCLASS>::ModbusBridge(HardwareSerial& serial, uint32_t timeout, int rtsPin) :
+ModbusBridge<SERVERCLASS>::ModbusBridge(uint32_t timeout, int rtsPin) :
-  SERVERCLASS(serial, timeout, rtsPin) { }
+  SERVERCLASS(timeout, rtsPin) { }
 
 // Alternate constructors for RTU variant
 template<typename SERVERCLASS>
-ModbusBridge<SERVERCLASS>::ModbusBridge(HardwareSerial& serial, uint32_t timeout, RTScallback rts) :
+ModbusBridge<SERVERCLASS>::ModbusBridge(uint32_t timeout, RTScallback rts) :
-  SERVERCLASS(serial, timeout, rts) { }
+  SERVERCLASS(timeout, rts) { }
 
 // Destructor
 template<typename SERVERCLASS>
@@ -156,6 +168,62 @@ bool ModbusBridge<SERVERCLASS>::denyFunctionCode(uint8_t aliasID, uint8_t functi
   return true;
 }
 
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::addRequestFilter(uint8_t aliasID, MBSworker rF) {
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) != servers.end()) {
+    // Yes. Chain in filter function
+    servers[aliasID]->requestFilter = rF;
+    LOG_D("Request filter added for server %02X\n", aliasID);
+  } else {
+    LOG_E("Server %d not attached to bridge, no request filter set!\n", aliasID);
+    return false;
+  }
+  return true;
+}
+
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::removeRequestFilter(uint8_t aliasID) {
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) != servers.end()) {
+    // Yes. Chain in filter function
+    servers[aliasID]->requestFilter = nullptr;
+    LOG_D("Request filter removed for server %02X\n", aliasID);
+  } else {
+    LOG_E("Server %d not attached to bridge, no request filter set!\n", aliasID);
+    return false;
+  }
+  return true;
+}
+
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::addResponseFilter(uint8_t aliasID, MBSworker rF) {
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) != servers.end()) {
+    // Yes. Chain in filter function
+    servers[aliasID]->responseFilter = rF;
+    LOG_D("Response filter added for server %02X\n", aliasID);
+  } else {
+    LOG_E("Server %d not attached to bridge, no response filter set!\n", aliasID);
+    return false;
+  }
+  return true;
+}
+
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::removeResponseFilter(uint8_t aliasID) {
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) != servers.end()) {
+    // Yes. Chain in filter function
+    servers[aliasID]->responseFilter = nullptr;
+    LOG_D("Response filter removed for server %02X\n", aliasID);
+  } else {
+    LOG_E("Server %d not attached to bridge, no response filter set!\n", aliasID);
+    return false;
+  }
+  return true;
+}
+
 // bridgeWorker: default worker function to process bridge requests
 template<typename SERVERCLASS>
 ModbusMessage ModbusBridge<SERVERCLASS>::bridgeWorker(ModbusMessage msg) {
@@ -167,11 +235,17 @@ ModbusMessage ModbusBridge<SERVERCLASS>::bridgeWorker(ModbusMessage msg) {
   if (servers.find(aliasID) != servers.end()) {
     // Found it. We may use servers[aliasID] now without allocating a new map slot
 
+    // Request filter hook to be called here
+    if (servers[aliasID]->requestFilter) {
+      LOG_D("Calling request filter\n");
+      msg = servers[aliasID]->requestFilter(msg);
+    }
+
     // Set real target server ID
     msg.setServerID(servers[aliasID]->serverID);
 
     // Issue the request
-    LOG_D("Request (%02X/%02X) sent\n", servers[aliasID]->serverID, functionCode);
+    LOG_D("Request (%02X/%02X) sent\n", servers[aliasID]->serverID, msg.getFunctionCode());
     // TCP servers have a target host/port that needs to be set in the client
     if (servers[aliasID]->serverType == TCP_SERVER) {
       response = reinterpret_cast<ModbusClientTCP *>(servers[aliasID]->client)->syncRequestMT(msg, (uint32_t)millis(), servers[aliasID]->host, servers[aliasID]->port);
@@ -179,8 +253,19 @@ ModbusMessage ModbusBridge<SERVERCLASS>::bridgeWorker(ModbusMessage msg) {
       response = servers[aliasID]->client->syncRequestM(msg, (uint32_t)millis());
     }
 
-    // Re-set the requested server ID
+    // Response filter hook to be called here
+    if (servers[aliasID]->responseFilter) {
+      LOG_D("Calling response filter\n");
+      response = servers[aliasID]->responseFilter(response);
+    }
+
+    // Re-set the requested server ID and function code (may have been modified by filters)
     response.setServerID(aliasID);
+    if (response.getError() != SUCCESS) {
+      response.setFunctionCode(functionCode | 0x80);
+    } else {
+      response.setFunctionCode(functionCode);
+    }
   } else {
     // If we get here, something has gone wrong internally. We send back an error response anyway.
     response.setError(aliasID, functionCode, INVALID_SERVER);

lib/eModbus/src/ModbusClientRTU.cpp

@@ -54,20 +54,20 @@ ModbusClientRTU::~ModbusClientRTU() {
 }
 
 // begin: start worker task - general version
-void ModbusClientRTU::begin(Stream& serial, uint32_t baudRate, int coreID) {
+void ModbusClientRTU::begin(Stream& serial, uint32_t baudRate, int coreID, uint32_t userInterval) {
   MR_serial = &serial;
-  doBegin(baudRate, coreID);
+  doBegin(baudRate, coreID, userInterval);
 }
 
 // begin: start worker task - HardwareSerial version
-void ModbusClientRTU::begin(HardwareSerial& serial, int coreID) {
+void ModbusClientRTU::begin(HardwareSerial& serial, int coreID, uint32_t userInterval) {
   MR_serial = &serial;
   uint32_t baudRate = serial.baudRate();
   serial.setRxFIFOFull(1);
-  doBegin(baudRate, coreID);
+  doBegin(baudRate, coreID, userInterval);
 }
 
-void ModbusClientRTU::doBegin(uint32_t baudRate, int coreID) {
+void ModbusClientRTU::doBegin(uint32_t baudRate, int coreID, uint32_t userInterval) {
   // Task already running? End it in case
   end();
 
@@ -77,6 +77,11 @@ void ModbusClientRTU::doBegin(uint32_t baudRate, int coreID) {
   // Set minimum interval time
   MR_interval = RTUutils::calculateInterval(baudRate);
 
+  // If user defined interval is longer, use that
+  if (MR_interval < userInterval) {
+    MR_interval = userInterval;
+  }
+
   // Create unique task name
   char taskName[18];
   snprintf(taskName, 18, "Modbus%02XRTU", instanceCounter);

lib/eModbus/src/ModbusClientRTU.h

@@ -31,9 +31,9 @@ public:
   ~ModbusClientRTU();
 
   // begin: start worker task
-  void begin(Stream& serial, uint32_t baudrate, int coreID = -1);
+  void begin(Stream& serial, uint32_t baudrate, int coreID = -1, uint32_t userInterval = 0);
   // Special variant for HardwareSerial
-  void begin(HardwareSerial& serial, int coreID = -1);
+  void begin(HardwareSerial& serial, int coreID = -1, uint32_t userInterval = 0);
 
   // end: stop the worker
   void end();
@@ -87,7 +87,7 @@ protected:
   ModbusMessage receive(const ModbusMessage request);
 
   // start background task
-  void doBegin(uint32_t baudRate, int coreID);
+  void doBegin(uint32_t baudRate, int coreID, uint32_t userInterval);
 
   void isInstance() { return; }   // make class instantiable
   queue<RequestEntry> requests;   // Queue to hold requests to be processed

lib/eModbus/src/ModbusClientTCPasync.cpp

@@ -7,213 +7,210 @@
 // #undef LOCAL_LOG_LEVEL
 #include "Logging.h"
 
-ModbusClientTCPasync::ModbusClientTCPasync(IPAddress address, uint16_t port, uint16_t queueLimit)
+ModbusClientTCPasync::ModbusClientTCPasync(IPAddress address, uint16_t port, uint16_t queueLimit) :
-    : ModbusClient()
+  ModbusClient(),
-    , txQueue()
+  txQueue(),
-    , rxQueue()
+  rxQueue(),
-    , MTA_client()
+  MTA_client(),
-    , MTA_timeout(DEFAULTTIMEOUT)
+  MTA_timeout(DEFAULTTIMEOUT),
-    , MTA_idleTimeout(DEFAULTIDLETIME)
+  MTA_idleTimeout(DEFAULTIDLETIME),
-    , MTA_qLimit(queueLimit)
+  MTA_qLimit(queueLimit),
-    , MTA_maxInflightRequests(queueLimit)
+  MTA_maxInflightRequests(queueLimit),
-    , MTA_lastActivity(0)
+  MTA_lastActivity(0),
-    , MTA_state(DISCONNECTED)
+  MTA_state(DISCONNECTED),
-    , MTA_host(address)
+  MTA_host(address),
-    , MTA_port(port) {
+  MTA_port(port)
-    // attach all handlers on async tcp events
+    {
-    MTA_client.onConnect([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onConnected(); }, this);
+      // attach all handlers on async tcp events
-    MTA_client.onDisconnect([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onDisconnected(); }, this);
+      MTA_client.onConnect([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onConnected(); }, this);
-    MTA_client.onError([](void * i, AsyncClient * c, int8_t error) { (static_cast<ModbusClientTCPasync *>(i))->onACError(c, error); }, this);
+      MTA_client.onDisconnect([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onDisconnected(); }, this);
-    // MTA_client.onTimeout([](void* i, AsyncClient* c, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onTimeout(time); }, this);
+      MTA_client.onError([](void* i, AsyncClient* c, int8_t error) { (static_cast<ModbusClientTCPasync*>(i))->onACError(c, error); }, this);
-    // MTA_client.onAck([](void* i, AsyncClient* c, size_t len, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onAck(len, time); }, this);
+      // MTA_client.onTimeout([](void* i, AsyncClient* c, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onTimeout(time); }, this);
-    MTA_client.onData([](void *        i,
+      // MTA_client.onAck([](void* i, AsyncClient* c, size_t len, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onAck(len, time); }, this);
-                         AsyncClient * c,
+      MTA_client.onData([](void* i, AsyncClient* c, void* data, size_t len) { (static_cast<ModbusClientTCPasync*>(i))->onPacket(static_cast<uint8_t*>(data), len); }, this);
-                         void *        data,
+      MTA_client.onPoll([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onPoll(); }, this);
-                         size_t        len) { (static_cast<ModbusClientTCPasync *>(i))->onPacket(static_cast<uint8_t *>(data), len); },
-                      this);
-    MTA_client.onPoll([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onPoll(); }, this);
 
-    // disable nagle algorithm ref Modbus spec
+      // disable nagle algorithm ref Modbus spec
-    MTA_client.setNoDelay(true);
+      MTA_client.setNoDelay(true);
-}
+    }
 
 // Destructor: clean up queue, task etc.
 ModbusClientTCPasync::~ModbusClientTCPasync() {
-    // Clean up queue
+  // Clean up queue
-    {
+  {
-        // Safely lock access
+    // Safely lock access
-        LOCK_GUARD(lock1, qLock);
-        LOCK_GUARD(lock2, sLock);
-        // Delete all elements from queues
-        while (!txQueue.empty()) {
-            delete txQueue.front();
-            txQueue.pop_front();
-        }
-        for (auto it = rxQueue.cbegin(); it != rxQueue.cend(); /* no increment */) {
-            delete it->second;
-            it = rxQueue.erase(it);
-        }
-    }
-    // force close client
-    MTA_client.close(true);
-}
-
-// optionally manually connect to modbus server. Otherwise connection will be made upon first request
-void ModbusClientTCPasync::connect() {
-    LOG_D("connecting\n");
-    LOCK_GUARD(lock1, sLock);
-    // only connect if disconnected
-    if (MTA_state == DISCONNECTED) {
-        MTA_state = CONNECTING;
-        MTA_client.connect(MTA_host, MTA_port);
-    }
-}
-
-// connect to another modbus server.
-void ModbusClientTCPasync::connect(IPAddress host, uint16_t port) {
-    // First disconnect, if connected
-    disconnect(true);
-    // Set new host and port
-    MTA_host = host;
-    MTA_port = port;
-    connect();
-}
-
-// manually disconnect from modbus server. Connection will also auto close after idle time
-void ModbusClientTCPasync::disconnect(bool force) {
-    LOG_D("disconnecting\n");
-    MTA_client.close(force);
-}
-
-// Set timeout value
-void ModbusClientTCPasync::setTimeout(uint32_t timeout) {
-    MTA_timeout = timeout;
-}
-
-// Set idle timeout value (time before connection auto closes after being idle)
-void ModbusClientTCPasync::setIdleTimeout(uint32_t timeout) {
-    MTA_idleTimeout = timeout;
-}
-
-void ModbusClientTCPasync::setMaxInflightRequests(uint32_t maxInflightRequests) {
-    MTA_maxInflightRequests = maxInflightRequests;
-}
-
-// Remove all pending request from queue
-void ModbusClientTCPasync::clearQueue() {
     LOCK_GUARD(lock1, qLock);
     LOCK_GUARD(lock2, sLock);
     // Delete all elements from queues
     while (!txQueue.empty()) {
-        delete txQueue.front();
+      delete txQueue.front();
-        txQueue.pop_front();
+      txQueue.pop_front();
     }
+    for (auto it = rxQueue.cbegin(); it != rxQueue.cend();/* no increment */) {
+      delete it->second;
+      it = rxQueue.erase(it);
+    }
+  }
+  // force close client
+  MTA_client.close(true);
+}
+
+// optionally manually connect to modbus server. Otherwise connection will be made upon first request
+void ModbusClientTCPasync::connect() {
+  LOG_D("connecting\n");
+  LOCK_GUARD(lock1, sLock);
+  // only connect if disconnected
+  if (MTA_state == DISCONNECTED) {
+    MTA_state = CONNECTING;
+    MTA_client.connect(MTA_host, MTA_port);
+  }
+}
+
+// connect to another modbus server.
+void ModbusClientTCPasync::connect(IPAddress host, uint16_t port) {
+  // First disconnect, if connected
+  disconnect(true);
+  // Set new host and port
+  MTA_host = host;
+  MTA_port = port;
+  connect();
+}
+
+// manually disconnect from modbus server. Connection will also auto close after idle time
+void ModbusClientTCPasync::disconnect(bool force) {
+  LOG_D("disconnecting\n");
+  MTA_client.close(force);
+}
+
+// Set timeout value
+void ModbusClientTCPasync::setTimeout(uint32_t timeout) {
+  MTA_timeout = timeout;
+}
+
+// Set idle timeout value (time before connection auto closes after being idle)
+void ModbusClientTCPasync::setIdleTimeout(uint32_t timeout) {
+  MTA_idleTimeout = timeout;
+}
+
+void ModbusClientTCPasync::setMaxInflightRequests(uint32_t maxInflightRequests) {
+  MTA_maxInflightRequests = maxInflightRequests;
+}
+
+// Remove all pending request from queue
+void ModbusClientTCPasync::clearQueue()
+{
+  LOCK_GUARD(lock1, qLock);
+  LOCK_GUARD(lock2, sLock);
+  // Delete all elements from queues
+  while (!txQueue.empty()) {
+    delete txQueue.front();
+    txQueue.pop_front();
+  }
 }
 
 // Base addRequest for preformatted ModbusMessage and last set target
 Error ModbusClientTCPasync::addRequestM(ModbusMessage msg, uint32_t token) {
-    Error rc = SUCCESS; // Return value
+  Error rc = SUCCESS;        // Return value
 
-    // Add it to the queue, if valid
+  // Add it to the queue, if valid
-    if (msg) {
+  if (msg) {
-        // Queue add successful?
+    // Queue add successful?
-        if (!addToQueue(token, msg)) {
+    if (!addToQueue(token, msg)) {
-            // No. Return error after deleting the allocated request.
+      // No. Return error after deleting the allocated request.
-            rc = REQUEST_QUEUE_FULL;
+      rc = REQUEST_QUEUE_FULL;
-        }
     }
+  }
 
-    LOG_D("Add TCP request result: %02X\n", rc);
+  LOG_D("Add TCP request result: %02X\n", rc);
-    return rc;
+  return rc;
 }
 
 // Base syncRequest follows the same pattern
 ModbusMessage ModbusClientTCPasync::syncRequestM(ModbusMessage msg, uint32_t token) {
-    ModbusMessage response;
+  ModbusMessage response;
 
-    if (msg) {
+  if (msg) {
-        // Queue add successful?
+    // Queue add successful?
-        if (!addToQueue(token, msg, true)) {
+    if (!addToQueue(token, msg, true)) {
-            // No. Return error after deleting the allocated request.
+      // No. Return error after deleting the allocated request.
-            response.setError(msg.getServerID(), msg.getFunctionCode(), REQUEST_QUEUE_FULL);
+      response.setError(msg.getServerID(), msg.getFunctionCode(), REQUEST_QUEUE_FULL);
-        } else {
-            // Request is queued - wait for the result.
-            response = waitSync(msg.getServerID(), msg.getFunctionCode(), token);
-        }
     } else {
-        response.setError(msg.getServerID(), msg.getFunctionCode(), EMPTY_MESSAGE);
+      // Request is queued - wait for the result.
+      response = waitSync(msg.getServerID(), msg.getFunctionCode(), token);
     }
-    return response;
+  } else {
+    response.setError(msg.getServerID(), msg.getFunctionCode(), EMPTY_MESSAGE);
+  }
+  return response;
 }
 
 // addToQueue: send freshly created request to queue
 bool ModbusClientTCPasync::addToQueue(int32_t token, ModbusMessage request, bool syncReq) {
-    // Did we get one?
+  // Did we get one?
-    if (request) {
+  if (request) {
-        LOCK_GUARD(lock1, qLock);
+    LOCK_GUARD(lock1, qLock);
-        if (txQueue.size() + rxQueue.size() < MTA_qLimit) {
+    if (txQueue.size() + rxQueue.size() < MTA_qLimit) {
-            HEXDUMP_V("Enqueue", request.data(), request.size());
+      HEXDUMP_V("Enqueue", request.data(), request.size());
-            RequestEntry * re = new RequestEntry(token, request, syncReq);
+      RequestEntry *re = new RequestEntry(token, request, syncReq);
-            if (!re)
+      if (!re) return false;  //TODO: proper error returning in case allocation fails
-                return false; // TODO proper error returning in case allocation fails
+      // inject proper transactionID
-            // inject proper transactionID
+      re->head.transactionID = messageCount++;
-            re->head.transactionID = messageCount++;
+      re->head.len = request.size();
-            re->head.len           = request.size();
+      // if we're already connected, try to send and push to rxQueue
-            // if we're already connected, try to send and push to rxQueue
+      // or else push to txQueue and (re)connect
-            // or else push to txQueue and (re)connect
+      if (MTA_state == CONNECTED && send(re)) {
-            if (MTA_state == CONNECTED && send(re)) {
+        re->sentTime = millis();
-                re->sentTime                    = millis();
+        rxQueue[re->head.transactionID] = re;
-                rxQueue[re->head.transactionID] = re;
+      } else {
-            } else {
+        txQueue.push_back(re);
-                txQueue.push_back(re);
+        if (MTA_state == DISCONNECTED) {
-                if (MTA_state == DISCONNECTED) {
+          connect();
-                    connect();
-                }
-            }
-            return true;
         }
-        LOG_E("queue is full\n");
+      }
+      return true;
     }
-    return false;
+    LOG_E("queue is full\n");
+  }
+  return false;
 }
 
 void ModbusClientTCPasync::onConnected() {
-    LOG_D("connected\n");
+  LOG_D("connected\n");
-    LOCK_GUARD(lock1, sLock);
+  LOCK_GUARD(lock1, sLock);
-    MTA_state        = CONNECTED;
+  MTA_state = CONNECTED;
-    MTA_lastActivity = millis();
+  MTA_lastActivity = millis();
-    // from now on onPoll will be called every 500 msec
+  // from now on onPoll will be called every 500 msec
 }
 
 void ModbusClientTCPasync::onDisconnected() {
-    LOG_D("disconnected\n");
+  LOG_D("disconnected\n");
-    LOCK_GUARD(lock1, sLock);
+  LOCK_GUARD(lock1, sLock);
-    MTA_state = DISCONNECTED;
+  MTA_state = DISCONNECTED;
 
-    // empty queue on disconnect, calling errorcode on every waiting request
+  // empty queue on disconnect, calling errorcode on every waiting request
-    LOCK_GUARD(lock2, qLock);
+  LOCK_GUARD(lock2, qLock);
-    while (!txQueue.empty()) {
+  while (!txQueue.empty()) {
-        RequestEntry * r = txQueue.front();
+    RequestEntry* r = txQueue.front();
-        if (onError) {
+    if (onError) {
-            onError(IP_CONNECTION_FAILED, r->token);
+      onError(IP_CONNECTION_FAILED, r->token);
-        }
-        delete r;
-        txQueue.pop_front();
     }
-    while (!rxQueue.empty()) {
+    delete r;
-        RequestEntry * r = rxQueue.begin()->second;
+    txQueue.pop_front();
-        if (onError) {
+  }
-            onError(IP_CONNECTION_FAILED, r->token);
+  while (!rxQueue.empty()) {
-        }
+    RequestEntry *r = rxQueue.begin()->second;
-        delete r;
+    if (onError) {
-        rxQueue.erase(rxQueue.begin());
+      onError(IP_CONNECTION_FAILED, r->token);
     }
+    delete r;
+    rxQueue.erase(rxQueue.begin());
+  }
 }
 
 
-void ModbusClientTCPasync::onACError(AsyncClient * c, int8_t error) {
+void ModbusClientTCPasync::onACError(AsyncClient* c, int8_t error) {
-    // onDisconnect will alse be called, so nothing to do here
+  // onDisconnect will alse be called, so nothing to do here
-    LOG_W("TCP error: %s\n", c->errorToString(error));
+  LOG_W("TCP error: %s\n", c->errorToString(error));
 }
 
 /*
@@ -225,178 +222,180 @@ void onAck(size_t len, uint32_t time) {
   // assuming we don't need this
 }
 */
-void ModbusClientTCPasync::onPacket(uint8_t * data, size_t length) {
+void ModbusClientTCPasync::onPacket(uint8_t* data, size_t length) {
-    LOG_D("packet received (len:%d)\n", length);
+  LOG_D("packet received (len:%d)\n", length);
-    // reset idle timeout
+  // reset idle timeout
-    MTA_lastActivity = millis();
+  MTA_lastActivity = millis();
 
-    if (length) {
+  if (length) {
-        LOG_D("parsing (len:%d)\n", length + 1);
+    LOG_D("parsing (len:%d)\n", length + 1);
+  }
+  while (length > 0) {
+    RequestEntry* request = nullptr;
+    ModbusMessage* response = nullptr;
+    uint16_t transactionID = 0;
+    uint16_t protocolID = 0;
+    uint16_t messageLength = 0;
+    bool isOkay = false;
+
+    // 1. Check for valid modbus message
+
+    // MBAP header is 6 bytes, we can't do anything with less
+    // total message should fit MBAP plus remaining bytes (in data[4], data[5])
+    if (length > 6) {
+      transactionID = (data[0] << 8) | data[1];
+      protocolID = (data[2] << 8) | data[3];
+      messageLength = (data[4] << 8) | data[5];
+      if (protocolID == 0 &&
+        length >= (uint32_t)messageLength + 6 &&
+        messageLength < 256) {
+        response = new ModbusMessage(messageLength);
+        response->add(&data[6], messageLength);
+        LOG_D("packet validated (len:%d)\n", messageLength);
+
+        // on next iteration: adjust remaining length and pointer to data
+        length -= 6 + messageLength;
+        data += 6 + messageLength;
+        isOkay = true;
+      }
     }
-    while (length > 0) {
-        RequestEntry *  request       = nullptr;
-        ModbusMessage * response      = nullptr;
-        uint16_t        transactionID = 0;
-        uint16_t        protocolID    = 0;
-        uint16_t        messageLength = 0;
-        bool            isOkay        = false;
 
-        // 1. Check for valid modbus message
+    if (!isOkay) {
+      // invalid packet, abort function
+      LOG_W("packet invalid\n");
+      return;
+    } else {
+      // 2. we got a valid response, match with a request
+      LOCK_GUARD(lock1, qLock);
+      auto i = rxQueue.find(transactionID);
+      if (i != rxQueue.end()) {
+        // found it, handle it and stop iterating
+        request = i->second;
+        i = rxQueue.erase(i);
+        LOG_D("matched request\n");
+      } else {
+        // TCP packet did not yield valid modbus response, abort function
+        LOG_W("no matching request found\n");
+        return;
+      }
+    }
 
-        // MBAP header is 6 bytes, we can't do anything with less
+    // 3. we have a valid request and a valid response, call appropriate callback
-        // total message should fit MBAP plus remaining bytes (in data[4], data[5])
+    if (request) {
-        if (length > 6) {
+      // compare request with response
-            transactionID = (data[0] << 8) | data[1];
+      Error error = SUCCESS;
-            protocolID    = (data[2] << 8) | data[3];
+      if (request->msg.getFunctionCode() != (response->getFunctionCode() & 0x7F)) {
-            messageLength = (data[4] << 8) | data[5];
+        error = FC_MISMATCH;
-            if (protocolID == 0 && length >= (uint32_t)messageLength + 6 && messageLength < 256) {
+      } else if (request->msg.getServerID() != response->getServerID()) {
-                response = new ModbusMessage(messageLength);
+        error = SERVER_ID_MISMATCH;
-                response->add(&data[6], messageLength);
+      } else {
-                LOG_D("packet validated (len:%d)\n", messageLength);
+        error = response->getError();
+      }
 
-                // on next iteration: adjust remaining length and pointer to data
+      if (error != SUCCESS) {
-                length -= 6 + messageLength;
+        LOCK_GUARD(errorCntLock, countAccessM);
-                data += 6 + messageLength;
+        errorCount++;
-                isOkay = true;
+      }
-            }
+
+      if (request->isSyncRequest) {
+        {
+          LOCK_GUARD(sL ,syncRespM);
+          syncResponse[request->token] = *response;
         }
-
+      } else if (onResponse) {
-        if (!isOkay) {
+        onResponse(*response, request->token);
-            // invalid packet, abort function
+      } else {
-            LOG_W("packet invalid\n");
+        if (error == SUCCESS) {
-            return;
+          if (onData) {
+            onData(*response, request->token);
+          }
         } else {
-            // 2. we got a valid response, match with a request
+          if (onError) {
-            LOCK_GUARD(lock1, qLock);
+            onError(response->getError(), request->token);
-            auto i = rxQueue.find(transactionID);
+          }
-            if (i != rxQueue.end()) {
-                // found it, handle it and stop iterating
-                request = i->second;
-                i       = rxQueue.erase(i);
-                LOG_D("matched request\n");
-            } else {
-                // TCP packet did not yield valid modbus response, abort function
-                LOG_W("no matching request found\n");
-                return;
-            }
         }
+      }
+      delete request;
+    }
+    delete response;
 
-        // 3. we have a valid request and a valid response, call appropriate callback
+  }  // end processing of incoming data
-        if (request) {
-            // compare request with response
-            Error error = SUCCESS;
-            if (request->msg.getFunctionCode() != (response->getFunctionCode() & 0x7F)) {
-                error = FC_MISMATCH;
-            } else if (request->msg.getServerID() != response->getServerID()) {
-                error = SERVER_ID_MISMATCH;
-            } else {
-                error = response->getError();
-            }
 
-            if (error != SUCCESS) {
+  // check if we have to send the next request
-                LOCK_GUARD(errorCntLock, countAccessM);
+  LOCK_GUARD(lock1, qLock);
-                errorCount++;
+  handleSendingQueue();
-            }
-
-            if (request->isSyncRequest) {
-                {
-                    LOCK_GUARD(sL, syncRespM);
-                    syncResponse[request->token] = *response;
-                }
-            } else if (onResponse) {
-                onResponse(*response, request->token);
-            } else {
-                if (error == SUCCESS) {
-                    if (onData) {
-                        onData(*response, request->token);
-                    }
-                } else {
-                    if (onError) {
-                        onError(response->getError(), request->token);
-                    }
-                }
-            }
-            delete request;
-        }
-        delete response;
-
-    } // end processing of incoming data
-
-    // check if we have to send the next request
-    LOCK_GUARD(lock1, qLock);
-    handleSendingQueue();
 }
 
 void ModbusClientTCPasync::onPoll() {
-    {
+  {
-        LOCK_GUARD(lock1, qLock);
+  LOCK_GUARD(lock1, qLock);
 
-        // try to send whatever is waiting
+  // try to send whatever is waiting
-        handleSendingQueue();
+  handleSendingQueue();
 
-        // next check if timeout has struck for oldest request
+  // next check if timeout has struck for oldest request
-        if (!rxQueue.empty()) {
+  if (!rxQueue.empty()) {
-            RequestEntry * request = rxQueue.begin()->second;
+    RequestEntry* request = rxQueue.begin()->second;
-            if (millis() - request->sentTime > MTA_timeout) {
+    if (millis() - request->sentTime > MTA_timeout) {
-                LOG_D("request timeouts (now:%lu-sent:%u)\n", millis(), request->sentTime);
+      LOG_D("request timeouts (now:%lu-sent:%u)\n", millis(), request->sentTime);
-                // oldest element timeouts, call onError and clean up
+      // oldest element timeouts, call onError and clean up
-                if (onError) {
+      if (onError) {
-                    // Handle timeout error
+        // Handle timeout error
-                    onError(TIMEOUT, request->token);
+        onError(TIMEOUT, request->token);
-                }
+      }
-                delete request;
+      delete request;
-                rxQueue.erase(rxQueue.begin());
+      rxQueue.erase(rxQueue.begin());
-            }
-        }
-
-    } // end lockguard scope
-
-    // if nothing happened during idle timeout, gracefully close connection
-    if (millis() - MTA_lastActivity > MTA_idleTimeout) {
-        disconnect();
     }
+  }
+    
+  }  // end lockguard scope
+
+  // if nothing happened during idle timeout, gracefully close connection
+  if (millis() - MTA_lastActivity > MTA_idleTimeout) {
+    disconnect();
+  }
 }
 
 void ModbusClientTCPasync::handleSendingQueue() {
-    // ATTENTION: This method does not have a lock guard.
+  // ATTENTION: This method does not have a lock guard.
-    // Calling sites must assure shared resources are protected
+  // Calling sites must assure shared resources are protected
-    // by mutex.
+  // by mutex.
 
-    // try to send everything we have waiting
+  // try to send everything we have waiting
-    std::list<RequestEntry *>::iterator it = txQueue.begin();
+  std::list<RequestEntry*>::iterator it = txQueue.begin();
-    while (it != txQueue.end()) {
+  while (it != txQueue.end()) {
-        // get the actual element
+    // get the actual element
-        if (send(*it)) {
+    if (send(*it)) {
-            // after sending, update timeout value, add to other queue and remove from this queue
+      // after sending, update timeout value, add to other queue and remove from this queue
-            (*it)->sentTime                    = millis();
+      (*it)->sentTime = millis();
-            rxQueue[(*it)->head.transactionID] = (*it);             // push request to other queue
+      rxQueue[(*it)->head.transactionID] = (*it);      // push request to other queue
-            it                                 = txQueue.erase(it); // remove from toSend queue and point i to next request
+      it = txQueue.erase(it);  // remove from toSend queue and point i to next request
-        } else {
+    } else {
-            // sending didn't succeed, try next request
+      // sending didn't succeed, try next request
-            ++it;
+      ++it;
-        }
     }
+  }
 }
 
-bool ModbusClientTCPasync::send(RequestEntry * re) {
+bool ModbusClientTCPasync::send(RequestEntry* re) {
-    // ATTENTION: This method does not have a lock guard.
+  // ATTENTION: This method does not have a lock guard.
-    // Calling sites must assure shared resources are protected
+  // Calling sites must assure shared resources are protected
-    // by mutex.
+  // by mutex.
 
-    if (rxQueue.size() >= MTA_maxInflightRequests) {
+  if (rxQueue.size() >= MTA_maxInflightRequests) {
-        return false;
-    }
-
-    // check if TCP client is able to send
-    if (MTA_client.space() > ((uint32_t)re->msg.size() + 6)) {
-        // Write TCP header first
-        MTA_client.add(reinterpret_cast<const char *>((const uint8_t *)(re->head)), 6, ASYNC_WRITE_FLAG_COPY);
-        // Request comes next
-        MTA_client.add(reinterpret_cast<const char *>(re->msg.data()), re->msg.size(), ASYNC_WRITE_FLAG_COPY);
-        // done
-        MTA_client.send();
-        LOG_D("request sent (msgid:%d)\n", re->head.transactionID);
-        return true;
-    }
     return false;
+  }
+
+  // check if TCP client is able to send
+  if (MTA_client.space() > ((uint32_t)re->msg.size() + 6)) {
+    // Write TCP header first
+    MTA_client.add(reinterpret_cast<const char *>((const uint8_t *)(re->head)), 6, ASYNC_WRITE_FLAG_COPY);
+    // Request comes next
+    MTA_client.add(reinterpret_cast<const char*>(re->msg.data()), re->msg.size(), ASYNC_WRITE_FLAG_COPY);
+    // done
+    MTA_client.send();
+    LOG_D("request sent (msgid:%d)\n", re->head.transactionID);
+    return true;
+  }
+  return false;
 }

lib/eModbus/src/ModbusServer.cpp

@@ -18,30 +18,48 @@ void ModbusServer::registerWorker(uint8_t serverID, uint8_t functionCode, MBSwor
 
 // getWorker: if a worker function is registered, return its address, nullptr otherwise
 MBSworker ModbusServer::getWorker(uint8_t serverID, uint8_t functionCode) {
+  bool serverFound = false;
+  LOG_D("Need worker for %02X-%02X : ", serverID, functionCode);
   // Search the FC map associated with the serverID
   auto svmap = workerMap.find(serverID);
   // Is there one?
   if (svmap != workerMap.end()) {
+    serverFound = true;
+  // No explicit serverID entry found, but we may have one for ANY_SERVER
+  } else {
+    svmap = workerMap.find(ANY_SERVER);
+    if (svmap != workerMap.end()) {
+      serverFound = true;
+      serverID = ANY_SERVER;
+    }
+  }
+  // Did we find a serverID?
+  if (serverFound) {
     // Yes. Now look for the function code in the inner map
+    bool functionCodeFound = false;
     auto fcmap = svmap->second.find(functionCode);;
     // Found it?
     if (fcmap != svmap->second.end()) {
       // Yes. Return the function pointer for it.
-      LOG_D("Worker found for %02X/%02X\n", serverID, functionCode);
+      functionCodeFound = true;
-      return fcmap->second;
       // No, no explicit worker found, but may be there is one for ANY_FUNCTION_CODE?
     } else {
       fcmap = svmap->second.find(ANY_FUNCTION_CODE);;
       // Found it?
       if (fcmap != svmap->second.end()) {
         // Yes. Return the function pointer for it.
-        LOG_D("Worker found for %02X/ANY\n", serverID);
+        functionCodeFound = true;
-        return fcmap->second;
+        functionCode = ANY_FUNCTION_CODE;
       }
     }
+    if (functionCodeFound) {
+      // Yes. Return the function pointer for it.
+      LOGRAW_D("Worker found for %02X/%02X\n", serverID, functionCode);
+      return fcmap->second;
+    }
   }
   // No matching function pointer found
-  LOG_D("No matching worker found\n");
+  LOGRAW_D("No matching worker found\n");
   return nullptr;
 }
 
@@ -68,16 +86,29 @@ bool ModbusServer::unregisterWorker(uint8_t serverID, uint8_t functionCode) {
   return (numEntries ? true : false);
 }
 
-// isServerFor: if any worker function is registered for the given serverID, return true
+// isServerFor: if a worker function is registered for the given serverID, return true
-bool ModbusServer::isServerFor(uint8_t serverID) {
+//              functionCode defaults to ANY_FUNCTION_CODE and will yield true for any function code,
-  // Search the FC map for the serverID
+//              including ANY_FUNCTION_CODE :D
-  auto svmap = workerMap.find(serverID);
+bool ModbusServer::isServerFor(uint8_t serverID, uint8_t functionCode) {
-  // Is it there? Then return true
+  // Check if there is a non-nullptr function for the given combination
-  if (svmap != workerMap.end()) return true;
+  if (getWorker(serverID, functionCode)) {
-  // No, serverID was not found. Return false
+    return true;
+  }
   return false;
 }
 
+// isServerFor: short version to look up if the server is known at all
+bool ModbusServer::isServerFor(uint8_t serverID) {
+  // Check if there is a non-nullptr function for the given combination
+  auto svmap = workerMap.find(serverID);
+  // Is there one?
+  if (svmap != workerMap.end()) {
+    return true;
+  }
+  return false;
+}
+
+
 // getMessageCount: read number of messages processed
 uint32_t ModbusServer::getMessageCount() { 
   return messageCount;

lib/eModbus/src/ModbusServer.h

@@ -42,7 +42,10 @@ public:
   // Returns true if the worker was found and removed
   bool unregisterWorker(uint8_t serverID, uint8_t functionCode = 0);
 
-  // isServerFor: if any worker function is registered for the given serverID, return true
+  // isServerFor: if a worker function is registered for the given serverID, return true
+  bool isServerFor(uint8_t serverID, uint8_t functionCode);
+
+  // isServerFor: short version to look up if the server is known at all
   bool isServerFor(uint8_t serverID);
 
   // getMessageCount: read number of messages processed

lib/eModbus/src/ModbusServerETH.h

@@ -0,0 +1,19 @@
+// =================================================================================================
+// eModbus: Copyright 2024 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_SERVER_ETH_H
+#define _MODBUS_SERVER_ETH_H
+#include "options.h"
+#if HAS_ETHERNET == 1
+#include <ETH.h>
+#include <SPI.h>
+
+#undef SERVER_END
+#define SERVER_END // NIL for Ethernet
+
+#include "ModbusServerTCPtemp.h"
+using ModbusServerEthernet = ModbusServerTCP<WiFiServer, WiFiClient>;
+#endif
+
+#endif

lib/eModbus/src/ModbusServerEthernet.h

@@ -12,8 +12,15 @@
 #undef SERVER_END
 #define SERVER_END // NIL for Ethernet
 
+// Create own non-virtual EthernetServer class
+class EthernetServerEM : public EthernetServer {
+public:
+  EthernetServerEM(uint16_t port) : EthernetServer(port) { }
+  void begin(uint16_t port = 0) { }
+};
+
 #include "ModbusServerTCPtemp.h"
-using ModbusServerEthernet = ModbusServerTCP<EthernetServer, EthernetClient>;
+using ModbusServerEthernet = ModbusServerTCP<EthernetServerEM, EthernetClient>;
 #endif
 
 #endif

lib/eModbus/src/ModbusServerRTU.cpp

@@ -64,26 +64,31 @@ ModbusServerRTU::~ModbusServerRTU() {
 }
 
 // start: create task with RTU server - general version
-void ModbusServerRTU::begin(Stream& serial, uint32_t baudRate, int coreID) {
+void ModbusServerRTU::begin(Stream& serial, uint32_t baudRate, int coreID, uint32_t userInterval) {
   MSRserial = &serial;
-  doBegin(baudRate, coreID);
+  doBegin(baudRate, coreID, userInterval);
 }
 
 // start: create task with RTU server - HardwareSerial versions
-void ModbusServerRTU::begin(HardwareSerial& serial, int coreID) {
+void ModbusServerRTU::begin(HardwareSerial& serial, int coreID, uint32_t userInterval) {
   MSRserial = &serial;
   uint32_t baudRate = serial.baudRate();
   serial.setRxFIFOFull(1);
-  doBegin(baudRate, coreID);
+  doBegin(baudRate, coreID, userInterval);
 }
 
-void ModbusServerRTU::doBegin(uint32_t baudRate, int coreID) {
+void ModbusServerRTU::doBegin(uint32_t baudRate, int coreID, uint32_t userInterval) {
   // Task already running? Stop it in case.
   end();
 
   // Set minimum interval time
   MSRinterval = RTUutils::calculateInterval(baudRate);
 
+  // If user defined interval is longer, use that
+  if (MSRinterval < userInterval) {
+    MSRinterval = userInterval;
+  }
+
   // Create unique task name
   char taskName[18];
   snprintf(taskName, 18, "MBsrv%02XRTU", instanceCounter);
@@ -180,10 +185,12 @@ void ModbusServerRTU::serve(ModbusServerRTU *myServer) {
       }
       // Is it a broadcast?
       if (request[0] == 0) {
+        LOG_D("Broadcast!\n");
         // Yes. Do we have a listener?
         if (myServer->listener) {
           // Yes. call it
           myServer->listener(request);
+          LOG_D("Broadcast served.\n");
         }
         // else we simply ignore it
       } else {

lib/eModbus/src/ModbusServerRTU.h

@@ -32,8 +32,8 @@ public:
   ~ModbusServerRTU();
 
   // begin: create task with RTU server to accept requests
-  void begin(Stream& serial, uint32_t baudRate, int coreID = -1);
+  void begin(Stream& serial, uint32_t baudRate, int coreID = -1, uint32_t userInterval = 0);
-  void begin(HardwareSerial& serial, int coreID = -1);
+  void begin(HardwareSerial& serial, int coreID = -1, uint32_t userInterval = 0);
 
   // end: kill server task
   void end();
@@ -64,7 +64,7 @@ protected:
   inline void isInstance() { }           // Make class instantiable
 
   // internal common begin function
-  void doBegin(uint32_t baudRate, int coreID);
+  void doBegin(uint32_t baudRate, int coreID, uint32_t userInterval);
 
   static uint8_t instanceCounter;        // Number of RTU servers created (for task names)
   TaskHandle_t serverTask;               // task of the started server

lib/eModbus/src/ModbusServerTCPasync.cpp

@@ -8,260 +8,261 @@
 // #undef LOCAL_LOG_LEVEL
 #include "Logging.h"
 
-ModbusServerTCPasync::mb_client::mb_client(ModbusServerTCPasync * s, AsyncClient * c)
+ModbusServerTCPasync::mb_client::mb_client(ModbusServerTCPasync* s, AsyncClient* c) :
-    : server(s)
+  server(s),
-    , client(c)
+  client(c),
-    , lastActiveTime(millis())
+  lastActiveTime(millis()),
-    , message(nullptr)
+  message(nullptr),
-    , error(SUCCESS)
+  error(SUCCESS),
-    , outbox() {
+  outbox() {
-    client->onData([](void * i, AsyncClient * c, void * data, size_t len) { (static_cast<mb_client *>(i))->onData(static_cast<uint8_t *>(data), len); }, this);
+    client->onData([](void* i, AsyncClient* c, void* data, size_t len) { (static_cast<mb_client*>(i))->onData(static_cast<uint8_t*>(data), len); }, this);
-    client->onPoll([](void * i, AsyncClient * c) { (static_cast<mb_client *>(i))->onPoll(); }, this);
+    client->onPoll([](void* i, AsyncClient* c) { (static_cast<mb_client*>(i))->onPoll(); }, this);
-    client->onDisconnect([](void * i, AsyncClient * c) { (static_cast<mb_client *>(i))->onDisconnect(); }, this);
+    client->onDisconnect([](void* i, AsyncClient* c) { (static_cast<mb_client*>(i))->onDisconnect(); }, this);
     client->setNoDelay(true);
 }
 
 ModbusServerTCPasync::mb_client::~mb_client() {
-    // clear outbox, if data is left
+  // clear outbox, if data is left
-    while (!outbox.empty()) {
+  while (!outbox.empty()) {
-        outbox.pop();
+    outbox.pop();
-    }
+  }
 
-    delete client; // will also close connection, if any
+  delete client;  // will also close connection, if any
 }
 
-void ModbusServerTCPasync::mb_client::onData(uint8_t * data, size_t len) {
+void ModbusServerTCPasync::mb_client::onData(uint8_t* data, size_t len) {
-    lastActiveTime = millis();
+  lastActiveTime = millis();
-    LOG_D("data len %d\n", len);
+  LOG_D("data len %d\n", len);
 
-    Error  error = SUCCESS;
+  Error error = SUCCESS;
-    size_t i     = 0;
+  size_t i = 0;
-    while (i < len) {
+  while (i < len) {
-        // 0. start
+    // 0. start
-        if (!message) {
+    if (!message) {
-            message = new ModbusMessage(8);
+      message = new ModbusMessage(8);
-            error   = SUCCESS;
+      error = SUCCESS;
-        }
+    }
 
-        //  1. get minimal 8 bytes to move on
+    //  1. get minimal 8 bytes to move on
-        while (message->size() < 8 && i < len) {
+    while (message->size() < 8 && i < len) {
-            message->push_back(data[i++]);
+      message->push_back(data[i++]);
-        }
+    }
+    
+    // 2. preliminary validation: protocol bytes and message length
+    if ((*message)[2] != 0 || (*message)[3] != 0) {
+      error = TCP_HEAD_MISMATCH;
+      LOG_D("invalid protocol\n");
+    }
+    size_t messageLength = (((*message)[4] << 8) | (*message)[5]) + 6;
+    if (messageLength > 262) {  // 256 + MBAP(6) = 262
+      error = PACKET_LENGTH_ERROR;
+      LOG_D("max length error\n");
+    }
+    if (error != SUCCESS) {
+      ModbusMessage response;
+      response.setError(message->getServerID(), message->getFunctionCode(), error);
+      message->resize(4);
+      message->add(static_cast<uint16_t>(3));
+      message->append(response);
+      addResponseToOutbox(message);  // outbox has pointer ownership now
+      // reset to starting values and process remaining data
+      message = nullptr;
+      return;  // protocol validation, abort further parsing
+    }
 
-        // 2. preliminary validation: protocol bytes and message length
+    // 3. receive until request is complete
-        if ((*message)[2] != 0 || (*message)[3] != 0) {
+    while (message->size() < messageLength && i < len) {
-            error = TCP_HEAD_MISMATCH;
+      message->push_back(data[i++]);
-            LOG_D("invalid protocol\n");
+    }
-        }
+    if (message->size() == messageLength) {
-        size_t messageLength = (((*message)[4] << 8) | (*message)[5]) + 6;
+      LOG_D("request complete (len:%d)\n", message->size());
-        if (messageLength > 262) { // 256 + MBAP(6) = 262
+    } else {
-            error = PACKET_LENGTH_ERROR;
+      LOG_D("request incomplete (len:%d), waiting for next TCP packet\n", message->size());
-            LOG_D("max length error\n");
+      continue;
-        }
+    }
-        if (error != SUCCESS) {
-            ModbusMessage response;
-            response.setError(message->getServerID(), message->getFunctionCode(), error);
-            message->resize(4);
-            message->add(static_cast<uint16_t>(3));
-            message->append(response);
-            addResponseToOutbox(message); // outbox has pointer ownership now
-            // reset to starting values and process remaining data
-            message = nullptr;
-            return; // protocol validation, abort further parsing
-        }
 
-        // 3. receive until request is complete
+    // 4. request complete, process
-        while (message->size() < messageLength && i < len) {
+    ModbusMessage request(messageLength - 6);  // create request without MBAP, with server ID
-            message->push_back(data[i++]);
+    request.add(message->data() + 6, message->size() - 6);
-        }
+    ModbusMessage userData;
-        if (message->size() == messageLength) {
+    if (server->isServerFor(request.getServerID())) {
-            LOG_D("request complete (len:%d)\n", message->size());
+      MBSworker callback = server->getWorker(request.getServerID(), request.getFunctionCode());
-        } else {
+      if (callback) {
-            LOG_D("request incomplete (len:%d), waiting for next TCP packet\n", message->size());
+        // request is well formed and is being served by user API
-            continue;
+        userData = callback(request);
-        }
+        // Process Response
-
+        // One of the predefined types?
-        // 4. request complete, process
+        if (userData[0] == 0xFF && (userData[1] == 0xF0 || userData[1] == 0xF1)) {
-        ModbusMessage request(messageLength - 6); // create request without MBAP, with server ID
+          // Yes. Check it
-        request.add(message->data() + 6, message->size() - 6);
+          switch (userData[1]) {
-        ModbusMessage userData;
+          case 0xF0: // NIL
-        if (server->isServerFor(request.getServerID())) {
+            userData.clear();
-            MBSworker callback = server->getWorker(request.getServerID(), request.getFunctionCode());
+            LOG_D("NIL response\n");
-            if (callback) {
+            break;
-                // request is well formed and is being served by user API
+          case 0xF1: // ECHO
-                userData = callback(request);
+            userData = request;
-                // Process Response
+            if (request.getFunctionCode() == WRITE_MULT_REGISTERS ||
-                // One of the predefined types?
+                request.getFunctionCode() == WRITE_MULT_COILS) {
-                if (userData[0] == 0xFF && (userData[1] == 0xF0 || userData[1] == 0xF1)) {
+              userData.resize(6);
-                    // Yes. Check it
-                    switch (userData[1]) {
-                    case 0xF0: // NIL
-                        userData.clear();
-                        LOG_D("NIL response\n");
-                        break;
-                    case 0xF1: // ECHO
-                        userData = request;
-                        if (request.getFunctionCode() == WRITE_MULT_REGISTERS || request.getFunctionCode() == WRITE_MULT_COILS) {
-                            userData.resize(6);
-                        }
-                        LOG_D("ECHO response\n");
-                        break;
-                    default: // Will not get here!
-                        break;
-                    }
-                } else {
-                    // No. User provided data response
-                    LOG_D("Data response\n");
-                }
-                error = SUCCESS;
-            } else { // no worker found
-                error = ILLEGAL_FUNCTION;
             }
-        } else { // mismatch server ID
+            LOG_D("ECHO response\n");
-            error = INVALID_SERVER;
+            break;
+          default:   // Will not get here!
+            break;
+          }
+        } else {
+          // No. User provided data response
+          LOG_D("Data response\n");
         }
-        if (error != SUCCESS) {
+        error = SUCCESS;
-            userData.setError(request.getServerID(), request.getFunctionCode(), error);
+      } else {  // no worker found
-        }
+        error = ILLEGAL_FUNCTION;
-        // Keep transaction id and protocol id
+      }
-        message->resize(4);
+    } else {  // mismatch server ID
-        // Add new payload length
+      error = INVALID_SERVER;
-        message->add(static_cast<uint16_t>(userData.size()));
+    }
-        // Append payload
+    if (error != SUCCESS) {
-        message->append(userData);
+      userData.setError(request.getServerID(), request.getFunctionCode(), error);
-        // Transfer message data to outbox
+    }
-        addResponseToOutbox(message);
+    // Keep transaction id and protocol id
-        message = nullptr;
+    message->resize(4);
-    } // end while loop iterating incoming data
+    // Add new payload length
+    message->add(static_cast<uint16_t>(userData.size()));
+    // Append payload
+    message->append(userData);
+    // Transfer message data to outbox
+    addResponseToOutbox(message);
+    message = nullptr;
+  }  // end while loop iterating incoming data
 }
 
 void ModbusServerTCPasync::mb_client::onPoll() {
-    LOCK_GUARD(lock1, obLock);
+  LOCK_GUARD(lock1, obLock);
-    handleOutbox();
+  handleOutbox();
-    if (server->idle_timeout > 0 && millis() - lastActiveTime > server->idle_timeout) {
+  if (server->idle_timeout > 0 && 
-        LOG_D("client idle, closing\n");
+      millis() - lastActiveTime > server->idle_timeout) {
-        client->close();
+    LOG_D("client idle, closing\n");
-    }
+    client->close();
+  }
 }
 
 void ModbusServerTCPasync::mb_client::onDisconnect() {
-    LOG_D("client disconnected\n");
+  LOG_D("client disconnected\n");
-    server->onClientDisconnect(this);
+  server->onClientDisconnect(this);
 }
 
-void ModbusServerTCPasync::mb_client::addResponseToOutbox(ModbusMessage * response) {
+void ModbusServerTCPasync::mb_client::addResponseToOutbox(ModbusMessage* response) {
-    if (response->size() > 0) {
+  if (response->size() > 0) {
-        LOCK_GUARD(lock1, obLock);
+    LOCK_GUARD(lock1, obLock);
-        outbox.push(response);
+    outbox.push(response);
-        handleOutbox();
+    handleOutbox();
-    }
+  }
 }
 
 void ModbusServerTCPasync::mb_client::handleOutbox() {
-    while (!outbox.empty()) {
+  while (!outbox.empty()) {
-        ModbusMessage * m = outbox.front();
+    ModbusMessage* m = outbox.front();
-        if (m->size() <= client->space()) {
+    if (m->size() <= client->space()) {
-            LOG_D("sending (%d)\n", m->size());
+      LOG_D("sending (%d)\n", m->size());
-            client->add(reinterpret_cast<const char *>(m->data()), m->size(), ASYNC_WRITE_FLAG_COPY);
+      client->add(reinterpret_cast<const char*>(m->data()), m->size(), ASYNC_WRITE_FLAG_COPY);
-            client->send();
+      client->send();
-            delete m;
+      delete m;
-            outbox.pop();
+      outbox.pop();
-        } else {
+    } else {
-            return;
+      return;
-        }
     }
+  }
 }
 
-ModbusServerTCPasync::ModbusServerTCPasync()
+ModbusServerTCPasync::ModbusServerTCPasync() :
-    : server(nullptr)
+  server(nullptr),
-    , clients()
+  clients(),
-    , maxNoClients(5)
+  maxNoClients(5),
-    , idle_timeout(60000) {
+  idle_timeout(60000) {
     // setup will be done in 'start'
 }
 
 
 ModbusServerTCPasync::~ModbusServerTCPasync() {
-    stop();
+  stop();
-    delete server;
+  delete server;
 }
 
 
 uint16_t ModbusServerTCPasync::activeClients() {
-    LOCK_GUARD(lock1, cListLock);
+  LOCK_GUARD(lock1, cListLock);
-    return clients.size();
+  return clients.size();
 }
 
 
-bool ModbusServerTCPasync::start(uint16_t port, uint8_t max_clients, uint32_t timeout, int coreID) {
+bool ModbusServerTCPasync::start(uint16_t port, uint8_t maxClients, uint32_t timeout, int coreID) {
-    // don't restart if already running
+  // don't restart if already running
-    if (server) {
+  if (server) {
-        LOG_W("Server already running.\n");
+    LOG_W("Server already running.\n");
-        return false;
-    }
-
-    maxNoClients = max_clients;
-    idle_timeout = timeout;
-    server       = new AsyncServer(port);
-    if (server) {
-        server->setNoDelay(true);
-        server->onClient([](void * i, AsyncClient * c) { (static_cast<ModbusServerTCPasync *>(i))->onClientConnect(c); }, this);
-        server->begin();
-        LOG_D("Modbus server started\n");
-        return true;
-    }
-    LOG_E("Could not start server\n");
     return false;
+  }
+  
+  maxNoClients = maxClients;
+  idle_timeout = timeout;
+  server = new AsyncServer(port);
+  if (server) {
+    server->setNoDelay(true);
+    server->onClient([](void* i, AsyncClient* c) { (static_cast<ModbusServerTCPasync*>(i))->onClientConnect(c); }, this);
+    server->begin();
+    LOG_D("Modbus server started\n");
+    return true;
+  }
+  LOG_E("Could not start server\n");
+  return false;
 }
 
 bool ModbusServerTCPasync::stop() {
-    if (!server) {
+  
-        LOG_W("Server not running.\n");
+  if (!server) {
-        return false;
+    LOG_W("Server not running.\n");
-    }
+    return false;
+  }
+  
+  // stop server to prevent new clients connecting
+  server->end();
 
-    // stop server to prevent new clients connecting
+  // now close existing clients
-    server->end();
+  LOCK_GUARD(lock1, cListLock);
-
+  while (!clients.empty()) {
-    // now close existing clients
+    // prevent onDisconnect handler to be called, resulting in deadlock
-    LOCK_GUARD(lock1, cListLock);
+    clients.front()->client->onDisconnect(nullptr, nullptr);
-    while (!clients.empty()) {
+    delete clients.front();
-        // prevent onDisconnect handler to be called, resulting in deadlock
+    clients.pop_front();
-        clients.front()->client->onDisconnect(nullptr, nullptr);
+  }
-        delete clients.front();
+  delete server;
-        clients.pop_front();
+  server = nullptr;
-    }
+  LOG_D("Modbus server stopped\n");
-    delete server;
+  return true;
-    server = nullptr;
-    LOG_D("Modbus server stopped\n");
-    return true;
 }
 
 bool ModbusServerTCPasync::isRunning() {
-    if (server)
+  if (server) return true;
-        return true;
+  else return false;
-    else
-        return false;
 }
 
-void ModbusServerTCPasync::onClientConnect(AsyncClient * client) {
+void ModbusServerTCPasync::onClientConnect(AsyncClient* client) {
-    LOG_D("new client\n");
+  LOG_D("new client\n");
-    LOCK_GUARD(lock1, cListLock);
+  LOCK_GUARD(lock1, cListLock);
-    if (clients.size() < maxNoClients) {
+  if (clients.size() < maxNoClients) {
-        clients.emplace_back(new mb_client(this, client));
+    clients.emplace_back(new mb_client(this, client));
-        LOG_D("nr clients: %d\n", clients.size());
-    } else {
-        LOG_D("max number of clients reached, closing new\n");
-        client->close(true);
-        delete client;
-    }
-}
-
-void ModbusServerTCPasync::onClientDisconnect(mb_client * client) {
-    LOCK_GUARD(lock1, cListLock);
-    // delete mb_client from list
-    clients.remove_if([client](mb_client * i) { return i->client == client->client; });
-    // delete client itself
-    delete client;
     LOG_D("nr clients: %d\n", clients.size());
+  } else {
+    LOG_D("max number of clients reached, closing new\n");
+    client->close(true);
+    delete client;
+  }
+}
+
+void ModbusServerTCPasync::onClientDisconnect(mb_client* client) {
+  LOCK_GUARD(lock1, cListLock);
+  // delete mb_client from list
+  clients.remove_if([client](mb_client* i) { return i->client == client->client; });
+  // delete client itself
+  delete client;
+  LOG_D("nr clients: %d\n", clients.size());
 }

lib/eModbus/src/ModbusServerTCPasync.h

@@ -14,7 +14,7 @@
 #endif
 #include <vector>
 
-#include <Arduino.h> // for millis()
+#include <Arduino.h>  // for millis()
 
 #if defined(ESP32)
 #include <AsyncTCP.h>
@@ -29,64 +29,64 @@ using std::lock_guard;
 #endif
 
 class ModbusServerTCPasync : public ModbusServer {
-  private:
-    class mb_client {
-        friend class ModbusServerTCPasync;
 
-      public:
+ private:
-        mb_client(ModbusServerTCPasync * s, AsyncClient * c);
+  class mb_client {
-        ~mb_client();
+   friend class ModbusServerTCPasync;
+   
+   public:
+    mb_client(ModbusServerTCPasync* s, AsyncClient* c);
+    ~mb_client();
 
-      private:
+   private:
-        void                        onData(uint8_t * data, size_t len);
+    void onData(uint8_t* data, size_t len);
-        void                        onPoll();
+    void onPoll();
-        void                        onDisconnect();
+    void onDisconnect();
-        void                        addResponseToOutbox(ModbusMessage * response);
+    void addResponseToOutbox(ModbusMessage* response);
-        void                        handleOutbox();
+    void handleOutbox();
-        ModbusServerTCPasync *      server;
+    ModbusServerTCPasync* server;
-        AsyncClient *               client;
+    AsyncClient* client;
-        uint32_t                    lastActiveTime;
+    uint32_t lastActiveTime;
-        ModbusMessage *             message;
+    ModbusMessage* message;
-        Modbus::Error               error;
+    Modbus::Error error;
-        std::queue<ModbusMessage *> outbox;
+    std::queue<ModbusMessage*> outbox;
-#if USE_MUTEX
+    #if USE_MUTEX
-        std::mutex obLock; // outbox protection
+    std::mutex obLock;  // outbox protection
-#endif
+    #endif
-    };
+  };
 
 
-  public:
+ public:
-    // Constructor
+  // Constructor
-    ModbusServerTCPasync();
+  ModbusServerTCPasync();
 
-    // Destructor: closes the connections
+  // Destructor: closes the connections
-    ~ModbusServerTCPasync();
+  ~ModbusServerTCPasync();
 
-    // activeClients: return number of clients currently employed
+  // activeClients: return number of clients currently employed
-    uint16_t activeClients();
+  uint16_t activeClients();
 
-    // start: create task with TCP server to accept requests
+  // start: create task with TCP server to accept requests
-    bool start(uint16_t port, uint8_t max_clients, uint32_t timeout, int coreID = -1);
+  bool start(uint16_t port, uint8_t maxClients, uint32_t timeout, int coreID = -1);
 
-    // stop: drop all connections and kill server task
+  // stop: drop all connections and kill server task
-    bool stop();
+  bool stop();
+ 
+  // isRunning: return true is server is running
+  bool isRunning();
 
-    // isRunning: return true is server is running
+ protected:
-    bool isRunning();
+  inline void isInstance() { }
+  void onClientConnect(AsyncClient* client);
+  void onClientDisconnect(mb_client* client);
 
-  protected:
+  AsyncServer* server;
-    inline void isInstance() {
+  std::list<mb_client*> clients;
-    }
+  uint8_t maxNoClients;
-    void onClientConnect(AsyncClient * client);
+  uint32_t idle_timeout;
-    void onClientDisconnect(mb_client * client);
+  #if USE_MUTEX
-
+  std::mutex cListLock;  // client list protection
-    AsyncServer *          server;
+  #endif
-    std::list<mb_client *> clients;
-    uint8_t                maxNoClients;
-    uint32_t               idle_timeout;
-#if USE_MUTEX
-    std::mutex cListLock; // client list protection
-#endif
 };
 
 #endif

lib/eModbus/src/ModbusServerTCPtemp.h

@@ -6,7 +6,7 @@
 #define _MODBUS_SERVER_TCP_TEMP_H
 
 #include <Arduino.h>
-#include <mutex> // NOLINT
+#include <mutex>  // NOLINT
 #include "ModbusServer.h"
 #undef LOCAL_LOG_LEVEL
 // #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
@@ -17,155 +17,142 @@ extern "C" {
 #include <freertos/task.h>
 }
 
-using std::lock_guard;
-using std::mutex;
 using std::vector;
+using std::mutex;
+using std::lock_guard;
 
 template <typename ST, typename CT>
 class ModbusServerTCP : public ModbusServer {
-  public:
+public:
-    // Constructor
+  // Constructor
-    ModbusServerTCP();
+  ModbusServerTCP();
 
-    // Destructor: closes the connections
+  // Destructor: closes the connections
-    ~ModbusServerTCP();
+  ~ModbusServerTCP();
 
-    // activeClients: return number of clients currently employed
+  // activeClients: return number of clients currently employed
-    uint16_t activeClients();
+  uint16_t activeClients();
 
-    // start: create task with TCP server to accept requests
+  // start: create task with TCP server to accept requests
-    bool start(uint16_t port, uint8_t max_clients, uint32_t timeout, int coreID = -1);
+  bool start(uint16_t port, uint8_t maxClients, uint32_t timeout, int coreID = -1);
 
-    // stop: drop all connections and kill server task
+  // stop: drop all connections and kill server task
-    bool stop();
+  bool stop();
 
-  protected:
+protected:
-    // Prevent copy construction and assignment
+  // Prevent copy construction and assignment
-    ModbusServerTCP(ModbusServerTCP & m)             = delete;
+  ModbusServerTCP(ModbusServerTCP& m) = delete;
-    ModbusServerTCP & operator=(ModbusServerTCP & m) = delete;
+  ModbusServerTCP& operator=(ModbusServerTCP& m) = delete;
 
-    inline void isInstance() {
+  inline void isInstance() { }
+
+  uint8_t numClients;
+  TaskHandle_t serverTask;
+  uint16_t serverPort;
+  uint32_t serverTimeout;
+  bool serverGoDown;
+  mutex clientLock;
+
+  struct ClientData {
+    ClientData() : task(nullptr), client(0), timeout(0), parent(nullptr) {}
+    ClientData(TaskHandle_t t, CT& c, uint32_t to, ModbusServerTCP<ST, CT> *p) : 
+      task(t), client(c), timeout(to), parent(p) {}
+    ~ClientData() {
+      if (client) {
+        client.stop();
+      }
+      if (task != nullptr) {
+        vTaskDelete(task);
+        LOG_D("Killed client task %d\n", (uint32_t)task);
+      }
     }
+    TaskHandle_t task;
+    CT client;
+    uint32_t timeout;
+    ModbusServerTCP<ST, CT> *parent;
+  };
+  ClientData **clients;
 
-    uint8_t      numClients;
+  // serve: loop function for server task
-    TaskHandle_t serverTask;
+  static void serve(ModbusServerTCP<ST, CT> *myself);
-    uint16_t     serverPort;
-    uint32_t     serverTimeout;
-    bool         serverGoDown;
-    mutex        clientLock;
 
-    struct ClientData {
+  // worker: loop function for client tasks
-        ClientData()
+  static void worker(ClientData *myData);
-            : task(nullptr)
-            , client(0)
-            , timeout(0)
-            , parent(nullptr) {
-        }
-        ClientData(TaskHandle_t t, CT & c, uint32_t to, ModbusServerTCP<ST, CT> * p)
-            : task(t)
-            , client(c)
-            , timeout(to)
-            , parent(p) {
-        }
-        ~ClientData() {
-            if (client) {
-                client.stop();
-            }
-            if (task != nullptr) {
-                vTaskDelete(task);
-                LOG_D("Killed client task %d\n", (uint32_t)task);
-            }
-        }
-        TaskHandle_t              task;
-        CT                        client;
-        uint32_t                  timeout;
-        ModbusServerTCP<ST, CT> * parent;
-    };
-    ClientData ** clients;
 
-    // serve: loop function for server task
+  // receive: read data from TCP
-    static void serve(ModbusServerTCP<ST, CT> * myself);
+  ModbusMessage receive(CT& client, uint32_t timeWait);
 
-    // worker: loop function for client tasks
+  // accept: start a task to receive requests and respond to a given client
-    static void worker(ClientData * myData);
+  bool accept(CT& client, uint32_t timeout, int coreID = -1);
 
-    // receive: read data from TCP
+  // clientAvailable: return true,. if a client slot is currently unused
-    ModbusMessage receive(CT & client, uint32_t timeWait);
+  bool clientAvailable() { return (numClients - activeClients()) > 0; }
-
-    // accept: start a task to receive requests and respond to a given client
-    bool accept(CT & client, uint32_t timeout, int coreID = -1);
-
-    // clientAvailable: return true,. if a client slot is currently unused
-    bool clientAvailable() {
-        return (numClients - activeClients()) > 0;
-    }
 };
 
 // Constructor
 template <typename ST, typename CT>
-ModbusServerTCP<ST, CT>::ModbusServerTCP()
+ModbusServerTCP<ST, CT>::ModbusServerTCP() :
-    : ModbusServer()
+  ModbusServer(),
-    , numClients(0)
+  numClients(0),
-    , serverTask(nullptr)
+  serverTask(nullptr),
-    , serverPort(502)
+  serverPort(502),
-    , serverTimeout(20000)
+  serverTimeout(20000),
-    , serverGoDown(false) {
+  serverGoDown(false) {
-    clients = new ClientData *[numClients]();
+    clients = new ClientData*[numClients]();
-}
+   }
 
 // Destructor: closes the connections
 template <typename ST, typename CT>
 ModbusServerTCP<ST, CT>::~ModbusServerTCP() {
-    for (uint8_t i = 0; i < numClients; ++i) {
+  for (uint8_t i = 0; i < numClients; ++i) {
-        if (clients[i] != nullptr) {
+    if (clients[i] != nullptr) {
-            delete clients[i];
+      delete clients[i];
-        }
     }
-    delete[] clients;
+  }
-    serverGoDown = true;
+  delete[] clients;
+  serverGoDown = true;
 }
 
 // activeClients: return number of clients currently employed
 template <typename ST, typename CT>
 uint16_t ModbusServerTCP<ST, CT>::activeClients() {
-    uint8_t cnt = 0;
+  uint8_t cnt = 0;
-    for (uint8_t i = 0; i < numClients; ++i) {
+  for (uint8_t i = 0; i < numClients; ++i) {
-        // Current slot could have been previously used - look for cleared task handles
+    // Current slot could have been previously used - look for cleared task handles
-        if (clients[i] != nullptr) {
+    if (clients[i] != nullptr) {
-            // Empty task handle?
+      // Empty task handle?
-            if (clients[i]->task == nullptr) {
+      if (clients[i]->task == nullptr) {
-                // Yes. Delete entry and init client pointer
+        // Yes. Delete entry and init client pointer
-                lock_guard<mutex> cL(clientLock);
+        lock_guard<mutex> cL(clientLock);
-                delete clients[i];
+        delete clients[i];
-                LOG_V("Delete client %d\n", i);
+        LOG_V("Delete client %d\n", i);
-                clients[i] = nullptr;
+        clients[i] = nullptr;
-            }
+      }
-        }
-        if (clients[i] != nullptr)
-            cnt++;
     }
-    return cnt;
+    if (clients[i] != nullptr) cnt++;
+  }
+  return cnt;
 }
 
-// start: create task with TCP server to accept requests
+  // start: create task with TCP server to accept requests
 template <typename ST, typename CT>
-bool ModbusServerTCP<ST, CT>::start(uint16_t port, uint8_t max_clients, uint32_t timeout, int coreID) {
+  bool ModbusServerTCP<ST, CT>::start(uint16_t port, uint8_t maxClients, uint32_t timeout, int coreID) {
     // Task already running?
     if (serverTask != nullptr) {
-        // Yes. stop it first
+      // Yes. stop it first
-        stop();
+      stop();
     }
     // Does the required number of slots fit?
-    if (numClients != max_clients) {
+    if (numClients != maxClients) {
-        // No. Drop array and allocate a new one
+      // No. Drop array and allocate a new one
-        delete[] clients;
+      delete[] clients;
-        // Now allocate a new one
+      // Now allocate a new one
-        numClients = max_clients;
+      numClients = maxClients;
-        clients    = new ClientData *[numClients]();
+      clients = new ClientData*[numClients]();
     }
-    serverPort    = port;
+    serverPort = port;
     serverTimeout = timeout;
-    serverGoDown  = false;
+    serverGoDown = false;
 
     // Create unique task name
     char taskName[18];
@@ -179,255 +166,255 @@ bool ModbusServerTCP<ST, CT>::start(uint16_t port, uint8_t max_clients, uint32_t
     delay(2000);
 
     return true;
-}
+  }
 
-// stop: drop all connections and kill server task
+  // stop: drop all connections and kill server task
 template <typename ST, typename CT>
-bool ModbusServerTCP<ST, CT>::stop() {
+  bool ModbusServerTCP<ST, CT>::stop() {
     // Check for clients still connected
     for (uint8_t i = 0; i < numClients; ++i) {
-        // Client is alive?
+      // Client is alive?
-        if (clients[i] != nullptr) {
+      if (clients[i] != nullptr) {
-            // Yes. Close the connection
+        // Yes. Close the connection
-            delete clients[i];
+        delete clients[i];
-            clients[i] = nullptr;
+        clients[i] = nullptr;
-        }
+      }
     }
     if (serverTask != nullptr) {
-        // Signal server task to stop
+      // Signal server task to stop
-        serverGoDown = true;
+      serverGoDown = true;
-        delay(5000);
+      delay(5000);
-        LOG_D("Killed server task %d\n", (uint32_t)(serverTask));
+      LOG_D("Killed server task %d\n", (uint32_t)(serverTask));
-        serverTask   = nullptr;
+      serverTask = nullptr;
-        serverGoDown = false;
+      serverGoDown = false;
     }
     return true;
-}
+  }
 
 // accept: start a task to receive requests and respond to a given client
 template <typename ST, typename CT>
-bool ModbusServerTCP<ST, CT>::accept(CT & client, uint32_t timeout, int coreID) {
+bool ModbusServerTCP<ST, CT>::accept(CT& client, uint32_t timeout, int coreID) {
-    // Look for an empty client slot
+  // Look for an empty client slot
-    for (uint8_t i = 0; i < numClients; ++i) {
+  for (uint8_t i = 0; i < numClients; ++i) {
-        // Empty slot?
+    // Empty slot?
-        if (clients[i] == nullptr) {
+    if (clients[i] == nullptr) {
-            // Yes. allocate new client data in slot
+      // Yes. allocate new client data in slot
-            clients[i] = new ClientData(0, client, timeout, this);
+      clients[i] = new ClientData(0, client, timeout, this);
 
-            // Create unique task name
+      // Create unique task name
-            char taskName[18];
+      char taskName[18];
-            snprintf(taskName, 18, "MBsrv%02Xclnt", i);
+      snprintf(taskName, 18, "MBsrv%02Xclnt", i);
 
-            // Start task to handle the client
+      // Start task to handle the client
-            xTaskCreatePinnedToCore((TaskFunction_t)&worker, taskName, SERVER_TASK_STACK, clients[i], 5, &clients[i]->task, coreID >= 0 ? coreID : NULL);
+      xTaskCreatePinnedToCore((TaskFunction_t)&worker, taskName, SERVER_TASK_STACK, clients[i], 5, &clients[i]->task, coreID >= 0 ? coreID : NULL);
-            LOG_D("Started client %d task %d\n", i, (uint32_t)(clients[i]->task));
+      LOG_D("Started client %d task %d\n", i, (uint32_t)(clients[i]->task));
 
-            return true;
+      return true;
-        }
     }
-    LOG_D("No client slot available.\n");
+  }
-    return false;
+  LOG_D("No client slot available.\n");
+  return false;
 }
 
 template <typename ST, typename CT>
-void ModbusServerTCP<ST, CT>::serve(ModbusServerTCP<ST, CT> * myself) {
+void ModbusServerTCP<ST, CT>::serve(ModbusServerTCP<ST, CT> *myself) {
-    // need a local scope here to delete the server at termination time
+  // need a local scope here to delete the server at termination time
-    if (1) {
+  if (1) {
-        // Set up server with given port
+    // Set up server with given port
-        ST server(myself->serverPort);
+    ST server(myself->serverPort);
 
-        // Start it
+    // Start it
-        server.begin();
+    server.begin();
 
-        // Loop until being killed
+    // Loop until being killed
-        while (!myself->serverGoDown) {
+    while (!myself->serverGoDown) {
-            // Do we have clients left to use?
+      // Do we have clients left to use?
-            if (myself->clientAvailable()) {
+      if (myself->clientAvailable()) {
-                // Yes. accept one, when it connects
+        // Yes. accept one, when it connects
-                CT ec = server.accept();
+        CT ec = server.accept();
-                // Did we get a connection?
+        // Did we get a connection?
-                if (ec) {
+        if (ec) {
-                    // Yes. Forward it to the Modbus server
+          // Yes. Forward it to the Modbus server
-                    myself->accept(ec, myself->serverTimeout, 0);
+          myself->accept(ec, myself->serverTimeout, 0);
-                    LOG_D("Accepted connection - %d clients running\n", myself->activeClients());
+          LOG_D("Accepted connection - %d clients running\n", myself->activeClients());
-                }
-            }
-            // Give scheduler room to breathe
-            delay(10);
         }
-        LOG_E("Server going down\n");
+      }
-        // We must go down
+      // Give scheduler room to breathe
-        SERVER_END;
+      delay(10);
     }
-    vTaskDelete(NULL);
+    LOG_E("Server going down\n");
+    // We must go down
+    SERVER_END;
+  }
+  vTaskDelete(NULL);
 }
 
 template <typename ST, typename CT>
-void ModbusServerTCP<ST, CT>::worker(ClientData * myData) {
+void ModbusServerTCP<ST, CT>::worker(ClientData *myData) {
-    // Get own reference data in handier form
+  // Get own reference data in handier form
-    CT       myClient  = myData->client;
+  CT myClient = myData->client;
-    uint32_t myTimeOut = myData->timeout;
+  uint32_t myTimeOut = myData->timeout;
-    // TaskHandle_t myTask = myData->task;
+  // TaskHandle_t myTask = myData->task;
-    ModbusServerTCP<ST, CT> * myParent      = myData->parent;
+  ModbusServerTCP<ST, CT> *myParent = myData->parent;
-    unsigned long             myLastMessage = millis();
+  unsigned long myLastMessage = millis();
 
-    LOG_D("Worker started, timeout=%d\n", myTimeOut);
+  LOG_D("Worker started, timeout=%d\n", myTimeOut);
 
-    // loop forever, if timeout is 0, or until timeout was hit
+  // loop forever, if timeout is 0, or until timeout was hit
-    while (myClient.connected() && (!myTimeOut || (millis() - myLastMessage < myTimeOut))) {
+  while (myClient.connected() && (!myTimeOut || (millis() - myLastMessage < myTimeOut))) {
-        ModbusMessage response; // Data buffer to hold prepared response
+    ModbusMessage response;               // Data buffer to hold prepared response
-        // Get a request
+    // Get a request
-        if (myClient.available()) {
+    if (myClient.available()) {
-            response.clear();
+      response.clear();
-            ModbusMessage m = myParent->receive(myClient, 100);
+      ModbusMessage m = myParent->receive(myClient, 100);
 
-            // has it the minimal length (6 bytes TCP header plus serverID plus FC)?
+      // has it the minimal length (6 bytes TCP header plus serverID plus FC)?
-            if (m.size() >= 8) {
+      if (m.size() >= 8) {
-                {
+        {
-                    LOCK_GUARD(cntLock, myParent->m);
+          LOCK_GUARD(cntLock, myParent->m);
-                    myParent->messageCount++;
+          myParent->messageCount++;
-                }
-                // Extract request data
-                ModbusMessage request;
-                request.add(m.data() + 6, m.size() - 6);
-
-                // Protocol ID shall be 0x0000 - is it?
-                if (m[2] == 0 && m[3] == 0) {
-                    // ServerID shall be at [6], FC at [7]. Check both
-                    if (myParent->isServerFor(request.getServerID())) {
-                        // Server is correct - in principle. Do we serve the FC?
-                        MBSworker callBack = myParent->getWorker(request.getServerID(), request.getFunctionCode());
-                        if (callBack) {
-                            // Yes, we do.
-                            // Invoke the worker method to get a response
-                            ModbusMessage data = callBack(request);
-                            // Process Response
-                            // One of the predefined types?
-                            if (data[0] == 0xFF && (data[1] == 0xF0 || data[1] == 0xF1)) {
-                                // Yes. Check it
-                                switch (data[1]) {
-                                case 0xF0: // NIL
-                                    response.clear();
-                                    LOG_D("NIL response\n");
-                                    break;
-                                case 0xF1: // ECHO
-                                    response = request;
-                                    if (request.getFunctionCode() == WRITE_MULT_REGISTERS || request.getFunctionCode() == WRITE_MULT_COILS) {
-                                        response.resize(6);
-                                    }
-                                    LOG_D("ECHO response\n");
-                                    break;
-                                default: // Will not get here!
-                                    break;
-                                }
-                            } else {
-                                // No. User provided data response
-                                response = data;
-                                LOG_D("Data response\n");
-                            }
-                        } else {
-                            // No, function code is not served here
-                            response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_FUNCTION);
-                        }
-                    } else {
-                        // No, serverID is not served here
-                        response.setError(request.getServerID(), request.getFunctionCode(), INVALID_SERVER);
-                    }
-                } else {
-                    // No, protocol ID was something weird
-                    response.setError(request.getServerID(), request.getFunctionCode(), TCP_HEAD_MISMATCH);
-                }
-            }
-            delay(1);
-            // Do we have a response to send?
-            if (response.size() >= 3) {
-                // Yes. Do it now.
-                // Cut off length and request data, then update TCP header
-                m.resize(4);
-                m.add(static_cast<uint16_t>(response.size()));
-                // Append response
-                m.append(response);
-                myClient.write(m.data(), m.size());
-                HEXDUMP_V("Response", m.data(), m.size());
-                // count error responses
-                if (response.getError() != SUCCESS) {
-                    LOCK_GUARD(cntLock, myParent->m);
-                    myParent->errorCount++;
-                }
-            }
-            // We did something communicationally - rewind timeout timer
-            myLastMessage = millis();
         }
-        delay(1);
+        // Extract request data
-    }
+        ModbusMessage request;
+        request.add(m.data() + 6, m.size() - 6);
 
-    if (millis() - myLastMessage >= myTimeOut) {
+        // Protocol ID shall be 0x0000 - is it?
-        // Timeout!
+        if (m[2] == 0 && m[3] == 0) {
-        LOG_D("Worker stopping due to timeout.\n");
+          // ServerID shall be at [6], FC at [7]. Check both
-    } else {
+          if (myParent->isServerFor(request.getServerID())) {
-        // Disconnected!
+            // Server is correct - in principle. Do we serve the FC?
-        LOG_D("Worker stopping due to client disconnect.\n");
+            MBSworker callBack = myParent->getWorker(request.getServerID(), request.getFunctionCode());
+            if (callBack) {
+              // Yes, we do.
+              // Invoke the worker method to get a response
+              ModbusMessage data = callBack(request);
+              // Process Response
+              // One of the predefined types?
+              if (data[0] == 0xFF && (data[1] == 0xF0 || data[1] == 0xF1)) {
+                // Yes. Check it
+                switch (data[1]) {
+                case 0xF0: // NIL
+                  response.clear();
+                  LOG_D("NIL response\n");
+                  break;
+                case 0xF1: // ECHO
+                  response = request;
+                  if (request.getFunctionCode() == WRITE_MULT_REGISTERS ||
+                      request.getFunctionCode() == WRITE_MULT_COILS) {
+                    response.resize(6);
+                  }
+                  LOG_D("ECHO response\n");
+                  break;
+                default:   // Will not get here!
+                  break;
+                }
+              } else {
+                // No. User provided data response
+                response = data;
+                LOG_D("Data response\n");
+              }
+            } else {
+              // No, function code is not served here
+              response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_FUNCTION);
+            }
+          } else {
+            // No, serverID is not served here
+            response.setError(request.getServerID(), request.getFunctionCode(), INVALID_SERVER);
+          }
+        } else {
+          // No, protocol ID was something weird
+          response.setError(request.getServerID(), request.getFunctionCode(), TCP_HEAD_MISMATCH);
+        }
+      }
+      delay(1);
+      // Do we have a response to send?
+      if (response.size() >= 3) {
+        // Yes. Do it now.
+        // Cut off length and request data, then update TCP header
+        m.resize(4);
+        m.add(static_cast<uint16_t>(response.size()));
+        // Append response
+        m.append(response);
+        myClient.write(m.data(), m.size());
+        HEXDUMP_V("Response", m.data(), m.size());
+        // count error responses
+        if (response.getError() != SUCCESS) {
+          LOCK_GUARD(cntLock, myParent->m);
+          myParent->errorCount++;
+        }
+      }
+      // We did something communicationally - rewind timeout timer
+      myLastMessage = millis();
     }
+    delay(1);
+  }
 
-    // Read away all that may still hang in the buffer
+  if (millis() - myLastMessage >= myTimeOut) {
-    while (myClient.read() != -1) {
+    // Timeout!
-    }
+    LOG_D("Worker stopping due to timeout.\n");
-    // Now stop the client
+  } else {
-    myClient.stop();
+    // Disconnected!
+    LOG_D("Worker stopping due to client disconnect.\n");
+  }
 
-    {
+  // Read away all that may still hang in the buffer
-        lock_guard<mutex> cL(myParent->clientLock);
+  while (myClient.read() != -1) {}
-        myData->task = nullptr;
+  // Now stop the client
-    }
+  myClient.stop();
 
-    delay(50);
+  {
-    vTaskDelete(NULL);
+    lock_guard<mutex> cL(myParent->clientLock);
+    myData->task = nullptr;
+  }
+
+  delay(50);
+  vTaskDelete(NULL);
 }
 
 // receive: get request via Client connection
 template <typename ST, typename CT>
-ModbusMessage ModbusServerTCP<ST, CT>::receive(CT & client, uint32_t timeWait) {
+ModbusMessage ModbusServerTCP<ST, CT>::receive(CT& client, uint32_t timeWait) {
-    unsigned long  lastMillis = millis(); // Timer to check for timeout
+  unsigned long lastMillis = millis();     // Timer to check for timeout
-    ModbusMessage  m;                     // to take read data
+  ModbusMessage m;                    // to take read data
-    uint16_t       lengthVal = 0;
+  uint16_t lengthVal = 0;
-    uint16_t       cnt       = 0;
+  uint16_t cnt = 0;
-    const uint16_t BUFFERSIZE(300);
+  const uint16_t BUFFERSIZE(300);
-    uint8_t        buffer[BUFFERSIZE];
+  uint8_t buffer[BUFFERSIZE];
 
-    // wait for sufficient packet data or timeout
+  // wait for sufficient packet data or timeout
-    while ((millis() - lastMillis < timeWait) && ((cnt < 6) || (cnt < lengthVal)) && (cnt < BUFFERSIZE)) {
+  while ((millis() - lastMillis < timeWait) && ((cnt < 6) || (cnt < lengthVal)) && (cnt < BUFFERSIZE)) 
-        // Is there data waiting?
+  {
-        if (client.available()) {
+    // Is there data waiting?
-            buffer[cnt] = client.read();
+    if (client.available()) {
-            // Are we at the TCP header length field byte #1?
+        buffer[cnt] = client.read();
-            if (cnt == 4)
+        // Are we at the TCP header length field byte #1?
-                lengthVal = buffer[cnt] << 8;
+        if (cnt == 4) lengthVal = buffer[cnt] << 8;
-            // Are we at the TCP header length field byte #2?
+        // Are we at the TCP header length field byte #2?
-            if (cnt == 5) {
+        if (cnt == 5) {
-                lengthVal |= buffer[cnt];
+          lengthVal |= buffer[cnt];
-                lengthVal += 6;
+          lengthVal += 6;
-            }
-            cnt++;
-            // Rewind EOT and timeout timers
-            lastMillis = millis();
-        } else {
-            delay(1); // Give scheduler room to breathe
         }
+        cnt++;
+        // Rewind EOT and timeout timers
+        lastMillis = millis();
+    } else {
+      delay(1); // Give scheduler room to breathe
     }
-    // Did we receive some data?
+  }
-    if (cnt) {
+  // Did we receive some data?
-        // Yes. Is it too much?
+  if (cnt) {
-        if (cnt >= BUFFERSIZE) {
+    // Yes. Is it too much?
-            // Yes, likely a buffer overflow of some sort
+    if (cnt >= BUFFERSIZE) {
-            // Adjust message size in TCP header
+      // Yes, likely a buffer overflow of some sort
-            buffer[4] = (cnt >> 8) & 0xFF;
+      // Adjust message size in TCP header
-            buffer[5] = cnt & 0xFF;
+      buffer[4] = (cnt >> 8) & 0xFF;
-            LOG_E("Potential buffer overrun (>%d)!\n", cnt);
+      buffer[5] = cnt & 0xFF;
-        }
+      LOG_E("Potential buffer overrun (>%d)!\n", cnt);
-        // Get as much buffer as was read
-        m.add(buffer, cnt);
     }
-    return m;
+    // Get as much buffer as was read
+    m.add(buffer, cnt);
+  }
+  return m;
 }
 
 #endif

lib/eModbus/src/ModbusTypeDefs.cpp

@@ -50,7 +50,7 @@ FCType FCT::getType(uint8_t functionCode) {
   return table[functionCode & 0x7F];
 }
 
-// setType: change the type of a function code.
+// redefineType: change the type of a function code.
 // This is possible only for the codes undefined yet and will return
 // the effective type
 FCType FCT::redefineType(uint8_t functionCode, const FCType type) {

lib/eModbus/src/ModbusTypeDefs.h

@@ -84,6 +84,9 @@ enum Error : uint8_t {
   UNDEFINED_ERROR        = 0xFF  // otherwise uncovered communication error
 };
 
+// Readable expression for the "illegal" server ID of 0
+#define ANY_SERVER 0x00
+
 #ifndef MINIMAL
 
 // Constants for float and double re-ordering