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();
@@ -44,6 +44,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
   struct ServerData {
@@ -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,35 +7,32 @@
 // #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);
+      MTA_client.onConnect([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onConnected(); }, this);
-    MTA_client.onDisconnect([](void * i, AsyncClient * c) { (static_cast<ModbusClientTCPasync *>(i))->onDisconnected(); }, this);
+      MTA_client.onDisconnect([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onDisconnected(); }, this);
-    MTA_client.onError([](void * i, AsyncClient * c, int8_t error) { (static_cast<ModbusClientTCPasync *>(i))->onACError(c, error); }, this);
+      MTA_client.onError([](void* i, AsyncClient* c, int8_t error) { (static_cast<ModbusClientTCPasync*>(i))->onACError(c, error); }, this);
       // MTA_client.onTimeout([](void* i, AsyncClient* c, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onTimeout(time); }, this);
       // MTA_client.onAck([](void* i, AsyncClient* c, size_t len, uint32_t time) { (static_cast<ModbusClientTCPasync*>(i))->onAck(len, time); }, this);
-    MTA_client.onData([](void *        i,
+      MTA_client.onData([](void* i, AsyncClient* c, void* data, size_t len) { (static_cast<ModbusClientTCPasync*>(i))->onPacket(static_cast<uint8_t*>(data), len); }, this);
-                         AsyncClient * c,
+      MTA_client.onPoll([](void* i, AsyncClient* c) { (static_cast<ModbusClientTCPasync*>(i))->onPoll(); }, this);
-                         void *        data,
-                         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
       MTA_client.setNoDelay(true);
-}
+    }
 
 // Destructor: clean up queue, task etc.
 ModbusClientTCPasync::~ModbusClientTCPasync() {
@@ -49,7 +46,7 @@ ModbusClientTCPasync::~ModbusClientTCPasync() {
       delete txQueue.front();
       txQueue.pop_front();
     }
-        for (auto it = rxQueue.cbegin(); it != rxQueue.cend(); /* no increment */) {
+    for (auto it = rxQueue.cbegin(); it != rxQueue.cend();/* no increment */) {
       delete it->second;
       it = rxQueue.erase(it);
     }
@@ -100,7 +97,8 @@ void ModbusClientTCPasync::setMaxInflightRequests(uint32_t maxInflightRequests)
 }
 
 // Remove all pending request from queue
-void ModbusClientTCPasync::clearQueue() {
+void ModbusClientTCPasync::clearQueue()
+{
   LOCK_GUARD(lock1, qLock);
   LOCK_GUARD(lock2, sLock);
   // Delete all elements from queues
@@ -153,9 +151,8 @@ bool ModbusClientTCPasync::addToQueue(int32_t token, ModbusMessage request, bool
     LOCK_GUARD(lock1, qLock);
     if (txQueue.size() + rxQueue.size() < MTA_qLimit) {
       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
       re->head.transactionID = messageCount++;
       re->head.len = request.size();
@@ -193,7 +190,7 @@ void ModbusClientTCPasync::onDisconnected() {
   // empty queue on disconnect, calling errorcode on every waiting request
   LOCK_GUARD(lock2, qLock);
   while (!txQueue.empty()) {
-        RequestEntry * r = txQueue.front();
+    RequestEntry* r = txQueue.front();
     if (onError) {
       onError(IP_CONNECTION_FAILED, r->token);
     }
@@ -201,7 +198,7 @@ void ModbusClientTCPasync::onDisconnected() {
     txQueue.pop_front();
   }
   while (!rxQueue.empty()) {
-        RequestEntry * r = rxQueue.begin()->second;
+    RequestEntry *r = rxQueue.begin()->second;
     if (onError) {
       onError(IP_CONNECTION_FAILED, r->token);
     }
@@ -211,7 +208,7 @@ void ModbusClientTCPasync::onDisconnected() {
 }
 
 
-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
   LOG_W("TCP error: %s\n", c->errorToString(error));
 }
@@ -225,7 +222,7 @@ 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);
   // reset idle timeout
   MTA_lastActivity = millis();
@@ -234,8 +231,8 @@ void ModbusClientTCPasync::onPacket(uint8_t * data, size_t length) {
     LOG_D("parsing (len:%d)\n", length + 1);
   }
   while (length > 0) {
-        RequestEntry *  request       = nullptr;
+    RequestEntry* request = nullptr;
-        ModbusMessage * response      = nullptr;
+    ModbusMessage* response = nullptr;
     uint16_t transactionID = 0;
     uint16_t protocolID = 0;
     uint16_t messageLength = 0;
@@ -249,7 +246,9 @@ void ModbusClientTCPasync::onPacket(uint8_t * data, size_t length) {
       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) {
+      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);
@@ -300,7 +299,7 @@ void ModbusClientTCPasync::onPacket(uint8_t * data, size_t length) {
 
       if (request->isSyncRequest) {
         {
-                    LOCK_GUARD(sL, syncRespM);
+          LOCK_GUARD(sL ,syncRespM);
           syncResponse[request->token] = *response;
         }
       } else if (onResponse) {
@@ -336,7 +335,7 @@ void ModbusClientTCPasync::onPoll() {
 
   // next check if timeout has struck for oldest request
   if (!rxQueue.empty()) {
-            RequestEntry * request = rxQueue.begin()->second;
+    RequestEntry* request = rxQueue.begin()->second;
     if (millis() - request->sentTime > MTA_timeout) {
       LOG_D("request timeouts (now:%lu-sent:%u)\n", millis(), request->sentTime);
       // oldest element timeouts, call onError and clean up
@@ -363,7 +362,7 @@ void ModbusClientTCPasync::handleSendingQueue() {
   // by mutex.
 
   // 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()) {
     // get the actual element
     if (send(*it)) {
@@ -378,7 +377,7 @@ void ModbusClientTCPasync::handleSendingQueue() {
   }
 }
 
-bool ModbusClientTCPasync::send(RequestEntry * re) {
+bool ModbusClientTCPasync::send(RequestEntry* re) {
   // ATTENTION: This method does not have a lock guard.
   // Calling sites must assure shared resources are protected
   // by mutex.
@@ -392,7 +391,7 @@ bool ModbusClientTCPasync::send(RequestEntry * re) {
     // 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);
+    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);

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;
+        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,16 +8,16 @@
 // #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);
 }
 
@@ -30,7 +30,7 @@ ModbusServerTCPasync::mb_client::~mb_client() {
   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();
   LOG_D("data len %d\n", len);
 
@@ -101,7 +101,8 @@ void ModbusServerTCPasync::mb_client::onData(uint8_t * data, size_t len) {
             break;
           case 0xF1: // ECHO
             userData = request;
-                        if (request.getFunctionCode() == WRITE_MULT_REGISTERS || request.getFunctionCode() == WRITE_MULT_COILS) {
+            if (request.getFunctionCode() == WRITE_MULT_REGISTERS ||
+                request.getFunctionCode() == WRITE_MULT_COILS) {
               userData.resize(6);
             }
             LOG_D("ECHO response\n");
@@ -138,7 +139,8 @@ void ModbusServerTCPasync::mb_client::onData(uint8_t * data, size_t len) {
 void ModbusServerTCPasync::mb_client::onPoll() {
   LOCK_GUARD(lock1, obLock);
   handleOutbox();
-    if (server->idle_timeout > 0 && millis() - lastActiveTime > server->idle_timeout) {
+  if (server->idle_timeout > 0 && 
+      millis() - lastActiveTime > server->idle_timeout) {
     LOG_D("client idle, closing\n");
     client->close();
   }
@@ -149,7 +151,7 @@ void ModbusServerTCPasync::mb_client::onDisconnect() {
   server->onClientDisconnect(this);
 }
 
-void ModbusServerTCPasync::mb_client::addResponseToOutbox(ModbusMessage * response) {
+void ModbusServerTCPasync::mb_client::addResponseToOutbox(ModbusMessage* response) {
   if (response->size() > 0) {
     LOCK_GUARD(lock1, obLock);
     outbox.push(response);
@@ -159,10 +161,10 @@ void ModbusServerTCPasync::mb_client::addResponseToOutbox(ModbusMessage * respon
 
 void ModbusServerTCPasync::mb_client::handleOutbox() {
   while (!outbox.empty()) {
-        ModbusMessage * m = outbox.front();
+    ModbusMessage* m = outbox.front();
     if (m->size() <= client->space()) {
       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();
       delete m;
       outbox.pop();
@@ -172,11 +174,11 @@ void ModbusServerTCPasync::mb_client::handleOutbox() {
   }
 }
 
-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'
 }
 
@@ -193,19 +195,19 @@ uint16_t ModbusServerTCPasync::activeClients() {
 }
 
 
-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
   if (server) {
     LOG_W("Server already running.\n");
     return false;
   }
   
-    maxNoClients = max_clients;
+  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->onClient([](void* i, AsyncClient* c) { (static_cast<ModbusServerTCPasync*>(i))->onClientConnect(c); }, this);
     server->begin();
     LOG_D("Modbus server started\n");
     return true;
@@ -215,6 +217,7 @@ bool ModbusServerTCPasync::start(uint16_t port, uint8_t max_clients, uint32_t ti
 }
 
 bool ModbusServerTCPasync::stop() {
+  
   if (!server) {
     LOG_W("Server not running.\n");
     return false;
@@ -238,13 +241,11 @@ bool ModbusServerTCPasync::stop() {
 }
 
 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");
   LOCK_GUARD(lock1, cListLock);
   if (clients.size() < maxNoClients) {
@@ -257,10 +258,10 @@ void ModbusServerTCPasync::onClientConnect(AsyncClient * client) {
   }
 }
 
-void ModbusServerTCPasync::onClientDisconnect(mb_client * 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; });
+  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

@@ -29,29 +29,30 @@ using std::lock_guard;
 #endif
 
 class ModbusServerTCPasync : public ModbusServer {
+
  private:
   class mb_client {
    friend class ModbusServerTCPasync;
    
    public:
-        mb_client(ModbusServerTCPasync * s, AsyncClient * c);
+    mb_client(ModbusServerTCPasync* s, AsyncClient* c);
     ~mb_client();
 
    private:
-        void                        onData(uint8_t * data, size_t len);
+    void onData(uint8_t* data, size_t len);
     void onPoll();
     void onDisconnect();
-        void                        addResponseToOutbox(ModbusMessage * response);
+    void addResponseToOutbox(ModbusMessage* response);
     void handleOutbox();
-        ModbusServerTCPasync *      server;
+    ModbusServerTCPasync* server;
-        AsyncClient *               client;
+    AsyncClient* client;
     uint32_t lastActiveTime;
-        ModbusMessage *             message;
+    ModbusMessage* message;
     Modbus::Error error;
-        std::queue<ModbusMessage *> outbox;
+    std::queue<ModbusMessage*> outbox;
-#if USE_MUTEX
+    #if USE_MUTEX
     std::mutex obLock;  // outbox protection
-#endif
+    #endif
   };
 
 
@@ -66,7 +67,7 @@ class ModbusServerTCPasync : public ModbusServer {
   uint16_t activeClients();
 
   // 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
   bool stop();
@@ -75,18 +76,17 @@ class ModbusServerTCPasync : public ModbusServer {
   bool isRunning();
 
  protected:
-    inline void isInstance() {
+  inline void isInstance() { }
-    }
+  void onClientConnect(AsyncClient* client);
-    void onClientConnect(AsyncClient * client);
+  void onClientDisconnect(mb_client* client);
-    void onClientDisconnect(mb_client * client);
 
-    AsyncServer *          server;
+  AsyncServer* server;
-    std::list<mb_client *> clients;
+  std::list<mb_client*> clients;
   uint8_t maxNoClients;
   uint32_t idle_timeout;
-#if USE_MUTEX
+  #if USE_MUTEX
   std::mutex cListLock;  // client list protection
-#endif
+  #endif
 };
 
 #endif

lib/eModbus/src/ModbusServerTCPtemp.h

@@ -17,13 +17,13 @@ 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
   ModbusServerTCP();
 
@@ -34,18 +34,17 @@ class ModbusServerTCP : public ModbusServer {
   uint16_t activeClients();
 
   // 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
   bool stop();
 
-  protected:
+protected:
   // 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;
@@ -55,18 +54,9 @@ class ModbusServerTCP : public ModbusServer {
   mutex clientLock;
 
   struct ClientData {
-        ClientData()
+    ClientData() : task(nullptr), client(0), timeout(0), parent(nullptr) {}
-            : task(nullptr)
+    ClientData(TaskHandle_t t, CT& c, uint32_t to, ModbusServerTCP<ST, CT> *p) : 
-            , client(0)
+      task(t), client(c), timeout(to), parent(p) {}
-            , 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();
@@ -79,39 +69,37 @@ class ModbusServerTCP : public ModbusServer {
     TaskHandle_t task;
     CT client;
     uint32_t timeout;
-        ModbusServerTCP<ST, CT> * parent;
+    ModbusServerTCP<ST, CT> *parent;
   };
-    ClientData ** clients;
+  ClientData **clients;
 
   // serve: loop function for server task
-    static void serve(ModbusServerTCP<ST, CT> * myself);
+  static void serve(ModbusServerTCP<ST, CT> *myself);
 
   // worker: loop function for client tasks
-    static void worker(ClientData * myData);
+  static void worker(ClientData *myData);
 
   // receive: read data from TCP
-    ModbusMessage receive(CT & client, uint32_t timeWait);
+  ModbusMessage receive(CT& client, uint32_t timeWait);
 
   // accept: start a task to receive requests and respond to a given client
-    bool accept(CT & client, uint32_t timeout, int coreID = -1);
+  bool accept(CT& client, uint32_t timeout, int coreID = -1);
 
   // clientAvailable: return true,. if a client slot is currently unused
-    bool clientAvailable() {
+  bool clientAvailable() { return (numClients - activeClients()) > 0; }
-        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>
@@ -141,27 +129,26 @@ uint16_t ModbusServerTCP<ST, CT>::activeClients() {
         clients[i] = nullptr;
       }
     }
-        if (clients[i] != nullptr)
+    if (clients[i] != nullptr) cnt++;
-            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
       stop();
     }
     // Does the required number of slots fit?
-    if (numClients != max_clients) {
+    if (numClients != maxClients) {
       // No. Drop array and allocate a new one
       delete[] clients;
       // Now allocate a new one
-        numClients = max_clients;
+      numClients = maxClients;
-        clients    = new ClientData *[numClients]();
+      clients = new ClientData*[numClients]();
     }
     serverPort = port;
     serverTimeout = timeout;
@@ -179,11 +166,11 @@ 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?
@@ -202,11 +189,11 @@ bool ModbusServerTCP<ST, CT>::stop() {
       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
   for (uint8_t i = 0; i < numClients; ++i) {
     // Empty slot?
@@ -230,7 +217,7 @@ bool ModbusServerTCP<ST, CT>::accept(CT & client, uint32_t timeout, int coreID)
 }
 
 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
   if (1) {
     // Set up server with given port
@@ -263,12 +250,12 @@ void ModbusServerTCP<ST, CT>::serve(ModbusServerTCP<ST, CT> * myself) {
 }
 
 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
   CT myClient = myData->client;
   uint32_t myTimeOut = myData->timeout;
   // TaskHandle_t myTask = myData->task;
-    ModbusServerTCP<ST, CT> * myParent      = myData->parent;
+  ModbusServerTCP<ST, CT> *myParent = myData->parent;
   unsigned long myLastMessage = millis();
 
   LOG_D("Worker started, timeout=%d\n", myTimeOut);
@@ -312,7 +299,8 @@ void ModbusServerTCP<ST, CT>::worker(ClientData * myData) {
                   break;
                 case 0xF1: // ECHO
                   response = request;
-                                    if (request.getFunctionCode() == WRITE_MULT_REGISTERS || request.getFunctionCode() == WRITE_MULT_COILS) {
+                  if (request.getFunctionCode() == WRITE_MULT_REGISTERS ||
+                      request.getFunctionCode() == WRITE_MULT_COILS) {
                     response.resize(6);
                   }
                   LOG_D("ECHO response\n");
@@ -370,8 +358,7 @@ void ModbusServerTCP<ST, CT>::worker(ClientData * myData) {
   }
 
   // Read away all that may still hang in the buffer
-    while (myClient.read() != -1) {
+  while (myClient.read() != -1) {}
-    }
   // Now stop the client
   myClient.stop();
 
@@ -386,7 +373,7 @@ void ModbusServerTCP<ST, CT>::worker(ClientData * myData) {
 
 // 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
   ModbusMessage m;                    // to take read data
   uint16_t lengthVal = 0;
@@ -395,13 +382,13 @@ ModbusMessage ModbusServerTCP<ST, CT>::receive(CT & client, uint32_t timeWait) {
   uint8_t buffer[BUFFERSIZE];
 
   // 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()) {
         buffer[cnt] = client.read();
         // Are we at the TCP header length field byte #1?
-            if (cnt == 4)
+        if (cnt == 4) lengthVal = buffer[cnt] << 8;
-                lengthVal = buffer[cnt] << 8;
         // Are we at the TCP header length field byte #2?
         if (cnt == 5) {
           lengthVal |= buffer[cnt];

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