feat: add Modbus support

lib/eModbus/src/CoilData.cpp

@@ -0,0 +1,554 @@
+// =================================================================================================
+// eModbus: Copyright 2020, 2021 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+
+#include "CoilData.h"
+#undef LOCAL_LOG_LEVEL
+#include "Logging.h"
+
+// Constructor: optional size in bits, optional initial value for all bits
+// Maximum size is 2000 coils (=250 bytes)
+CoilData::CoilData(uint16_t size, bool initValue) :
+  CDsize(0),
+  CDbyteSize(0), 
+  CDbuffer(nullptr) {
+  // Limit the size to 2000 (Modbus rules)
+  if (size > 2000) size = 2000;
+  // Do we have a size?
+  if (size) {
+    // Calculate number of bytes needed
+    CDbyteSize = byteIndex(size - 1) + 1;
+    // Allocate and init buffer
+    CDbuffer = new uint8_t[CDbyteSize];
+    memset(CDbuffer, initValue ? 0xFF : 0, CDbyteSize);
+    if (initValue) {
+      CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(size - 1)];
+    }
+    CDsize = size;
+  }
+}
+
+// Alternate constructor, taking a "1101..." bit image char array to init
+CoilData::CoilData(const char *initVector) :
+  CDsize(0),
+  CDbyteSize(0), 
+  CDbuffer(nullptr) {
+  // Init with bit image array. 
+  setVector(initVector);
+}
+
+// Destructor: take care of cleaning up
+CoilData::~CoilData() {
+  if (CDbuffer) {
+    delete CDbuffer;
+  }
+}
+
+// Assignment operator
+CoilData& CoilData::operator=(const CoilData& m) {
+  // Remove old data
+  if (CDbuffer) {
+    delete CDbuffer;
+  }
+  // Are coils in source?
+  if (m.CDsize > 0) {
+    // Yes. Allocate new buffer and copy data
+    CDbuffer = new uint8_t[m.CDbyteSize];
+    memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize);
+    CDsize = m.CDsize;
+    CDbyteSize = m.CDbyteSize;
+  } else {
+    // No, leave buffer empty
+    CDsize = 0;
+    CDbyteSize = 0;
+    CDbuffer = nullptr;
+  }
+  return *this;
+}
+
+// Copy constructor
+CoilData::CoilData(const CoilData& m) :
+  CDsize(0),
+  CDbyteSize(0), 
+  CDbuffer(nullptr) {
+  // Has the source coils at all?
+  if (m.CDsize > 0) {
+    // Yes. Allocate new buffer and copy data
+    CDbuffer = new uint8_t[m.CDbyteSize];
+    memcpy(CDbuffer, m.CDbuffer, m.CDbyteSize);
+    CDsize = m.CDsize;
+    CDbyteSize = m.CDbyteSize;
+  }
+}
+
+#ifndef NO_MOVE
+// Move constructor
+CoilData::CoilData(CoilData&& m) {
+  // Copy all data
+  CDbuffer = m.CDbuffer;
+  CDsize = m.CDsize;
+  CDbyteSize = m.CDbyteSize;
+  // Then clear source
+  m.CDbuffer = nullptr;
+  m.CDsize = 0;
+  m.CDbyteSize = 0;
+}
+
+// Move assignment
+CoilData& CoilData::operator=(CoilData&& m) {
+  // Remove buffer, if already allocated
+  if (CDbuffer) {
+    delete CDbuffer;
+  }
+  // Are there coils in the source at all?
+  if (m.CDsize > 0) {
+    // Yes. Copy over all data
+    CDbuffer = m.CDbuffer;
+    CDsize = m.CDsize;
+    CDbyteSize = m.CDbyteSize;
+    // Then clear source
+    m.CDbuffer = nullptr;
+    m.CDsize = 0;
+    m.CDbyteSize = 0;
+  } else {
+    // No, leave object empty.
+    CDbuffer = nullptr;
+    CDsize = 0;
+    CDbyteSize = 0;
+  }
+  return *this;
+}
+#endif
+
+// Comparison operators
+bool CoilData::operator==(const CoilData& m) {
+  // Self-compare is always true
+  if (this == &m) return true;
+  // Different sizes are never equal
+  if (CDsize != m.CDsize) return false;
+  // Compare the data
+  if (CDsize > 0 && memcmp(CDbuffer, m.CDbuffer, CDbyteSize)) return false;
+  return true;
+}
+
+// Inequality: invert the result of the equality comparison
+bool CoilData::operator!=(const CoilData& m) {
+  return !(*this == m);
+}
+
+// Assignment of a bit image char array to re-init
+CoilData& CoilData::operator=(const char *initVector) {
+  // setVector() may be unsuccessful - then data is deleted!
+  setVector(initVector);
+  return *this;
+}
+
+// If used as vector<uint8_t>, return a complete slice
+CoilData::operator vector<uint8_t> const () {
+  // Create new vector to return
+  vector<uint8_t> retval;
+  if (CDsize > 0) {
+    // Copy over all buffer content
+      retval.assign(CDbuffer, CDbuffer + CDbyteSize);
+  }
+  // return the copy (or an empty vector)
+  return retval;
+}
+
+// slice: return a CoilData object with coils shifted leftmost
+// will return empty object if illegal parameters are detected
+CoilData CoilData::slice(uint16_t start, uint16_t length) {
+  CoilData retval;
+
+  // Any slice of an empty coilset is an empty coilset ;)
+  if (CDsize == 0) return retval;
+
+  // If start is beyond the available coils, return empty slice
+  if (start > CDsize) return retval;
+
+  // length default is all up to the end
+  if (length == 0) length = CDsize - start;
+
+  // Does the requested slice fit in the buffer?
+  if ((start + length) <= CDsize) {
+    // Yes, it does. Extend return object
+    retval = CoilData(length);
+
+  // Loop over all requested bits
+    for (uint16_t i = start; i < start + length; ++i) {
+      if (CDbuffer[byteIndex(i)] & (1 << bitIndex(i))) {
+        retval.set(i - start, true);
+      }
+    }
+  }
+  return retval;
+}
+
+// operator[]: return value of a single coil
+bool CoilData::operator[](uint16_t index) const {
+  if (index < CDsize) {
+    return (CDbuffer[byteIndex(index)] & (1 << bitIndex(index))) ? true : false;
+  }
+  // Wrong parameter -> always return false
+  return false;
+}
+
+// set functions to change coil value(s)
+// Will return true if done, false if impossible (wrong address or data)
+
+// set #1: alter one single coil
+bool CoilData::set(uint16_t index, bool value) {
+  // Within coils?
+  if (index < CDsize) {
+    // Yes. Determine affected byte and bit therein
+    uint16_t by = byteIndex(index);
+    uint8_t mask = 1 << bitIndex(index);
+
+    // Stamp out bit
+    CDbuffer[by] &= ~mask;
+    // If required, set it to 1 now
+    if (value) {
+      CDbuffer[by] |= mask;
+    }
+    return true;
+  }
+  // Wrong parameter -> always return false
+  return false;
+}
+
+// set #2: alter a group of coils, overwriting it by the bits from vector newValue
+bool CoilData::set(uint16_t start, uint16_t length, vector<uint8_t> newValue) {
+  // Does the vector contain enough data for the specified size?
+  if (newValue.size() >= (size_t)(byteIndex(length - 1) + 1)) {
+    // Yes, we safely may call set #3 with it
+    return set(start, length, newValue.data());
+  }
+  return false;
+}
+
+// set #3: alter a group of coils, overwriting it by the bits from uint8_t buffer newValue
+// **** Watch out! ****
+// This may be a potential risk if newValue is pointing to an array shorter than required. 
+// Then heap data behind the array may be used to set coils!
+bool CoilData::set(uint16_t start, uint16_t length, uint8_t *newValue) {
+  // Does the requested slice fit in the buffer?
+  if (length && (start + length) <= CDsize) {
+    // Yes, it does. 
+    // Prepare pointers to the source byte and the bit within
+    uint8_t *cp = newValue;
+    uint8_t bitPtr = 0;
+
+    // Loop over all bits to be set
+    for (uint16_t i = start; i < start + length; i++) {
+      // Get affected byte
+      uint8_t by = byteIndex(i);
+      // Calculate single-bit mask in target byte
+      uint8_t mask = 1 << bitIndex(i);
+      // Stamp out bit
+      CDbuffer[by] &= ~mask;
+      // is source bit set?
+      if (*cp & (1 << bitPtr)) {
+        // Yes. Set it in target as well
+        CDbuffer[by] |= mask;
+      }
+      // Advance source bit ptr
+      bitPtr++;
+      // Overflow?
+      if (bitPtr >= 8) {
+        // Yes. move pointers to first bit in next source byte
+        bitPtr = 0;
+        cp++;
+      }
+    }
+    return true;
+  }
+  return false;
+}
+
+// set #4: alter a group of coils, overwriting it by the coils in another CoilData object
+// Setting stops when either target storage or source coils are exhausted
+bool CoilData::set(uint16_t index, const CoilData& c) {
+  // if source object is empty, return false
+  if (c.empty()) return false;
+
+  // If target is empty, or index is beyond coils, return false
+  if (CDsize == 0 || index >= CDsize) return false;
+
+  // Take the minimum of remaining coils after index and the length of c
+  uint16_t length = CDsize - index;
+  if (c.coils() < length) length = c.coils();
+
+  // Loop over all coils to be copied
+  for (uint16_t i = index; i < index + length; ++i) {
+    set(i, c[i - index]);
+  }
+  return true;
+}
+
+// set #5: alter a group of coils, overwriting it by a bit image array
+// Setting stops when either target storage or source bits are exhausted
+bool CoilData::set(uint16_t index, const char *initVector) {
+  // if target is empty or index is beyond coils, return false
+  if (CDsize == 0 || index >= CDsize) return false;
+
+  // We do a single pass on the bit image array, until it ends or the target is exhausted
+  const char *cp = initVector;   // pointer to source array
+  bool skipFlag = false;         // Signal next character irrelevant
+
+  while (*cp && index < CDsize) {
+    switch (*cp) {
+    case '1':  // A valid 1 bit
+    case '0':  // A valid 0 bit
+      // Shall we ignore it?
+      if (skipFlag) {
+        // Yes. just reset the ignore flag
+        skipFlag = false;
+      } else {
+        // No, we can set it. First stamp out the existing bit
+        CDbuffer[byteIndex(index)] &= ~(1 << bitIndex(index));
+        // Do we have a 1 bit here?
+        if (*cp == '1') {
+          // Yes. set it in coil storage
+          CDbuffer[byteIndex(index)] |= (1 << bitIndex(index));
+        }
+        index++;
+      }
+      break;
+    case '_':  // Skip next
+      skipFlag = true;
+      break;
+    default:  // anything else
+      skipFlag = false;
+      break;
+    }
+    cp++;
+  }
+  return true;
+}
+
+// Comparison against bit image array
+bool CoilData::operator==(const char *initVector) {
+  const char *cp = initVector;   // pointer to source array
+  bool skipFlag = false;         // Signal next character irrelevant
+  uint16_t index = 0;
+
+  // We do a single pass on the bit image array, until it ends or the target is exhausted
+  while (*cp && index < CDsize) {
+    switch (*cp) {
+    case '1':  // A valid 1 bit
+    case '0':  // A valid 0 bit
+      // Shall we ignore it?
+      if (skipFlag) {
+        // Yes. just reset the ignore flag
+        skipFlag = false;
+      } else {
+        // No, we can compare it
+        uint8_t value = CDbuffer[byteIndex(index)] & (1 << bitIndex(index));
+        // Do we have a 1 bit here?
+        if (*cp == '1') {
+          // Yes. Is the source different? Then we can stop
+          if (value == 0) return false;
+        } else {
+          // No, it is a 0. Different?
+          if (value) return false;
+        }
+        index++;
+      }
+      break;
+    case '_':  // Skip next
+      skipFlag = true;
+      break;
+    default:  // anything else
+      skipFlag = false;
+      break;
+    }
+    cp++;
+  }
+  // So far everything was equal, but we may have more bits in the image array!
+  if (*cp) {
+    // There is more. Check for more valid bits
+    while (*cp) {
+      switch (*cp) {
+      case '1':  // A valid 1 bit
+      case '0':  // A valid 0 bit
+        // Shall we ignore it?
+        if (skipFlag) {
+          // Yes. just reset the ignore flag
+          skipFlag = false;
+        } else {
+          // No, a valid bit that exceeds the target coils count
+          return false;
+        }
+        break;
+      case '_':  // Skip next
+        skipFlag = true;
+        break;
+      default:  // anything else
+        skipFlag = false;
+        break;
+      }
+      cp++;
+    }
+  }
+  return true;
+}
+
+bool CoilData::operator!=(const char *initVector) {
+  return !(*this == initVector);
+}
+
+// Init all coils by a readable bit image array
+bool CoilData::setVector(const char *initVector) {
+  uint16_t length = 0;           // resulting bit pattern length
+  const char *cp = initVector;   // pointer to source array
+  bool skipFlag = false;         // Signal next character irrelevant
+
+  // Do a first pass to count all valid bits in array
+  while (*cp) {
+    switch (*cp) {
+    case '1':  // A valid 1 bit
+    case '0':  // A valid 0 bit
+      // Shall we ignore it?
+      if (skipFlag) {
+        // Yes. just reset the ignore flag
+        skipFlag = false;
+      } else {
+        // No, we can count it
+        length++;
+      }
+      break;
+    case '_':  // Skip next
+      skipFlag = true;
+      break;
+    default:  // anything else
+      skipFlag = false;
+      break;
+    }
+    cp++;
+  }
+
+  // If there are coils already, trash them.
+  if (CDbuffer) {
+    delete CDbuffer;
+  }
+  CDsize = 0;
+  CDbyteSize = 0;
+
+  // Did we count a manageable number?
+  if (length && length <= 2000) {
+    // Yes. Init the coils
+    CDsize = length;
+    CDbyteSize = byteIndex(length - 1) + 1;
+    // Allocate new coil storage
+    CDbuffer = new uint8_t[CDbyteSize];
+    memset(CDbuffer, 0, CDbyteSize);
+
+    // Prepare second loop
+    uint16_t ptr = 0;    // bit pointer in coil storage
+    skipFlag = false;
+    cp = initVector;
+
+    // Do a second pass, converting 1 and 0 into coils (bits)
+    // loop as above, only difference is setting the bits
+    while (*cp) {
+      switch (*cp) {
+      case '1':
+      case '0':
+        if (skipFlag) {
+          skipFlag = false;
+        } else {
+          // Do we have a 1 bit here?
+          if (*cp == '1') {
+            // Yes. set it in coil storage
+            CDbuffer[byteIndex(ptr)] |= (1 << bitIndex(ptr));
+          }
+          // advance bit pointer in any case 0 or 1
+          ptr++;
+        }
+        break;
+      case '_':
+        skipFlag = true;
+        break;
+      default:
+        skipFlag = false;
+        break;
+      }
+      cp++;
+    }
+    // We had content, so return true
+    return true;
+  }
+  // No valid bits found, return false 
+  return false;
+}
+
+// init: set all coils to 1 or 0 (default)
+void CoilData::init(bool value) {
+  if (CDsize > 0) {
+    memset(CDbuffer, value ? 0xFF : 0, CDbyteSize);
+    // Stamp out overhang bits
+    CDbuffer[CDbyteSize - 1] &= CDfilter[bitIndex(CDsize - 1)];
+  }
+}
+
+// Return number of coils set to 1 (or not)
+// Uses Brian Kernighan's algorithm!
+uint16_t CoilData::coilsSetON() const {
+  uint16_t count = 0;
+
+  // Do we have coils at all?
+  if (CDbyteSize) {
+    // Yes. Loop over all bytes summing up the '1' bits
+    for (uint8_t i = 0; i < CDbyteSize; ++i) {
+      uint8_t by = CDbuffer[i];
+      while (by) {
+        by &= by - 1; // this clears the LSB-most set bit
+        count++;
+      }
+    }
+  }
+  return count;
+}
+
+uint16_t CoilData::coilsSetOFF() const {
+  return CDsize - coilsSetON();
+}
+
+#if !IS_LINUX
+// Not for Linux for the Print reference!
+
+// Print out a coil storage in readable form to ease debugging
+void CoilData::print(const char *label, Print& s) {
+  uint8_t bitptr = 0;
+  uint8_t labellen = strlen(label);
+  uint8_t pos = labellen;
+
+  // Put out the label
+  s.print(label);
+
+  // Print out all coils as "1" or "0"
+  for (uint16_t i = 0; i < CDsize; ++i) {
+    s.print((CDbuffer[byteIndex(i)] & (1 << bitptr)) ? "1" : "0");
+    pos++;
+    // Have a blank after every group of 4
+    if (i % 4 == 3) {
+      // Have a line break if > 80 characters, including the last group of 4
+      if (pos >= 80) {
+        s.println("");
+        pos = 0;
+        // Leave a nice empty space below the label
+        while (pos++ < labellen) {
+          s.print(" ");
+        }
+      } else {
+        s.print(" ");
+        pos++;
+      }
+    }
+    bitptr++;
+    bitptr &= 0x07;
+  }
+  s.println("");
+}
+#endif

lib/eModbus/src/CoilData.h

@@ -0,0 +1,122 @@
+// =================================================================================================
+// eModbus: Copyright 2020, 2021 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+
+#ifndef _COILDATA_H
+#define _COILDATA_H
+
+#include <vector>
+#include <cstdint>
+#include "options.h"
+
+using std::vector;
+
+// CoilData: representing Modbus coil (=bit) values
+class CoilData {
+public:
+  // Constructor: optional size in bits, optional initial value for all bits
+  // Maximum size is 2000 coils (=250 bytes)
+  explicit CoilData(uint16_t size = 0, bool initValue = false);
+
+  // Alternate constructor, taking a "1101..." bit image char array to init
+  explicit CoilData(const char *initVector);
+
+  // Destructor: take care of cleaning up
+  ~CoilData();
+
+  // Assignment operator
+  CoilData& operator=(const CoilData& m);
+  
+  // Copy constructor
+  CoilData(const CoilData& m);
+
+#ifndef NO_MOVE
+  // Move constructor
+	CoilData(CoilData&& m);
+  
+	// Move assignment
+	CoilData& operator=(CoilData&& m);
+#endif
+
+  // Comparison operators
+  bool operator==(const CoilData& m);
+  bool operator!=(const CoilData& m);
+  bool operator==(const char *initVector);
+  bool operator!=(const char *initVector);
+
+  // Assignment of a bit image char array to re-init
+  CoilData& operator=(const char *initVector);
+
+  // If used as vector<uint8_t>, return the complete set
+  operator vector<uint8_t> const ();
+
+  // slice: return a new CoilData object with coils shifted leftmost
+  // will return empty set if illegal parameters are detected
+  // Default start is first coil, default length all to the end
+  CoilData slice(uint16_t start = 0, uint16_t length = 0);
+
+  // operator[]: return value of a single coil
+  bool operator[](uint16_t index) const;
+
+  // Set functions to change coil value(s)
+  // Will return true if done, false if impossible (wrong address or data)
+
+  // set #1: alter one single coil
+  bool set(uint16_t index, bool value);
+
+  // set #2: alter a group of coils, overwriting it by the bits from newValue
+  bool set(uint16_t index, uint16_t length, vector<uint8_t> newValue);
+
+  // set #3: alter a group of coils, overwriting it by the bits from unit8_t buffer newValue
+  bool set(uint16_t index, uint16_t length, uint8_t *newValue);
+
+  // set #4: alter a group of coils, overwriting it by the coils in another CoilData object
+  // Setting stops when either target storage or source coils are exhausted
+  bool set(uint16_t index, const CoilData& c);
+
+  // set #5: alter a group of coils, overwriting it by a bit image array
+  // Setting stops when either target storage or source bits are exhausted
+  bool set(uint16_t index, const char *initVector);
+
+  // (Re-)init complete coil set to 1 or 0
+  void init(bool value = false);
+
+  // get size in coils
+  inline uint16_t coils() const { return CDsize; }
+
+  // Raw access to coil data buffer
+  inline uint8_t *data() const { return CDbuffer; };
+  inline uint8_t size() const { return CDbyteSize; };
+
+  // Test if there are any coils in object
+  inline bool empty() const { return (CDsize >0) ? true : false; }
+  inline operator bool () const { return empty(); }
+
+  // Return number of coils set to 1 (or ON)
+  uint16_t coilsSetON() const;
+  // Return number of coils set to 0 (or OFF)
+  uint16_t coilsSetOFF() const;
+
+#if !LINUX
+  // Helper function to dump out coils in logical order
+  void print(const char *label, Print& s);
+#endif
+
+protected:
+  // bit masks for bits left of a bit index in a byte
+  const uint8_t CDfilter[8] = { 0x01, 0x03, 0x07, 0x0F, 0x1F, 0x3F, 0x7F, 0xFF };
+  // Calculate byte index and bit index within that byte
+  inline uint8_t byteIndex(uint16_t index) const { return index >> 3; }
+  inline uint8_t bitIndex(uint16_t index) const { return index & 0x07; }
+  // Calculate reversed bit sequence for a byte (taken from http://graphics.stanford.edu/~seander/bithacks.html#ReverseByteWith32Bits)
+  inline uint8_t reverseBits(uint8_t b) { return ((b * 0x0802LU & 0x22110LU) | (b * 0x8020LU & 0x88440LU)) * 0x10101LU >> 16; }
+  // (Re-)init with bit image vector
+  bool setVector(const char *initVector);
+
+  uint16_t CDsize;         // Size of the CoilData store in bits
+  uint8_t  CDbyteSize;     // Size in bytes
+  uint8_t *CDbuffer;       // Pointer to bit storage
+};
+
+#endif

lib/eModbus/src/Logging.cpp

@@ -0,0 +1,68 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "Logging.h"
+#include <cinttypes>
+
+int MBUlogLvl = LOG_LEVEL;
+#if IS_LINUX
+#define PrintOut printf
+
+void logHexDump(const char *letter, const char *label, const uint8_t *data, const size_t length) {
+#else
+Print *LOGDEVICE = &Serial;
+#define PrintOut output->printf
+
+void logHexDump(Print *output, const char *letter, const char *label, const uint8_t *data, const size_t length) {
+#endif
+  size_t cnt = 0;
+  size_t step = 0;
+  char limiter = '|';
+  // Use line buffer to speed up output
+  const uint16_t BUFLEN(80);
+  const uint16_t ascOffset(61);
+  char linebuf[BUFLEN];
+  char *cp = linebuf;
+  const char HEXDIGIT[] = "0123456789ABCDEF";
+
+  // Print out header
+  PrintOut ("[%s] %s: @%" PRIXPTR "/%" PRIu32 ":\n", letter, label, (uintptr_t)data, (uint32_t)(length & 0xFFFFFFFF));
+
+  // loop over data in steps of 16
+  for (cnt = 0; cnt < length; ++cnt) {
+    step = cnt % 16;
+    // New line?
+    if (step == 0) {
+      // Yes. Clear line and print address header
+      memset(linebuf, ' ', BUFLEN);
+      linebuf[60] = limiter;
+      linebuf[77] = limiter;
+      linebuf[78] = '\n';
+      linebuf[BUFLEN - 1] = 0;
+      snprintf(linebuf, BUFLEN, "  %c %04X: ", limiter, (uint16_t)(cnt & 0xFFFF));
+      cp = linebuf + strlen(linebuf);
+      // No, but first block of 8 done?
+    } else if (step == 8) {
+      // Yes, put out additional space
+      cp++;
+    }
+    // Print data byte
+    uint8_t c = data[cnt];
+    *cp++ = HEXDIGIT[(c >> 4) & 0x0F];
+    *cp++ = HEXDIGIT[c & 0x0F];
+    *cp++ = ' ';
+    if (c >= 32 && c <= 127) linebuf[ascOffset + step] = c;
+    else                     linebuf[ascOffset + step] = '.';
+    // Line end?
+    if (step == 15) {
+      // Yes, print line
+      PrintOut ("%s", linebuf);
+    }
+  }
+  // Unfinished line?
+  if (length && step != 15) {
+      // Yes, print line
+      PrintOut ("%s", linebuf);
+  }
+}

lib/eModbus/src/Logging.h

@@ -0,0 +1,181 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+
+#ifndef LOG_LEVEL
+#define LOG_LEVEL LOG_LEVEL_ERROR
+#endif
+
+#ifndef LOCAL_LOG_LEVEL
+#define LOCAL_LOG_LEVEL LOG_LEVEL
+#endif
+
+// The following needs to be defined only once
+#ifndef _MODBUS_LOGGING
+#define _MODBUS_LOGGING
+#include "options.h"
+
+#define LOG_LEVEL_NONE (0)
+#define LOG_LEVEL_CRITICAL (1)
+#define LOG_LEVEL_ERROR (2)
+#define LOG_LEVEL_WARNING (3)
+#define LOG_LEVEL_INFO (4)
+#define LOG_LEVEL_DEBUG (5)
+#define LOG_LEVEL_VERBOSE (6)
+
+#define LL_RED "\e[1;31m"
+#define LL_GREEN "\e[32m"
+#define LL_YELLOW "\e[1;33m"
+#define LL_BLUE "\e[34m"
+#define LL_MAGENTA "\e[35m"
+#define LL_CYAN "\e[36m"
+#define LL_NORM "\e[0m"
+
+#define LOG_HEADER(x) "[" #x "] %lu| %-20s [%4d] %s: "
+
+constexpr const char* str_end(const char *str) {
+    return *str ? str_end(str + 1) : str;
+}
+
+constexpr bool str_slant(const char *str) {
+    return ((*str == '/') || (*str == '\\')) ? true : (*str ? str_slant(str + 1) : false);
+}
+
+constexpr const char* r_slant(const char* str) {
+    return ((*str == '/') || (*str == '\\')) ? (str + 1) : r_slant(str - 1);
+}
+constexpr const char* file_name(const char* str) {
+    return str_slant(str) ? r_slant(str_end(str)) : str;
+}
+
+#if IS_LINUX
+void logHexDump(const char *letter, const char *label, const uint8_t *data, const size_t length);
+#else
+extern Print *LOGDEVICE;
+void logHexDump(Print *output, const char *letter, const char *label, const uint8_t *data, const size_t length);
+#endif
+extern int MBUlogLvl;
+#endif  // _MODBUS_LOGGING
+
+// The remainder may need to be redefined if LOCAL_LOG_LEVEL was set differently before
+#ifdef LOG_LINE_T
+#undef LOG_LINE_C
+#undef LOG_LINE_E
+#undef LOG_LINE_T
+#undef LOG_RAW_C
+#undef LOG_RAW_E
+#undef LOG_RAW_T
+#undef HEX_DUMP_T
+#undef LOG_N
+#undef LOG_C
+#undef LOG_E
+#undef LOG_W
+#undef LOG_I
+#undef LOG_D
+#undef LOG_V
+#undef LOGRAW_N
+#undef LOGRAW_C
+#undef LOGRAW_E
+#undef LOGRAW_W
+#undef LOGRAW_I
+#undef LOGRAW_D
+#undef LOGRAW_V
+#undef HEXDUMP_N
+#undef HEXDUMP_C
+#undef HEXDUMP_E
+#undef HEXDUMP_W
+#undef HEXDUMP_I
+#undef HEXDUMP_D
+#undef HEXDUMP_V
+#endif
+
+// Now we can define the macros based on LOCAL_LOG_LEVEL
+#if IS_LINUX
+#define LOG_LINE_C(level, x, format, ...) if (MBUlogLvl >= level) printf(LL_RED LOG_HEADER(x) format LL_NORM, millis(), file_name(__FILE__), __LINE__, __func__, ##__VA_ARGS__)
+#define LOG_LINE_E(level, x, format, ...) if (MBUlogLvl >= level) printf(LL_YELLOW LOG_HEADER(x) format LL_NORM, millis(), file_name(__FILE__), __LINE__, __func__, ##__VA_ARGS__)
+#define LOG_LINE_T(level, x, format, ...) if (MBUlogLvl >= level) printf(LOG_HEADER(x) format, millis(), file_name(__FILE__), __LINE__, __func__, ##__VA_ARGS__)
+#define LOG_RAW_C(level, x, format, ...) if (MBUlogLvl >= level) printf(LL_RED format LL_NORM, ##__VA_ARGS__)
+#define LOG_RAW_E(level, x, format, ...) if (MBUlogLvl >= level) printf(LL_YELLOW format LL_NORM, ##__VA_ARGS__)
+#define LOG_RAW_T(level, x, format, ...) if (MBUlogLvl >= level) printf(format, ##__VA_ARGS__)
+#define HEX_DUMP_T(x, level, label, address, length) if (MBUlogLvl >= level) logHexDump(#x, label, address, length)
+#else
+#define LOG_LINE_C(level, x, format, ...) if (MBUlogLvl >= level) LOGDEVICE->printf(LL_RED LOG_HEADER(x) format LL_NORM, millis(), file_name(__FILE__), __LINE__, __func__, ##__VA_ARGS__)
+#define LOG_LINE_E(level, x, format, ...) if (MBUlogLvl >= level) LOGDEVICE->printf(LL_YELLOW LOG_HEADER(x) format LL_NORM, millis(), file_name(__FILE__), __LINE__, __func__, ##__VA_ARGS__)
+#define LOG_LINE_T(level, x, format, ...) if (MBUlogLvl >= level) LOGDEVICE->printf(LOG_HEADER(x) format, millis(), file_name(__FILE__), __LINE__, __func__, ##__VA_ARGS__)
+#define LOG_RAW_C(level, x, format, ...) if (MBUlogLvl >= level) LOGDEVICE->printf(LL_RED format LL_NORM, ##__VA_ARGS__)
+#define LOG_RAW_E(level, x, format, ...) if (MBUlogLvl >= level) LOGDEVICE->printf(LL_YELLOW format LL_NORM, ##__VA_ARGS__)
+#define LOG_RAW_T(level, x, format, ...) if (MBUlogLvl >= level) LOGDEVICE->printf(format, ##__VA_ARGS__)
+#define HEX_DUMP_T(x, level, label, address, length) if (MBUlogLvl >= level) logHexDump(LOGDEVICE, #x, label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_NONE
+#define LOG_N(format, ...) LOG_LINE_T(LOG_LEVEL_NONE, N, format, ##__VA_ARGS__)
+#define LOGRAW_N(format, ...) LOG_RAW_T(LOG_LEVEL_NONE, N, format, ##__VA_ARGS__)
+#define HEXDUMP_N(label, address, length) HEX_DUMP_T(N, LOG_LEVEL_NONE, label, address, length)
+#else
+#define LOG_N(format, ...)
+#define LOGRAW_N(format, ...)
+#define HEXDUMP_N(label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_CRITICAL
+#define LOG_C(format, ...) LOG_LINE_C(LOG_LEVEL_CRITICAL, C, format, ##__VA_ARGS__)
+#define LOGRAW_C(format, ...) LOG_RAW_C(LOG_LEVEL_CRITICAL, C, format, ##__VA_ARGS__)
+#define HEXDUMP_C(label, address, length) HEX_DUMP_T(C, LOG_LEVEL_CRITICAL, label, address, length)
+#else
+#define LOG_C(format, ...)
+#define LOGRAW_C(format, ...)
+#define HEXDUMP_C(label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_ERROR
+#define LOG_E(format, ...) LOG_LINE_E(LOG_LEVEL_ERROR, E, format, ##__VA_ARGS__)
+#define LOGRAW_E(format, ...) LOG_RAW_E(LOG_LEVEL_ERROR, E, format, ##__VA_ARGS__)
+#define HEXDUMP_E(label, address, length) HEX_DUMP_T(E, LOG_LEVEL_ERROR, label, address, length)
+#else
+#define LOG_E(format, ...)
+#define LOGRAW_E(format, ...)
+#define HEXDUMP_E(label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_WARNING
+#define LOG_W(format, ...) LOG_LINE_T(LOG_LEVEL_WARNING, W, format, ##__VA_ARGS__)
+#define LOGRAW_W(format, ...) LOG_RAW_T(LOG_LEVEL_WARNING, W, format, ##__VA_ARGS__)
+#define HEXDUMP_W(label, address, length) HEX_DUMP_T(W, LOG_LEVEL_WARNING, label, address, length)
+#else
+#define LOG_W(format, ...)
+#define LOGRAW_W(format, ...)
+#define HEXDUMP_W(label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_INFO
+#define LOG_I(format, ...) LOG_LINE_T(LOG_LEVEL_INFO, I, format, ##__VA_ARGS__)
+#define LOGRAW_I(format, ...) LOG_RAW_T(LOG_LEVEL_INFO, I, format, ##__VA_ARGS__)
+#define HEXDUMP_I(label, address, length) HEX_DUMP_T(I, LOG_LEVEL_INFO, label, address, length)
+#else
+#define LOG_I(format, ...)
+#define LOGRAW_I(format, ...)
+#define HEXDUMP_I(label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_DEBUG
+#define LOG_D(format, ...) LOG_LINE_T(LOG_LEVEL_DEBUG, D, format, ##__VA_ARGS__)
+#define LOGRAW_D(format, ...) LOG_RAW_T(LOG_LEVEL_DEBUG, D, format, ##__VA_ARGS__)
+#define HEXDUMP_D(label, address, length) HEX_DUMP_T(D, LOG_LEVEL_DEBUG, label, address, length)
+#else
+#define LOG_D(format, ...)
+#define LOGRAW_D(format, ...)
+#define HEXDUMP_D(label, address, length)
+#endif
+
+#if LOCAL_LOG_LEVEL >= LOG_LEVEL_VERBOSE
+#define LOG_V(format, ...) LOG_LINE_T(LOG_LEVEL_VERBOSE, V, format, ##__VA_ARGS__)
+#define LOGRAW_V(format, ...) LOG_RAW_T(LOG_LEVEL_VERBOSE, V, format, ##__VA_ARGS__)
+#define HEXDUMP_V(label, address, length) HEX_DUMP_T(V, LOG_LEVEL_VERBOSE, label, address, length)
+#else
+#define LOG_V(format, ...)
+#define LOGRAW_V(format, ...)
+#define HEXDUMP_V(label, address, length)
+#endif
+

lib/eModbus/src/ModbusBridgeEthernet.h

@@ -0,0 +1,21 @@
+// =================================================================================================
+// eModbus: Copyright 2020 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 <Ethernet.h>
+#include <SPI.h>
+
+#undef SERVER_END
+#define SERVER_END // NIL for Ethernet
+
+#include "ModbusServerTCPtemp.h"
+#include "ModbusBridgeTemp.h"
+
+using ModbusBridgeEthernet = ModbusBridge<ModbusServerTCP<EthernetServer, EthernetClient>>;
+#endif
+
+#endif

lib/eModbus/src/ModbusBridgeRTU.h

@@ -0,0 +1,14 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_BRIDGE_RTU_H
+#define _MODBUS_BRIDGE_RTU_H
+#include "options.h"
+#include "ModbusServerRTU.h"
+#include "ModbusBridgeTemp.h"
+#include "RTUutils.h"
+
+using ModbusBridgeRTU = ModbusBridge<ModbusServerRTU>;
+
+#endif

lib/eModbus/src/ModbusBridgeTemp.h

@@ -0,0 +1,199 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_BRIDGE_TEMP_H
+#define _MODBUS_BRIDGE_TEMP_H
+
+#include <map>
+#include <functional>
+#include "ModbusClient.h"
+#include "ModbusClientTCP.h"  // Needed for client.setTarget()
+#include "RTUutils.h"  // Needed for RTScallback
+
+#undef LOCAL_LOG_LEVEL
+#define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+#include "Logging.h"
+
+using std::bind;
+using std::placeholders::_1;
+
+// Known server types: TCP (client, host/port) and RTU (client)
+enum ServerType : uint8_t { TCP_SERVER, RTU_SERVER };
+
+// Bridge class template, takes one of ModbusServerRTU, ModbusServerWiFi, ModbusServerEthernet or ModbusServerTCPasync as parameter
+template<typename SERVERCLASS>
+class ModbusBridge : public SERVERCLASS {
+public:
+  // Constructor for TCP server variants.
+  ModbusBridge();
+
+  // Constructors for the RTU variant. Parameters as are for ModbusServerRTU
+  ModbusBridge(HardwareSerial& serial, uint32_t timeout, int rtsPin = -1);
+  ModbusBridge(HardwareSerial& serial, uint32_t timeout, RTScallback rts);
+
+  // Destructor
+  ~ModbusBridge();
+
+  // Method to link external servers to the bridge
+  bool attachServer(uint8_t aliasID, uint8_t serverID, uint8_t functionCode, ModbusClient *client, IPAddress host = IPAddress(0, 0, 0, 0), uint16_t port = 0);
+
+  // Link another function code to the server
+  bool addFunctionCode(uint8_t aliasID, uint8_t functionCode);
+
+  // Block a function code (respond with ILLEGAL_FUNCTION error)
+  bool denyFunctionCode(uint8_t aliasID, uint8_t functionCode);
+
+protected:
+  // ServerData holds all data necessary to address a single server
+  struct ServerData {
+    uint8_t serverID;             // External server id
+    ModbusClient *client;         // client to be used to request the server
+    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
+
+    // RTU constructor
+    ServerData(uint8_t sid, ModbusClient *c) :
+      serverID(sid),
+      client(c),
+      serverType(RTU_SERVER),
+      host(IPAddress(0, 0, 0, 0)),
+      port(0) {}
+    
+    // TCP constructor
+    ServerData(uint8_t sid, ModbusClient *c, IPAddress h, uint16_t p) :
+      serverID(sid),
+      client(c),
+      serverType(TCP_SERVER),
+      host(h),
+      port(p) {}
+  };
+
+  // Default worker functions
+  ModbusMessage bridgeWorker(ModbusMessage msg);
+  ModbusMessage bridgeDenyWorker(ModbusMessage msg);
+
+  // Map of servers attached
+  std::map<uint8_t, ServerData *> servers;
+};
+
+// Constructor for TCP variants
+template<typename SERVERCLASS>
+ModbusBridge<SERVERCLASS>::ModbusBridge() :
+  SERVERCLASS() { } 
+
+// Constructors for RTU variant
+template<typename SERVERCLASS>
+ModbusBridge<SERVERCLASS>::ModbusBridge(HardwareSerial& serial, uint32_t timeout, int rtsPin) :
+  SERVERCLASS(serial, timeout, rtsPin) { }
+
+// Alternate constructors for RTU variant
+template<typename SERVERCLASS>
+ModbusBridge<SERVERCLASS>::ModbusBridge(HardwareSerial& serial, uint32_t timeout, RTScallback rts) :
+  SERVERCLASS(serial, timeout, rts) { }
+
+// Destructor
+template<typename SERVERCLASS>
+ModbusBridge<SERVERCLASS>::~ModbusBridge() { 
+  // Release ServerData storage in servers array
+  for (auto itr = servers.begin(); itr != servers.end(); itr++) {
+    delete (itr->second);
+  }
+  servers.clear();
+}
+
+// attachServer: memorize the access data for an external server with ID serverID under bridge ID aliasID
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::attachServer(uint8_t aliasID, uint8_t serverID, uint8_t functionCode, ModbusClient *client, IPAddress host, uint16_t port) {
+
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) == servers.end()) {
+    // No. Store server data in map.
+
+    // Do we have a port number?
+    if (port != 0) {
+      // Yes. Must be a TCP client
+      servers[aliasID] = new ServerData(serverID, static_cast<ModbusClient *>(client), host, port);
+      LOG_D("(TCP): %02X->%02X %d.%d.%d.%d:%d\n", aliasID, serverID, host[0], host[1], host[2], host[3], port);
+    } else {
+      // No - RTU client required
+      servers[aliasID] = new ServerData(serverID, static_cast<ModbusClient *>(client));
+      LOG_D("(RTU): %02X->%02X\n", aliasID, serverID);
+    }
+  }
+
+  // Register the server/FC combination for the bridgeWorker
+  addFunctionCode(aliasID, functionCode);
+  return true;
+}
+
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::addFunctionCode(uint8_t aliasID, uint8_t functionCode) {
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) != servers.end()) {
+    // Yes. Link server to own worker function
+    this->registerWorker(aliasID, functionCode, std::bind(&ModbusBridge<SERVERCLASS>::bridgeWorker, this, std::placeholders::_1));
+    LOG_D("FC %02X added for server %02X\n", functionCode, aliasID);
+  } else {
+    LOG_E("Server %d not attached to bridge!\n", aliasID);
+    return false;
+  }
+  return true;
+}
+
+template<typename SERVERCLASS>
+bool ModbusBridge<SERVERCLASS>::denyFunctionCode(uint8_t aliasID, uint8_t functionCode) {
+  // Is there already an entry for the aliasID?
+  if (servers.find(aliasID) != servers.end()) {
+    // Yes. Link server to own worker function
+    this->registerWorker(aliasID, functionCode, std::bind(&ModbusBridge<SERVERCLASS>::bridgeDenyWorker, this, std::placeholders::_1));
+    LOG_D("FC %02X blocked for server %02X\n", functionCode, aliasID);
+  } else {
+    LOG_E("Server %d not attached to bridge!\n", aliasID);
+    return false;
+  }
+  return true;
+}
+
+// bridgeWorker: default worker function to process bridge requests
+template<typename SERVERCLASS>
+ModbusMessage ModbusBridge<SERVERCLASS>::bridgeWorker(ModbusMessage msg) {
+  uint8_t aliasID = msg.getServerID();
+  uint8_t functionCode = msg.getFunctionCode();
+  ModbusMessage response;
+
+  // Find the (alias) serverID
+  if (servers.find(aliasID) != servers.end()) {
+    // Found it. We may use servers[aliasID] now without allocating a new map slot
+
+    // Set real target server ID
+    msg.setServerID(servers[aliasID]->serverID);
+
+    // Issue the request
+    LOG_D("Request (%02X/%02X) sent\n", servers[aliasID]->serverID, functionCode);
+    // 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);
+    } else {
+      response = servers[aliasID]->client->syncRequestM(msg, (uint32_t)millis());
+    }
+
+    // Re-set the requested server ID
+    response.setServerID(aliasID);
+  } else {
+    // If we get here, something has gone wrong internally. We send back an error response anyway.
+    response.setError(aliasID, functionCode, INVALID_SERVER);
+  }
+  return response;
+}
+
+// bridgeDenyWorker: worker function to block function codes
+template<typename SERVERCLASS>
+ModbusMessage ModbusBridge<SERVERCLASS>::bridgeDenyWorker(ModbusMessage msg) {
+  ModbusMessage response;
+  response.setError(msg.getServerID(), msg.getFunctionCode(), ILLEGAL_FUNCTION);
+  return response;
+}
+
+#endif

lib/eModbus/src/ModbusBridgeWiFi.h

@@ -0,0 +1,18 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_BRIDGE_WIFI_H
+#define _MODBUS_BRIDGE_WIFI_H
+#include "options.h"
+#include <WiFi.h>
+
+#undef SERVER_END
+#define SERVER_END server.end();
+
+#include "ModbusServerTCPtemp.h"
+#include "ModbusBridgeTemp.h"
+
+using ModbusBridgeWiFi = ModbusBridge<ModbusServerTCP<WiFiServer, WiFiClient>>;
+
+#endif

lib/eModbus/src/ModbusClient.cpp

@@ -0,0 +1,103 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusClient.h"
+#undef LOCAL_LOG_LEVEL
+#include "Logging.h"
+
+uint16_t ModbusClient::instanceCounter = 0;
+
+// Default constructor: set the default timeout to 2000ms, zero out all other 
+ModbusClient::ModbusClient() :
+  messageCount(0),
+  errorCount(0),
+  #if HAS_FREERTOS
+  worker(NULL),
+  #elif IS_LINUX
+  worker(0),
+  #endif
+  onData(nullptr),
+  onError(nullptr),
+  onResponse(nullptr) { instanceCounter++; }
+
+// onDataHandler: register callback for data responses
+bool ModbusClient::onDataHandler(MBOnData handler) {
+  if (onData) {
+    LOG_W("onData handler was already claimed\n");
+  } else if (onResponse) {
+    LOG_E("onData handler is unavailable with an onResponse handler\n");
+    return false;
+  }
+  onData = handler;
+  return true;
+}
+
+// onErrorHandler: register callback for error responses
+bool ModbusClient::onErrorHandler(MBOnError handler) {
+  if (onError) {
+    LOG_W("onError handler was already claimed\n");
+  } else if (onResponse) {
+    LOG_E("onError handler is unavailable with an onResponse handler\n");
+    return false;
+  } 
+  onError = handler;
+  return true;
+}
+
+// onResponseHandler: register callback for error responses
+bool ModbusClient::onResponseHandler(MBOnResponse handler) {
+  if (onError || onData) {
+    LOG_E("onResponse handler is unavailable with an onData or onError handler\n");
+    return false;
+  } 
+  onResponse = handler;
+  return true;
+}
+
+// getMessageCount: return message counter value
+uint32_t ModbusClient::getMessageCount() {
+  return messageCount;
+}
+
+// getErrorCount: return error counter value
+uint32_t ModbusClient::getErrorCount() {
+  return errorCount;
+}
+
+// resetCounts: Set both message and error counts to zero
+void ModbusClient::resetCounts() {
+  {
+    LOCK_GUARD(cntLock, countAccessM);
+    messageCount = 0;
+    errorCount = 0;
+  }
+}
+
+// waitSync: wait for response on syncRequest to arrive
+ModbusMessage ModbusClient::waitSync(uint8_t serverID, uint8_t functionCode, uint32_t token) {
+  ModbusMessage response;
+  unsigned long lostPatience = millis();
+ 
+  // Default response is TIMEOUT
+  response.setError(serverID, functionCode, TIMEOUT);
+
+  // Loop 60 seconds, if unlucky
+  while (millis() - lostPatience < 60000) {
+    {
+      LOCK_GUARD(lg, syncRespM);
+      // Look for the token
+      auto sR = syncResponse.find(token);
+      // Is it there?
+      if (sR != syncResponse.end()) {
+        // Yes. get the response, delete it from the map and return
+        response = sR->second;
+        syncResponse.erase(sR);
+        break;
+      }
+    }
+    // Give the watchdog time to act
+    delay(10);
+  }
+  return response;
+}

lib/eModbus/src/ModbusClient.h

@@ -0,0 +1,119 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_CLIENT_H
+#define _MODBUS_CLIENT_H
+
+#include <functional> 
+#include <map>
+#include "options.h"
+#include "ModbusMessage.h"
+
+#if HAS_FREERTOS
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
+#elif IS_LINUX
+#include <pthread.h>
+#endif
+
+#if USE_MUTEX
+#include <mutex>                    // NOLINT
+using std::mutex;
+using std::lock_guard;
+#endif
+
+typedef std::function<void(ModbusMessage msg, uint32_t token)> MBOnData;
+typedef std::function<void(Modbus::Error errorCode, uint32_t token)> MBOnError;
+typedef std::function<void(ModbusMessage msg, uint32_t token)> MBOnResponse;
+
+class ModbusClient {
+public:
+  bool onDataHandler(MBOnData handler);   // Accept onData handler 
+  bool onErrorHandler(MBOnError handler); // Accept onError handler 
+  bool onResponseHandler(MBOnResponse handler); // Accept onResponse handler 
+  uint32_t getMessageCount();             // Informative: return number of messages created
+  uint32_t getErrorCount();              // Informative: return number of errors received
+  void resetCounts();                    // Set both message and error counts to zero
+  inline Error addRequest(ModbusMessage m, uint32_t token) { return addRequestM(m, token); }
+  inline ModbusMessage syncRequest(ModbusMessage m, uint32_t token) { return syncRequestM(m, token); }
+
+  // Template function to generate syncRequest functions as long as there is a 
+  // matching ModbusMessage::setMessage() call
+  template <typename... Args>
+  ModbusMessage syncRequest(uint32_t token, Args&&... args) {
+    Error rc = SUCCESS;
+    // Create request, if valid
+    ModbusMessage m;
+    rc = m.setMessage(std::forward<Args>(args) ...);
+
+    // Add it to the queue and wait for a response, if valid
+    if (rc == SUCCESS) {
+      return syncRequestM(m, token);
+    } 
+    // Else return the error as a message
+    return buildErrorMsg(rc, std::forward<Args>(args) ...);
+  }
+
+  // Template function to create an error response message from a variadic pattern
+  template <typename... Args>
+  ModbusMessage buildErrorMsg(Error e, uint8_t serverID, uint8_t functionCode, Args&&... args) {
+    ModbusMessage m;
+    m.setError(serverID, functionCode, e);
+    return m;
+  }
+
+  // Template function to generate addRequest functions as long as there is a 
+  // matching ModbusMessage::setMessage() call
+  template <typename... Args>
+  Error addRequest(uint32_t token, Args&&... args) {
+    Error rc = SUCCESS;        // Return value
+
+    // Create request, if valid
+    ModbusMessage m;
+    rc = m.setMessage(std::forward<Args>(args) ...);
+
+    // Add it to the queue, if valid
+    if (rc == SUCCESS) {
+      return addRequestM(m, token);
+    }
+    // Else return the error
+    return rc;
+  }
+
+protected:
+  ModbusClient();             // Default constructor
+  virtual void isInstance() = 0;   // Make class abstract
+  ModbusMessage waitSync(uint8_t serverID, uint8_t functionCode, uint32_t token); // wait for syncRequest response to arrive
+  // Virtual addRequest variant needed internally. All others done by template!
+  virtual Error addRequestM(ModbusMessage msg, uint32_t token) = 0;
+  // Virtual syncRequest variant following the same pattern
+  virtual ModbusMessage syncRequestM(ModbusMessage msg, uint32_t token) = 0;
+  // Prevent copy construction or assignment
+  ModbusClient(ModbusClient& other) = delete;
+  ModbusClient& operator=(ModbusClient& other) = delete;
+
+  uint32_t messageCount;           // Number of requests generated. Used for transactionID in TCPhead
+  uint32_t errorCount;             // Number of errors received
+#if HAS_FREERTOS
+  TaskHandle_t worker;             // Interface instance worker task
+#elif IS_LINUX
+  pthread_t worker;
+#endif
+  MBOnData onData;                 // Data response handler
+  MBOnError onError;               // Error response handler
+  MBOnResponse onResponse;         // Uniform response handler
+  static uint16_t instanceCounter; // Number of ModbusClients created
+  std::map<uint32_t, ModbusMessage> syncResponse; // Map to hold response messages on synchronous requests
+#if USE_MUTEX
+  std::mutex syncRespM;            // Mutex protecting syncResponse map against race conditions
+  std::mutex countAccessM;         // Mutex protecting access to the message and error counts
+#endif
+
+  // Let any ModbusBridge class use protected members
+  template<typename SERVERCLASS> friend class ModbusBridge;
+};
+
+#endif

lib/eModbus/src/ModbusClientRTU.cpp

@@ -0,0 +1,349 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusClientRTU.h"
+
+#if HAS_FREERTOS
+
+#undef LOCAL_LOG_LEVEL
+// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+#include "Logging.h"
+
+// Constructor takes an optional DE/RE pin and queue size
+ModbusClientRTU::ModbusClientRTU(int8_t rtsPin, uint16_t queueLimit) :
+  ModbusClient(),
+  MR_serial(nullptr),
+  MR_lastMicros(micros()),
+  MR_interval(2000),
+  MR_rtsPin(rtsPin),
+  MR_qLimit(queueLimit),
+  MR_timeoutValue(DEFAULTTIMEOUT),
+  MR_useASCII(false),
+  MR_skipLeadingZeroByte(false) {
+    if (MR_rtsPin >= 0) {
+      pinMode(MR_rtsPin, OUTPUT);
+      MTRSrts = [this](bool level) {
+        digitalWrite(MR_rtsPin, level);
+      };
+      MTRSrts(LOW);
+    } else {
+      MTRSrts = RTUutils::RTSauto;
+    }
+}
+
+// Alternative constructor takes an RTS callback function
+ModbusClientRTU::ModbusClientRTU(RTScallback rts, uint16_t queueLimit) :
+  ModbusClient(),
+  MR_serial(nullptr),
+  MR_lastMicros(micros()),
+  MR_interval(2000),
+  MTRSrts(rts),
+  MR_qLimit(queueLimit),
+  MR_timeoutValue(DEFAULTTIMEOUT),
+  MR_useASCII(false),
+  MR_skipLeadingZeroByte(false) {
+    MR_rtsPin = -1;
+    MTRSrts(LOW);
+}
+
+// Destructor: clean up queue, task etc.
+ModbusClientRTU::~ModbusClientRTU() {
+  // Kill worker task and clean up request queue
+  end();
+}
+
+// begin: start worker task - general version
+void ModbusClientRTU::begin(Stream& serial, uint32_t baudRate, int coreID) {
+  MR_serial = &serial;
+  doBegin(baudRate, coreID);
+}
+
+// begin: start worker task - HardwareSerial version
+void ModbusClientRTU::begin(HardwareSerial& serial, int coreID) {
+  MR_serial = &serial;
+  uint32_t baudRate = serial.baudRate();
+  serial.setRxFIFOFull(1);
+  doBegin(baudRate, coreID);
+}
+
+void ModbusClientRTU::doBegin(uint32_t baudRate, int coreID) {
+  // Task already running? End it in case
+  end();
+
+  // Pull down RTS toggle, if necessary
+  MTRSrts(LOW);
+
+  // Set minimum interval time
+  MR_interval = RTUutils::calculateInterval(baudRate);
+
+  // Create unique task name
+  char taskName[18];
+  snprintf(taskName, 18, "Modbus%02XRTU", instanceCounter);
+  // Start task to handle the queue
+  xTaskCreatePinnedToCore((TaskFunction_t)&handleConnection, taskName, CLIENT_TASK_STACK, this, 6, &worker, coreID >= 0 ? coreID : NULL);
+
+  LOG_D("Client task %d started. Interval=%d\n", (uint32_t)worker, MR_interval);
+}
+
+// end: stop worker task
+void ModbusClientRTU::end() {
+  if (worker) {
+    // Clean up queue
+    {
+      // Safely lock access
+      LOCK_GUARD(lockGuard, qLock);
+      // Get all queue entries one by one
+      while (!requests.empty()) {
+        // Remove front entry
+        requests.pop();
+      }
+    }
+    // Kill task
+    vTaskDelete(worker);
+    LOG_D("Client task %d killed.\n", (uint32_t)worker);
+    worker = nullptr;
+  }
+}
+
+// setTimeOut: set/change the default interface timeout
+void ModbusClientRTU::setTimeout(uint32_t TOV) {
+  MR_timeoutValue = TOV;
+  LOG_D("Timeout set to %d\n", TOV);
+}
+
+// Toggle protocol to ModbusASCII
+void ModbusClientRTU::useModbusASCII(unsigned long timeout) {
+  MR_useASCII = true;
+  MR_timeoutValue = timeout; // Switch timeout to ASCII's value
+  LOG_D("Protocol mode: ASCII\n");
+}
+
+// Toggle protocol to ModbusRTU
+void ModbusClientRTU::useModbusRTU() {
+  MR_useASCII = false;
+  LOG_D("Protocol mode: RTU\n");
+}
+
+// Inquire protocol mode
+bool ModbusClientRTU::isModbusASCII() {
+  return MR_useASCII;
+}
+
+// Toggle skipping of leading 0x00 byte
+void ModbusClientRTU::skipLeading0x00(bool onOff) {
+  MR_skipLeadingZeroByte = onOff;
+  LOG_D("Skip leading 0x00 mode = %s\n", onOff ? "ON" : "OFF");
+}
+
+// Return number of unprocessed requests in queue
+uint32_t ModbusClientRTU::pendingRequests() {
+  return requests.size();
+}
+
+// Remove all pending request from queue
+void ModbusClientRTU::clearQueue()
+{
+  std::queue<RequestEntry> empty;
+  LOCK_GUARD(lockGuard, qLock);
+  std::swap(requests, empty);
+}
+
+// Base addRequest taking a preformatted data buffer and length as parameters
+Error ModbusClientRTU::addRequestM(ModbusMessage msg, uint32_t token) {
+  Error rc = SUCCESS;        // Return value
+
+  LOG_D("request for %02X/%02X\n", msg.getServerID(), msg.getFunctionCode());
+
+  // Add it to the queue, if valid
+  if (msg) {
+    // Queue add successful?
+    if (!addToQueue(token, msg)) {
+      // No. Return error after deleting the allocated request.
+      rc = REQUEST_QUEUE_FULL;
+    }
+  }
+
+  LOG_D("RC=%02X\n", rc);
+  return rc;
+}
+
+// Base syncRequest follows the same pattern
+ModbusMessage ModbusClientRTU::syncRequestM(ModbusMessage msg, uint32_t token) {
+  ModbusMessage response;
+
+  if (msg) {
+    // Queue add successful?
+    if (!addToQueue(token, msg, true)) {
+      // No. Return error after deleting the allocated request.
+      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);
+  }
+  return response;
+}
+
+// addBroadcastMessage: create a fire-and-forget message to all servers on the RTU bus
+Error ModbusClientRTU::addBroadcastMessage(const uint8_t *data, uint8_t len) {
+  Error rc = SUCCESS;        // Return value
+
+  LOG_D("Broadcast request of length %d\n", len);
+
+  // We do only accept requests with data, 0 byte, data and CRC must fit into 256 bytes.
+  if (len && len < 254) {
+    // Create a "broadcast token"
+    uint32_t token = (millis() & 0xFFFFFF) | 0xBC000000;
+    ModbusMessage msg;
+    
+    // Server ID is 0x00 for broadcast
+    msg.add((uint8_t)0x00);
+    // Append data
+    msg.add(data, len);
+
+    // Queue add successful?
+    if (!addToQueue(token, msg)) {
+      // No. Return error after deleting the allocated request.
+      rc = REQUEST_QUEUE_FULL;
+    }
+  } else {
+    rc =  BROADCAST_ERROR;
+  }
+
+  LOG_D("RC=%02X\n", rc);
+  return rc;
+}
+
+
+// addToQueue: send freshly created request to queue
+bool ModbusClientRTU::addToQueue(uint32_t token, ModbusMessage request, bool syncReq) {
+  bool rc = false;
+  // Did we get one?
+  if (request) {
+    RequestEntry re(token, request, syncReq);
+    if (requests.size()<MR_qLimit) {
+      // Yes. Safely lock queue and push request to queue
+      rc = true;
+      LOCK_GUARD(lockGuard, qLock);
+      requests.push(re);
+    }
+    {
+      LOCK_GUARD(cntLock, countAccessM);
+      messageCount++;
+    }
+  }
+
+  LOG_D("RC=%02X\n", rc);
+  return rc;
+}
+
+// handleConnection: worker task
+// This was created in begin() to handle the queue entries
+void ModbusClientRTU::handleConnection(ModbusClientRTU *instance) {
+  // initially clean the serial buffer
+  while (instance->MR_serial->available()) instance->MR_serial->read();
+  delay(100);
+
+  // Loop forever - or until task is killed
+  while (1) {
+    // Do we have a reuest in queue?
+    if (!instance->requests.empty()) {
+      // Yes. pull it.
+      RequestEntry request = instance->requests.front();
+
+      LOG_D("Pulled request from queue\n");
+
+      // Send it via Serial
+      RTUutils::send(*(instance->MR_serial), instance->MR_lastMicros, instance->MR_interval, instance->MTRSrts, request.msg, instance->MR_useASCII);
+
+      LOG_D("Request sent.\n");
+      // HEXDUMP_V("Data", request.msg.data(), request.msg.size());
+
+      // For a broadcast, we will not wait for a response
+      if (request.msg.getServerID() != 0 || ((request.token & 0xFF000000) != 0xBC000000)) {
+        // This is a regular request, Get the response - if any
+        ModbusMessage response = RTUutils::receive(
+          'C',
+          *(instance->MR_serial), 
+          instance->MR_timeoutValue, 
+          instance->MR_lastMicros, 
+          instance->MR_interval, 
+          instance->MR_useASCII,
+          instance->MR_skipLeadingZeroByte);
+  
+        LOG_D("%s response (%d bytes) received.\n", response.size()>1 ? "Data" : "Error", response.size());
+        HEXDUMP_V("Data", response.data(), response.size());
+  
+        // No error in receive()?
+        if (response.size() > 1) {
+          // No. Check message contents
+          // Does the serverID match the requested?
+          if (request.msg.getServerID() != response.getServerID()) {
+            // No. Return error response
+            response.setError(request.msg.getServerID(), request.msg.getFunctionCode(), SERVER_ID_MISMATCH);
+          // ServerID ok, but does the FC match as well?
+          } else if (request.msg.getFunctionCode() != (response.getFunctionCode() & 0x7F)) {
+            // No. Return error response
+            response.setError(request.msg.getServerID(), request.msg.getFunctionCode(), FC_MISMATCH);
+          } 
+        } else {
+          // No, we got an error code from receive()
+          // Return it as error response
+          response.setError(request.msg.getServerID(), request.msg.getFunctionCode(), static_cast<Error>(response[0]));
+        }
+  
+        LOG_D("Response generated.\n");
+        HEXDUMP_V("Response packet", response.data(), response.size());
+
+        // If we got an error, count it
+        if (response.getError() != SUCCESS) {
+          instance->errorCount++;
+        }
+  
+        // Was it a synchronous request?
+        if (request.isSyncRequest) {
+          // Yes. Put it into the response map
+          {
+            LOCK_GUARD(sL, instance->syncRespM);
+            instance->syncResponse[request.token] = response;
+          }
+        // No, an async request. Do we have an onResponse handler?
+        } else if (instance->onResponse) {
+          // Yes. Call it
+          instance->onResponse(response, request.token);
+        } else {
+          // No, but we may have onData or onError handlers
+          // Did we get a normal response?
+          if (response.getError()==SUCCESS) {
+            // Yes. Do we have an onData handler registered?
+            if (instance->onData) {
+              // Yes. call it
+              instance->onData(response, request.token);
+            }
+          } else {
+            // No, something went wrong. All we have is an error
+            // Do we have an onError handler?
+            if (instance->onError) {
+              // Yes. Forward the error code to it
+              instance->onError(response.getError(), request.token);
+            }
+          }
+        }
+      }
+      // Clean-up time. 
+      {
+        // Safely lock the queue
+        LOCK_GUARD(lockGuard, instance->qLock);
+        // Remove the front queue entry
+        instance->requests.pop();
+      }
+    } else {
+      delay(1);
+    }
+  }
+}
+
+#endif  // HAS_FREERTOS

lib/eModbus/src/ModbusClientRTU.h

@@ -0,0 +1,111 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_CLIENT_RTU_H
+#define _MODBUS_CLIENT_RTU_H
+
+#include "options.h"
+
+#if HAS_FREERTOS
+
+#include "ModbusClient.h"
+#include "Stream.h"
+#include "RTUutils.h"
+#include <queue>
+#include <vector>
+
+using std::queue;
+
+#define DEFAULTTIMEOUT 2000
+
+class ModbusClientRTU : public ModbusClient {
+public:
+  // Constructor takes an optional DE/RE pin and queue limit
+  explicit ModbusClientRTU(int8_t rtsPin = -1, uint16_t queueLimit = 100);
+
+  // Alternative Constructor takes an RTS line toggle callback
+  explicit ModbusClientRTU(RTScallback rts, uint16_t queueLimit = 100);
+
+  // Destructor: clean up queue, task etc.
+  ~ModbusClientRTU();
+
+  // begin: start worker task
+  void begin(Stream& serial, uint32_t baudrate, int coreID = -1);
+  // Special variant for HardwareSerial
+  void begin(HardwareSerial& serial, int coreID = -1);
+
+  // end: stop the worker
+  void end();
+
+  // Set default timeout value for interface
+  void setTimeout(uint32_t TOV);
+
+  // Toggle protocol to ModbusASCII
+  void useModbusASCII(unsigned long timeout = 1000);
+
+  // Toggle protocol to ModbusRTU
+  void useModbusRTU();
+
+  // Inquire protocol mode
+  bool isModbusASCII();
+
+  // Toggle skipping of leading 0x00 byte
+  void skipLeading0x00(bool onOff = true);
+
+  // Return number of unprocessed requests in queue
+  uint32_t pendingRequests();
+
+  // Remove all pending request from queue
+  void clearQueue();
+  
+  // addBroadcastMessage: create a fire-and-forget message to all servers on the RTU bus
+  Error addBroadcastMessage(const uint8_t *data, uint8_t len);
+
+protected:
+  struct RequestEntry {
+    uint32_t token;
+    ModbusMessage msg;
+    bool isSyncRequest;
+    RequestEntry(uint32_t t, ModbusMessage m, bool syncReq = false) :
+      token(t),
+      msg(m),
+      isSyncRequest(syncReq) {}
+  };
+
+  // Base addRequest and syncRequest must be present
+  Error addRequestM(ModbusMessage msg, uint32_t token);
+  ModbusMessage syncRequestM(ModbusMessage msg, uint32_t token);
+
+  // addToQueue: send freshly created request to queue
+  bool addToQueue(uint32_t token, ModbusMessage msg, bool syncReq = false);
+
+  // handleConnection: worker task method
+  static void handleConnection(ModbusClientRTU *instance);
+
+  // receive: get response via Serial
+  ModbusMessage receive(const ModbusMessage request);
+
+  // start background task
+  void doBegin(uint32_t baudRate, int coreID);
+
+  void isInstance() { return; }   // make class instantiable
+  queue<RequestEntry> requests;   // Queue to hold requests to be processed
+  #if USE_MUTEX
+  mutex qLock;                    // Mutex to protect queue
+  #endif
+  Stream *MR_serial;              // Ptr to the serial interface used
+  unsigned long MR_lastMicros;    // Microseconds since last bus activity
+  uint32_t MR_interval;           // Modbus RTU bus quiet time
+  int8_t MR_rtsPin;               // GPIO pin to toggle RS485 DE/RE line. -1 if none.
+  RTScallback MTRSrts;            // RTS line callback function
+  uint16_t MR_qLimit;             // Maximum number of requests to hold in the queue
+  uint32_t MR_timeoutValue;       // Interface default timeout
+  bool MR_useASCII;               // true=ModbusASCII, false=ModbusRTU
+  bool MR_skipLeadingZeroByte;    // true=skip the first byte if it is 0x00, false=accept all bytes
+
+};
+
+#endif  // HAS_FREERTOS
+
+#endif  // INCLUDE GUARD

lib/eModbus/src/ModbusClientTCP.cpp

@@ -0,0 +1,428 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusClientTCP.h"
+
+#if HAS_FREERTOS || IS_LINUX
+
+#undef LOCAL_LOG_LEVEL
+// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+#include "Logging.h"
+
+// Constructor takes reference to Client (EthernetClient or WiFiClient)
+ModbusClientTCP::ModbusClientTCP(Client& client, uint16_t queueLimit) :
+  ModbusClient(),
+  MT_client(client),
+  MT_lastTarget(IPAddress(0, 0, 0, 0), 0, DEFAULTTIMEOUT, TARGETHOSTINTERVAL),
+  MT_target(IPAddress(0, 0, 0, 0), 0, DEFAULTTIMEOUT, TARGETHOSTINTERVAL),
+  MT_defaultTimeout(DEFAULTTIMEOUT),
+  MT_defaultInterval(TARGETHOSTINTERVAL),
+  MT_qLimit(queueLimit)
+  { }
+
+// Alternative Constructor takes reference to Client (EthernetClient or WiFiClient) plus initial target host
+ModbusClientTCP::ModbusClientTCP(Client& client, IPAddress host, uint16_t port, uint16_t queueLimit) :
+  ModbusClient(),
+  MT_client(client),
+  MT_lastTarget(IPAddress(0, 0, 0, 0), 0, DEFAULTTIMEOUT, TARGETHOSTINTERVAL),
+  MT_target(host, port, DEFAULTTIMEOUT, TARGETHOSTINTERVAL),
+  MT_defaultTimeout(DEFAULTTIMEOUT),
+  MT_defaultInterval(TARGETHOSTINTERVAL),
+  MT_qLimit(queueLimit)
+  { }
+
+// Destructor: clean up queue, task etc.
+ModbusClientTCP::~ModbusClientTCP() {
+  end();
+}
+
+// end: stop worker task
+void ModbusClientTCP::end() {
+  // Clean up queue
+  {
+    // Safely lock access
+    LOCK_GUARD(lockGuard, qLock);
+    // Get all queue entries one by one
+    while (!requests.empty()) {
+      requests.pop();
+    }
+  }
+  LOG_D("TCP client worker killed.\n");
+  // Kill task
+  if (worker) {
+#if IS_LINUX
+    pthread_cancel(worker);
+    worker = NULL;
+#else
+    vTaskDelete(worker);
+    worker = nullptr;
+#endif
+  }
+}
+
+// begin: start worker task
+#if IS_LINUX
+void *ModbusClientTCP::pHandle(void *p) {
+  handleConnection((ModbusClientTCP *)p);
+  return nullptr;
+}
+#endif
+
+void ModbusClientTCP::begin(int coreID) {
+  if (!worker) {
+#if IS_LINUX
+    int rc = pthread_create(&worker, NULL, &pHandle, this);
+    if (rc) {
+      LOG_E("Error creating TCP client thread: %d\n", rc);
+    } else {
+      LOG_D("TCP client worker started.\n");
+    }
+
+#else
+    // Create unique task name
+    char taskName[18];
+    snprintf(taskName, 18, "Modbus%02XTCP", instanceCounter);
+    // Start task to handle the queue
+    xTaskCreatePinnedToCore((TaskFunction_t)&handleConnection, taskName, CLIENT_TASK_STACK, this, 5, &worker, coreID >= 0 ? coreID : NULL);
+    LOG_D("TCP client worker %s started\n", taskName);
+#endif
+  } else {
+    LOG_E("Worker thread has been already started!");
+  }
+}
+
+// Set default timeout value (and interval)
+void ModbusClientTCP::setTimeout(uint32_t timeout, uint32_t interval) {
+  MT_defaultTimeout = timeout;
+  MT_defaultInterval = interval;
+}
+
+// Switch target host (if necessary)
+// Return true, if host/port is different from last host/port used
+bool ModbusClientTCP::setTarget(IPAddress host, uint16_t port, uint32_t timeout, uint32_t interval) {
+  MT_target.host = host;
+  MT_target.port = port;
+  MT_target.timeout = timeout ? timeout : MT_defaultTimeout;
+  MT_target.interval = interval ? interval : MT_defaultInterval;
+  LOG_D("Target set: %d.%d.%d.%d:%d\n", host[0], host[1], host[2], host[3], port);
+  if (MT_target.host == MT_lastTarget.host && MT_target.port == MT_lastTarget.port) return false;
+  return true;
+}
+
+// Return number of unprocessed requests in queue
+uint32_t ModbusClientTCP::pendingRequests() {
+  return requests.size();
+}
+
+// Remove all pending request from queue
+void ModbusClientTCP::clearQueue() {
+  std::queue<RequestEntry *> empty;
+  LOCK_GUARD(lockGuard, qLock);
+  std::swap(requests, empty);
+}
+
+// Base addRequest for preformatted ModbusMessage and last set target
+Error ModbusClientTCP::addRequestM(ModbusMessage msg, uint32_t token) {
+  Error rc = SUCCESS;        // Return value
+
+  // Add it to the queue, if valid
+  if (msg) {
+    // Queue add successful?
+    if (!addToQueue(token, msg, MT_target)) {
+      // No. Return error after deleting the allocated request.
+      rc = REQUEST_QUEUE_FULL;
+    }
+  }
+
+  LOG_D("Add TCP request result: %02X\n", rc);
+  return rc;
+}
+
+// TCP addRequest for preformatted ModbusMessage and adhoc target
+Error ModbusClientTCP::addRequestMT(ModbusMessage msg, uint32_t token, IPAddress targetHost, uint16_t targetPort) {
+  Error rc = SUCCESS;        // Return value
+
+  // Add it to the queue, if valid
+  if (msg) {
+    // Set up adhoc target 
+    TargetHost adhocTarget(targetHost, targetPort, MT_defaultTimeout, MT_defaultInterval);
+    // Queue add successful?
+    if (!addToQueue(token, msg, adhocTarget, true)) {
+      // No. Return error after deleting the allocated request.
+      rc = REQUEST_QUEUE_FULL;
+    }
+  }
+
+  LOG_D("Add TCP request result: %02X\n", rc);
+  return rc;
+}
+
+// Base syncRequest follows the same pattern
+ModbusMessage ModbusClientTCP::syncRequestM(ModbusMessage msg, uint32_t token) {
+  ModbusMessage response;
+
+  if (msg) {
+    // Queue add successful?
+    if (!addToQueue(token, msg, MT_target, true)) {
+      // No. Return error after deleting the allocated request.
+      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);
+  }
+  return response;
+}
+
+// TCP syncRequest with adhoc target parameters
+ModbusMessage ModbusClientTCP::syncRequestMT(ModbusMessage msg, uint32_t token, IPAddress targetHost, uint16_t targetPort) {
+  ModbusMessage response;
+
+  if (msg) {
+    // Set up adhoc target 
+    TargetHost adhocTarget(targetHost, targetPort, MT_defaultTimeout, MT_defaultInterval);
+    // Queue add successful?
+    if (!addToQueue(token, msg, adhocTarget, true)) {
+      // No. Return error after deleting the allocated request.
+      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);
+  }
+  return response;
+}
+
+// addToQueue: send freshly created request to queue
+bool ModbusClientTCP::addToQueue(uint32_t token, ModbusMessage request, TargetHost target, bool syncReq) {
+  bool rc = false;
+  // Did we get one?
+  LOG_D("Queue size: %d\n", (uint32_t)requests.size());
+  HEXDUMP_D("Enqueue", request.data(), request.size());
+  if (request) {
+    if (requests.size()<MT_qLimit) {
+      RequestEntry *re = new RequestEntry(token, request, target, syncReq);
+      // inject proper transactionID
+      re->head.transactionID = messageCount++;
+      re->head.len = request.size();
+      // Safely lock queue and push request to queue
+      rc = true;
+      LOCK_GUARD(lockGuard, qLock);
+      requests.push(re);
+    }
+  }
+
+  return rc;
+}
+
+// handleConnection: worker task
+// This was created in begin() to handle the queue entries
+void ModbusClientTCP::handleConnection(ModbusClientTCP *instance) {
+  bool doNotPop;
+  unsigned long lastRequest = millis();
+
+  // Loop forever - or until task is killed
+  while (1) {
+    // Do we have a request in queue?
+    if (!instance->requests.empty()) {
+      // Yes. pull it.
+      RequestEntry *request = instance->requests.front();
+      doNotPop = false;
+      LOG_D("Got request from queue\n");
+
+      // Do we have a connection open?
+      if (instance->MT_client.connected()) {
+        // Empty the RX buffer in case there is a stray response left
+        while (instance->MT_client.read() != -1) {}
+        // check if lastHost/lastPort!=host/port off the queued request
+        if (instance->MT_lastTarget != request->target) {
+          // It is different. Disconnect it.
+          instance->MT_client.stop();
+          LOG_D("Target different, disconnect\n");
+          delay(1);  // Give scheduler room to breathe
+        } else {
+          // it is the same host/port.
+          // Give it some slack to get ready again
+          while (millis() - lastRequest < request->target.interval) { delay(1); }
+        }
+      }
+      // if client is disconnected (we will have to switch hosts)
+      if (!instance->MT_client.connected()) {
+        // Serial.println("Client reconnecting");
+        // It is disconnected. connect to host/port from queue
+        instance->MT_client.connect(request->target.host, request->target.port);
+        LOG_D("Target connect (%d.%d.%d.%d:%d).\n", request->target.host[0], request->target.host[1], request->target.host[2], request->target.host[3], request->target.port);
+
+        delay(1);  // Give scheduler room to breathe
+      }
+      ModbusMessage response;
+      // Are we connected (again)?
+      if (instance->MT_client.connected()) {
+        LOG_D("Is connected. Send request.\n");
+        // Yes. Send the request via IP
+        instance->send(request);
+
+        // Get the response - if any
+        response = instance->receive(request);
+
+        // Did we get a normal response?
+        if (response.getError()==SUCCESS) {
+          LOG_D("Data response.\n");
+          // Yes. Is it a synchronous request?
+          if (request->isSyncRequest) {
+            // Yes. Put the response into the response map
+            {
+              LOCK_GUARD(sL, instance->syncRespM);
+              instance->syncResponse[request->token] = response;
+            }
+          // No, async request. Do we have an onResponse handler?
+          } else if (instance->onResponse) {
+            // Yes. Call it.
+            instance->onResponse(response, request->token);
+          // No, but do we have an onData handler registered?
+          } else if (instance->onData) {
+            // Yes. call it
+            instance->onData(response, request->token);
+          } else {
+            LOG_D("No handler for response!\n");
+          }
+        } else {
+          // No, something went wrong. All we have is an error
+          LOG_D("Error response.\n");
+          // Count it
+          {
+            LOCK_GUARD(responseCnt, instance->countAccessM);
+            instance->errorCount++;
+          }
+          // Is it a synchronous request?
+          if (request->isSyncRequest) {
+            // Yes. Put the response into the response map
+            {
+              LOCK_GUARD(sL, instance->syncRespM);
+              instance->syncResponse[request->token] = response;
+            }
+          // No, but do we have an onResponse handler?
+          } else if (instance->onResponse) {
+            // Yes, call it.
+            instance->onResponse(response, request->token);
+          // No, but do we have an onError handler?
+          } else if (instance->onError) {
+            // Yes. Forward the error code to it
+            instance->onError(response.getError(), request->token);
+          } else {
+            LOG_D("No onError handler\n");
+          }
+        }
+        //   set lastHost/lastPort tp host/port
+        instance->MT_lastTarget = request->target;
+      } else {
+        // Oops. Connection failed
+        response.setError(request->msg.getServerID(), request->msg.getFunctionCode(), IP_CONNECTION_FAILED);
+        // Is it a synchronous request?
+        if (request->isSyncRequest) {
+          // Yes. Put the response into the response map
+          {
+            LOCK_GUARD(sL, instance->syncRespM);
+            instance->syncResponse[request->token] = response;
+          }
+        // No, but do we have an onResponse handler?
+        } else if (instance->onResponse) {
+          // Yes, call it.
+          instance->onResponse(response, request->token);
+        // Finally, do we have an onError handler?
+        } else if (instance->onError) {
+          // Yes. Forward the error code to it
+          instance->onError(IP_CONNECTION_FAILED, request->token);
+        }
+      }
+      // Clean-up time. 
+      if (!doNotPop)
+      {
+        // Safely lock the queue
+        LOCK_GUARD(lockGuard, instance->qLock);
+        // Remove the front queue entry
+        instance->requests.pop();
+        // Delete request
+        delete request;
+        LOG_D("Request popped from queue.\n");
+      }
+      lastRequest = millis();
+    } else {
+      delay(1);  // Give scheduler room to breathe
+    }
+  }
+}
+
+// send: send request via Client connection
+void ModbusClientTCP::send(RequestEntry *request) {
+  // We have a established connection here, so we can write right away.
+  // Move tcpHead and request into one continuous buffer, since the very first request tends to 
+  // take too long to be sent to be recognized.
+  ModbusMessage m;
+  m.add((const uint8_t *)request->head, 6);
+  m.append(request->msg);
+
+  MT_client.write(m.data(), m.size());
+  // Done. Are we?
+  MT_client.flush();
+  HEXDUMP_V("Request packet", m.data(), m.size());
+}
+
+// receive: get response via Client connection
+ModbusMessage ModbusClientTCP::receive(RequestEntry *request) {
+  unsigned long lastMillis = millis();     // Timer to check for timeout
+  bool hadData = false;               // flag data received
+  const uint16_t dataLen(300);        // Modbus Packet supposedly will fit (260<300)
+  uint8_t data[dataLen];              // Local buffer to collect received data
+  uint16_t dataPtr = 0;               // Pointer into data
+  ModbusMessage response;             // Response structure to be returned
+
+  // wait for packet data, overflow or timeout
+  while (millis() - lastMillis < request->target.timeout && dataPtr < dataLen && !hadData) {
+    // Is there data waiting?
+    if (MT_client.available()) {
+      // Yes. catch as much as is there and fits into buffer
+      while (MT_client.available() && dataPtr < dataLen) {
+        data[dataPtr++] = MT_client.read();
+      }
+      // Register data received
+      hadData = true;
+      // Rewind EOT and timeout timers
+      lastMillis = millis();
+    }
+    delay(1); // Give scheduler room to breathe
+  }
+  // Did we get some data?
+  if (hadData) {
+    LOG_D("Received response.\n");
+    HEXDUMP_V("Response packet", data, dataPtr);
+    // Yes. check it for validity
+    // First transactionID and protocolID shall be identical, length has to match the remainder.
+    ModbusTCPhead head(request->head.transactionID, request->head.protocolID, dataPtr - 6);
+    // Matching head?
+    if (memcmp((const uint8_t *)head, data, 6)) {
+      // No. return Error response
+      response.setError(request->msg.getServerID(), request->msg.getFunctionCode(), TCP_HEAD_MISMATCH);
+      // If the server id does not match that of the request, report error
+    } else if (data[6] != request->msg.getServerID()) {
+      response.setError(request->msg.getServerID(), request->msg.getFunctionCode(), SERVER_ID_MISMATCH);
+      // If the function code does not match that of the request, report error
+    } else if ((data[7] & 0x7F) != request->msg.getFunctionCode()) {
+      response.setError(request->msg.getServerID(), request->msg.getFunctionCode(), FC_MISMATCH);
+    } else {
+      // Looks good.
+      response.add(data + 6, dataPtr - 6);
+    }
+  } else {
+    // No, timeout must have struck
+    response.setError(request->msg.getServerID(), request->msg.getFunctionCode(), TIMEOUT);
+  }
+  return response;
+}
+
+#endif

lib/eModbus/src/ModbusClientTCP.h

@@ -0,0 +1,195 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_CLIENT_TCP_H
+#define _MODBUS_CLIENT_TCP_H
+
+#include "options.h"
+
+#if HAS_FREERTOS || IS_LINUX
+#if HAS_FREERTOS
+#include <Arduino.h>
+#endif
+
+#include "ModbusClient.h"
+#include "Client.h"
+#include <queue>
+#include <vector>
+using std::queue;
+
+#define TARGETHOSTINTERVAL 10
+#define DEFAULTTIMEOUT 2000
+
+class ModbusClientTCP : public ModbusClient {
+public:
+  // Constructor takes reference to Client (EthernetClient or WiFiClient)
+  explicit ModbusClientTCP(Client& client, uint16_t queueLimit = 100);
+
+  // Alternative Constructor takes reference to Client (EthernetClient or WiFiClient) plus initial target host
+  ModbusClientTCP(Client& client, IPAddress host, uint16_t port, uint16_t queueLimit = 100);
+
+  // Destructor: clean up queue, task etc.
+  ~ModbusClientTCP();
+
+  // begin: start worker task
+  void begin(int coreID = -1);
+
+  // end: stop worker task
+  void end();
+
+  // Set default timeout value (and interval)
+  void setTimeout(uint32_t timeout = DEFAULTTIMEOUT, uint32_t interval = TARGETHOSTINTERVAL);
+
+  // Switch target host (if necessary)
+  bool setTarget(IPAddress host, uint16_t port, uint32_t timeout = 0, uint32_t interval = 0);
+
+  // Return number of unprocessed requests in queue
+  uint32_t pendingRequests();
+
+  // Remove all pending request from queue
+  void clearQueue();
+
+protected:
+  // class describing a target server
+  struct TargetHost {
+    IPAddress     host;         // IP address
+    uint16_t      port;         // Port number
+    uint32_t      timeout;      // Time in ms waiting for a response
+    uint32_t      interval;     // Time in ms to wait between requests
+    
+    inline TargetHost& operator=(TargetHost& t) {
+      host = t.host;
+      port = t.port;
+      timeout = t.timeout;
+      interval = t.interval;
+      return *this;
+    }
+    
+    inline TargetHost(TargetHost& t) :
+      host(t.host),
+      port(t.port),
+      timeout(t.timeout),
+      interval(t.interval) {}
+    
+    inline TargetHost() :
+      host(IPAddress(0, 0, 0, 0)),
+      port(0),
+      timeout(0),
+      interval(0)
+    { }
+
+    inline TargetHost(IPAddress host, uint16_t port, uint32_t timeout, uint32_t interval) :
+      host(host),
+      port(port),
+      timeout(timeout),
+      interval(interval)
+    { }
+
+    inline bool operator==(TargetHost& t) {
+      if (host != t.host) return false;
+      if (port != t.port) return false;
+      return true;
+    }
+
+    inline bool operator!=(TargetHost& t) {
+      if (host != t.host) return true;
+      if (port != t.port) return true;
+      return false;
+    }
+  };
+
+  // class describing the TCP header of Modbus packets
+  class ModbusTCPhead {
+  public:
+    ModbusTCPhead() :
+    transactionID(0),
+    protocolID(0),
+    len(0) {}
+
+    ModbusTCPhead(uint16_t tid, uint16_t pid, uint16_t _len) :
+    transactionID(tid),
+    protocolID(pid),
+    len(_len) {}
+
+    uint16_t transactionID;     // Caller-defined identification
+    uint16_t protocolID;        // const 0x0000
+    uint16_t len;               // Length of remainder of TCP packet
+
+    inline explicit operator const uint8_t *() {
+      uint8_t *cp = headRoom;
+      *cp++ = (transactionID >> 8) & 0xFF;
+      *cp++ = transactionID  & 0xFF;
+      *cp++ = (protocolID >> 8) & 0xFF;
+      *cp++ = protocolID  & 0xFF;
+      *cp++ = (len >> 8) & 0xFF;
+      *cp++ = len  & 0xFF;
+      return headRoom;
+    }
+
+    inline ModbusTCPhead& operator= (ModbusTCPhead& t) {
+      transactionID = t.transactionID;
+      protocolID    = t.protocolID;
+      len           = t.len;
+      return *this;
+    }
+
+  protected:
+    uint8_t headRoom[6];        // Buffer to hold MSB-first TCP header
+  };
+
+  struct RequestEntry {
+    uint32_t token;
+    ModbusMessage msg;
+    TargetHost target;
+    ModbusTCPhead head;
+    bool isSyncRequest;
+    RequestEntry(uint32_t t, ModbusMessage m, TargetHost tg, bool syncReq = false) :
+      token(t),
+      msg(m),
+      target(tg),
+      head(ModbusTCPhead()),
+      isSyncRequest(syncReq) {}
+  };
+
+  // Base addRequest and syncRequest must be present
+  Error addRequestM(ModbusMessage msg, uint32_t token);
+  ModbusMessage syncRequestM(ModbusMessage msg, uint32_t token);
+  // TCP-specific addition "...MT()" including adhoc target - used by bridge 
+  Error addRequestMT(ModbusMessage msg, uint32_t token, IPAddress targetHost, uint16_t targetPort);
+  ModbusMessage syncRequestMT(ModbusMessage msg, uint32_t token, IPAddress targetHost, uint16_t targetPort);
+
+  // addToQueue: send freshly created request to queue
+  bool addToQueue(uint32_t token, ModbusMessage request, TargetHost target, bool syncReq = false);
+
+  // handleConnection: worker task method
+  static void handleConnection(ModbusClientTCP *instance);
+#if IS_LINUX
+  static void *pHandle(void *p);
+#endif
+
+  // send: send request via Client connection
+  void send(RequestEntry *request);
+
+  // receive: get response via Client connection
+  ModbusMessage receive(RequestEntry *request);
+
+  void isInstance() { return; }   // make class instantiable
+  queue<RequestEntry *> requests;   // Queue to hold requests to be processed
+  #if USE_MUTEX
+  mutex qLock;                    // Mutex to protect queue
+  #endif
+  Client& MT_client;              // Client reference for Internet connections (EthernetClient or WifiClient)
+  TargetHost MT_lastTarget;       // last used server
+  TargetHost MT_target;           // Description of target server
+  uint32_t MT_defaultTimeout;     // Standard timeout value taken if no dedicated was set
+  uint32_t MT_defaultInterval;    // Standard interval value taken if no dedicated was set
+  uint16_t MT_qLimit;             // Maximum number of requests to accept in queue
+
+  // Let any ModbusBridge class use protected members
+  template<typename SERVERCLASS> friend class ModbusBridge;
+};
+
+#endif  // HAS_FREERTOS
+
+#endif  // INCLUDE GUARD

lib/eModbus/src/ModbusClientTCPasync.cpp

@@ -0,0 +1,401 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusClientTCPasync.h"
+#define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+// #undef LOCAL_LOG_LEVEL
+#include "Logging.h"
+
+ModbusClientTCPasync::ModbusClientTCPasync(IPAddress address, uint16_t port, uint16_t queueLimit) :
+  ModbusClient(),
+  txQueue(),
+  rxQueue(),
+  MTA_client(),
+  MTA_timeout(DEFAULTTIMEOUT),
+  MTA_idleTimeout(DEFAULTIDLETIME),
+  MTA_qLimit(queueLimit),
+  MTA_maxInflightRequests(queueLimit),
+  MTA_lastActivity(0),
+  MTA_state(DISCONNECTED),
+  MTA_host(address),
+  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.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.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, AsyncClient* c, 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() {
+  // Clean up queue
+  {
+    // 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();
+    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
+
+  // Add it to the queue, if valid
+  if (msg) {
+    // Queue add successful?
+    if (!addToQueue(token, msg)) {
+      // No. Return error after deleting the allocated request.
+      rc = REQUEST_QUEUE_FULL;
+    }
+  }
+
+  LOG_D("Add TCP request result: %02X\n", rc);
+  return rc;
+}
+
+// Base syncRequest follows the same pattern
+ModbusMessage ModbusClientTCPasync::syncRequestM(ModbusMessage msg, uint32_t token) {
+  ModbusMessage response;
+
+  if (msg) {
+    // Queue add successful?
+    if (!addToQueue(token, msg, true)) {
+      // No. Return error after deleting the allocated request.
+      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);
+  }
+  return response;
+}
+
+// addToQueue: send freshly created request to queue
+bool ModbusClientTCPasync::addToQueue(int32_t token, ModbusMessage request, bool syncReq) {
+  // Did we get one?
+  if (request) {
+    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);
+      if (!re) return false;  //TODO: proper error returning in case allocation fails
+      // inject proper transactionID
+      re->head.transactionID = messageCount++;
+      re->head.len = request.size();
+      // if we're already connected, try to send and push to rxQueue
+      // or else push to txQueue and (re)connect
+      if (MTA_state == CONNECTED && send(re)) {
+        re->sentTime = millis();
+        rxQueue[re->head.transactionID] = re;
+      } else {
+        txQueue.push_back(re);
+        if (MTA_state == DISCONNECTED) {
+          connect();
+        }
+      }
+      return true;
+    }
+    LOG_E("queue is full\n");
+  }
+  return false;
+}
+
+void ModbusClientTCPasync::onConnected() {
+  LOG_D("connected\n");
+  LOCK_GUARD(lock1, sLock);
+  MTA_state = CONNECTED;
+  MTA_lastActivity = millis();
+  // from now on onPoll will be called every 500 msec
+}
+
+void ModbusClientTCPasync::onDisconnected() {
+  LOG_D("disconnected\n");
+  LOCK_GUARD(lock1, sLock);
+  MTA_state = DISCONNECTED;
+
+  // empty queue on disconnect, calling errorcode on every waiting request
+  LOCK_GUARD(lock2, qLock);
+  while (!txQueue.empty()) {
+    RequestEntry* r = txQueue.front();
+    if (onError) {
+      onError(IP_CONNECTION_FAILED, r->token);
+    }
+    delete r;
+    txQueue.pop_front();
+  }
+  while (!rxQueue.empty()) {
+    RequestEntry *r = rxQueue.begin()->second;
+    if (onError) {
+      onError(IP_CONNECTION_FAILED, r->token);
+    }
+    delete r;
+    rxQueue.erase(rxQueue.begin());
+  }
+}
+
+
+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));
+}
+
+/*
+void onTimeout(uint32_t time) {
+  // timeOut is handled by onPoll or onDisconnect
+}
+
+void onAck(size_t len, uint32_t time) {
+  // assuming we don't need this
+}
+*/
+void ModbusClientTCPasync::onPacket(uint8_t* data, size_t length) {
+  LOG_D("packet received (len:%d)\n", length);
+  // reset idle timeout
+  MTA_lastActivity = millis();
+
+  if (length) {
+    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;
+      }
+    }
+
+    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;
+      }
+    }
+
+    // 3. we have a valid request and a valid response, call appropriate callback
+    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) {
+        LOCK_GUARD(errorCntLock, countAccessM);
+        errorCount++;
+      }
+
+      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);
+
+  // try to send whatever is waiting
+  handleSendingQueue();
+
+  // next check if timeout has struck for oldest request
+  if (!rxQueue.empty()) {
+    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
+      if (onError) {
+        // Handle timeout error
+        onError(TIMEOUT, request->token);
+      }
+      delete request;
+      rxQueue.erase(rxQueue.begin());
+    }
+  }
+    
+  }  // 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.
+  // Calling sites must assure shared resources are protected
+  // by mutex.
+
+  // try to send everything we have waiting
+  std::list<RequestEntry*>::iterator it = txQueue.begin();
+  while (it != txQueue.end()) {
+    // get the actual element
+    if (send(*it)) {
+      // after sending, update timeout value, add to other queue and remove from this queue
+      (*it)->sentTime = millis();
+      rxQueue[(*it)->head.transactionID] = (*it);      // push request to other queue
+      it = txQueue.erase(it);  // remove from toSend queue and point i to next request
+    } else {
+      // sending didn't succeed, try next request
+      ++it;
+    }
+  }
+}
+
+bool ModbusClientTCPasync::send(RequestEntry* re) {
+  // ATTENTION: This method does not have a lock guard.
+  // Calling sites must assure shared resources are protected
+  // by mutex.
+
+  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;
+}

lib/eModbus/src/ModbusClientTCPasync.h

@@ -0,0 +1,158 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_CLIENT_TCP_ASYNC_H
+#define _MODBUS_CLIENT_TCP_ASYNC_H
+#include <Arduino.h>
+#if defined ESP32
+#include <AsyncTCP.h>
+#elif defined ESP8266
+#include <ESPAsyncTCP.h>
+#endif
+#include "options.h"
+#include "ModbusMessage.h"
+#include "ModbusClient.h"
+#include <list>
+#include <map>
+#include <vector>
+#if USE_MUTEX
+#include <mutex>      // NOLINT
+#endif
+
+using std::vector;
+
+#define DEFAULTTIMEOUT 10000
+#define DEFAULTIDLETIME 60000
+
+class ModbusClientTCPasync : public ModbusClient {
+public:
+  // Constructor takes address and port
+  explicit ModbusClientTCPasync(IPAddress address, uint16_t port = 502, uint16_t queueLimit = 100);
+
+  // Destructor: clean up queue, task etc.
+  ~ModbusClientTCPasync();
+
+  // optionally manually connect to modbus server. Otherwise connection will be made upon first request
+  void connect();
+  // Connect to another Modbus server
+  void connect(IPAddress host, uint16_t port = 502);
+
+  // manually disconnect from modbus server. Connection will also auto close after idle time
+  void disconnect(bool force = false);
+
+  // Set timeout value
+  void setTimeout(uint32_t timeout);
+
+  // Set idle timeout value (time before connection auto closes after being idle)
+  void setIdleTimeout(uint32_t timeout);
+
+  // Set maximum amount of messages awaiting a response. Subsequent messages will be queued.
+  void setMaxInflightRequests(uint32_t maxInflightRequests);
+
+// Remove all pending request from queue
+  void clearQueue();
+
+protected:
+
+  // class describing the TCP header of Modbus packets
+  class ModbusTCPhead {
+  public:
+    ModbusTCPhead() :
+    transactionID(0),
+    protocolID(0),
+    len(0) {}
+
+    ModbusTCPhead(uint16_t tid, uint16_t pid, uint16_t _len) :
+    transactionID(tid),
+    protocolID(pid),
+    len(_len) {}
+
+    uint16_t transactionID;     // Caller-defined identification
+    uint16_t protocolID;        // const 0x0000
+    uint16_t len;               // Length of remainder of TCP packet
+
+    inline explicit operator const uint8_t *() {
+      uint8_t *cp = headRoom;
+      *cp++ = (transactionID >> 8) & 0xFF;
+      *cp++ = transactionID  & 0xFF;
+      *cp++ = (protocolID >> 8) & 0xFF;
+      *cp++ = protocolID  & 0xFF;
+      *cp++ = (len >> 8) & 0xFF;
+      *cp++ = len  & 0xFF;
+      return headRoom;
+    }
+
+    inline ModbusTCPhead& operator= (ModbusTCPhead& t) {
+      transactionID = t.transactionID;
+      protocolID    = t.protocolID;
+      len           = t.len;
+      return *this;
+    }
+
+  protected:
+    uint8_t headRoom[6];        // Buffer to hold MSB-first TCP header
+  };
+
+  struct RequestEntry {
+    uint32_t token;
+    ModbusMessage msg;
+    ModbusTCPhead head;
+    uint32_t sentTime;
+    bool isSyncRequest;
+    RequestEntry(uint32_t t, ModbusMessage m, bool syncReq = false) :
+      token(t),
+      msg(m),
+      head(ModbusTCPhead()),
+      sentTime(0),
+      isSyncRequest(syncReq) {}
+  };
+
+  // Base addRequest and syncRequest both must be present
+  Error addRequestM(ModbusMessage msg, uint32_t token);
+  ModbusMessage syncRequestM(ModbusMessage msg, uint32_t token);
+
+  // addToQueue: send freshly created request to queue
+  bool addToQueue(int32_t token, ModbusMessage request, bool syncReq = false);
+
+  // send: send request via Client connection
+  bool send(RequestEntry *request);
+
+  // receive: get response via Client connection
+  // TCPResponse* receive(uint8_t* data, size_t length);
+
+  void isInstance() { return; }     // make class instantiable
+
+  // TCP handling code, all static taking a class instancs as param
+  void onConnected();
+  void onDisconnected();
+  void onACError(AsyncClient* c, int8_t error);
+  // void onTimeout(uint32_t time);
+  // void onAck(size_t len, uint32_t time);
+  void onPacket(uint8_t* data, size_t length);
+  void onPoll();
+  void handleSendingQueue();
+
+  std::list<RequestEntry*> txQueue;           // Queue to hold requests to be sent
+  std::map<uint16_t, RequestEntry*> rxQueue;  // Queue to hold requests to be processed
+  #if USE_MUTEX
+  std::mutex sLock;                         // Mutex to protect state
+  std::mutex qLock;                         // Mutex to protect queues
+  #endif
+
+  AsyncClient MTA_client;           // Async TCP client
+  uint32_t MTA_timeout;             // Standard timeout value taken
+  uint32_t MTA_idleTimeout;         // Standard timeout value taken
+  uint16_t MTA_qLimit;              // Maximum number of requests to accept in queue
+  uint32_t MTA_maxInflightRequests; // Maximum number of inflight requests
+  uint32_t MTA_lastActivity;        // Last time there was activity (disabled when queues are not empty)
+  enum {
+    DISCONNECTED,
+    CONNECTING,
+    CONNECTED
+  } MTA_state;                      // TCP connection state
+  IPAddress MTA_host;
+  uint16_t MTA_port;
+};
+
+#endif

lib/eModbus/src/ModbusError.h

@@ -0,0 +1,138 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_ERROR_H
+#define _MODBUS_ERROR_H
+#include "ModbusTypeDefs.h"
+
+using namespace Modbus;  // NOLINT
+
+class ModbusError {
+public:
+  // Constructor with error code
+  inline explicit ModbusError(Error e) : err(e) {}
+  // Empty constructor defaults to 0
+  inline ModbusError() : err(SUCCESS) {}
+  // Assignment operators
+  inline ModbusError& operator=(const ModbusError& e) { err = e.err; return *this; }
+  inline ModbusError& operator=(const Error e) { err = e; return *this; }
+  // Copy constructor
+  inline ModbusError(const ModbusError& m) : err(m.err) {}
+  // Equality comparison
+  inline bool operator==(const ModbusError& m) { return (err == m.err); }
+  inline bool operator==(const Error e) { return (err == e); }
+  // Inequality comparison
+  inline bool operator!=(const ModbusError& m) { return (err != m.err); }
+  inline bool operator!=(const Error e) { return (err != e); }
+  inline explicit operator Error() { return err; }
+  inline operator int() { return static_cast<int>(err); }
+
+#ifndef MINIMAL
+  inline explicit operator const char *() { return getText(err); }
+#endif
+
+private:
+  Error err;          // The error code
+
+#ifndef MINIMAL
+// Return error as static text
+  inline static const char *getText(Error err) {
+    switch (err) {
+    case SUCCESS               : // 0x00,
+      return "Success";
+      break;
+    case ILLEGAL_FUNCTION      : // 0x01,
+      return "Illegal function code";
+      break;
+    case ILLEGAL_DATA_ADDRESS  : // 0x02,
+      return "Illegal data address";
+      break;
+    case ILLEGAL_DATA_VALUE    : // 0x03,
+      return "Illegal data value";
+      break;
+    case SERVER_DEVICE_FAILURE : // 0x04,
+      return "Server device failure";
+      break;
+    case ACKNOWLEDGE           : // 0x05,
+      return "Acknowledge";
+      break;
+    case SERVER_DEVICE_BUSY    : // 0x06,
+      return "Server device busy";
+      break;
+    case NEGATIVE_ACKNOWLEDGE  : // 0x07,
+      return "Negative acknowledge";
+      break;
+    case MEMORY_PARITY_ERROR   : // 0x08,
+      return "Memory parity error";
+      break;
+    case GATEWAY_PATH_UNAVAIL  : // 0x0A,
+      return "Gateway path unavailable";
+      break;
+    case GATEWAY_TARGET_NO_RESP: // 0x0B,
+      return "Gateway target not responding";
+      break;
+    case TIMEOUT               : // 0xE0,
+      return "Timeout";
+      break;
+    case INVALID_SERVER        : // 0xE1,
+      return "Invalid server";
+      break;
+    case CRC_ERROR             : // 0xE2, // only for Modbus-RTU
+      return "CRC check error";
+      break;
+    case FC_MISMATCH           : // 0xE3,
+      return "Function code mismatch";
+      break;
+    case SERVER_ID_MISMATCH    : // 0xE4,
+      return "Server ID mismatch";
+      break;
+    case PACKET_LENGTH_ERROR   : // 0xE5,
+      return "Packet length error";
+      break;
+    case PARAMETER_COUNT_ERROR : // 0xE6,
+      return "Wrong # of parameters";
+      break;
+    case PARAMETER_LIMIT_ERROR : // 0xE7,
+      return "Parameter out of bounds";
+      break;
+    case REQUEST_QUEUE_FULL    : // 0xE8,
+      return "Request queue full";
+      break;
+    case ILLEGAL_IP_OR_PORT    : // 0xE9,
+      return "Illegal IP or port";
+      break;
+    case IP_CONNECTION_FAILED  : // 0xEA,
+      return "IP connection failed";
+      break;
+    case TCP_HEAD_MISMATCH     : // 0xEB,
+      return "TCP header mismatch";
+      break;
+    case EMPTY_MESSAGE         : // 0xEC,
+      return "Incomplete request";
+      break;
+    case ASCII_FRAME_ERR       : // 0xED,
+      return "Invalid ASCII frame";
+      break;
+    case ASCII_CRC_ERR         : // 0xEE,
+      return "Invalid ASCII CRC";
+      break;
+    case ASCII_INVALID_CHAR    : // 0xEF,
+      return "Invalid ASCII character";
+      break;
+    case BROADCAST_ERROR       : // 0xF0,
+      return "Broadcast data invalid";
+      break;
+    case UNDEFINED_ERROR       : // 0xFF  // otherwise uncovered communication error
+    default:
+      return "Unspecified error";
+      break;
+    }
+    return "What?";
+  }
+
+#endif
+
+};
+
+#endif

lib/eModbus/src/ModbusMessage.cpp

@@ -0,0 +1,705 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusMessage.h"
+#undef LOCAL_LOG_LEVEL
+// #define LOCAL_LOG_LEVEL LOG_LEVEL_ERROR
+#include "Logging.h"
+
+// Default Constructor - takes optional size of MM_data to allocate memory
+ModbusMessage::ModbusMessage(uint16_t dataLen) {
+  if (dataLen) MM_data.reserve(dataLen);
+}
+
+// Special message Constructor - takes a std::vector<uint8_t>
+ModbusMessage::ModbusMessage(std::vector<uint8_t> s) :
+MM_data(s) { }
+
+// Destructor
+ModbusMessage::~ModbusMessage() { 
+  // If paranoid, one can use the below :D
+  // std::vector<uint8_t>().swap(MM_data);
+}
+
+// Assignment operator
+ModbusMessage& ModbusMessage::operator=(const ModbusMessage& m) {
+  // Do anything only if not self-assigning
+  if (this != &m) {
+    // Copy data from source to target
+    MM_data = m.MM_data;
+  }
+  return *this;
+}
+
+#ifndef NO_MOVE
+  // Move constructor
+ModbusMessage::ModbusMessage(ModbusMessage&& m) {
+  MM_data = std::move(m.MM_data);
+}
+  
+	// Move assignment
+ModbusMessage& ModbusMessage::operator=(ModbusMessage&& m) {
+  MM_data = std::move(m.MM_data);
+  return *this;
+}
+#endif
+
+// Copy constructor
+ModbusMessage::ModbusMessage(const ModbusMessage& m) :
+  MM_data(m.MM_data) { }
+
+// Equality comparison
+bool ModbusMessage::operator==(const ModbusMessage& m) {
+  // Prevent self-compare
+  if (this == &m) return true;
+  // If size is different, we assume inequality
+  if (MM_data.size() != m.MM_data.size()) return false;
+  // We will compare bytes manually - for uint8_t it should work out-of-the-box,
+  // but the data type might be changed later.
+  // If we find a difference byte, we found inequality
+  for (uint16_t i = 0; i < MM_data.size(); ++i) {
+    if (MM_data[i] != m.MM_data[i]) return false;
+  }
+  // Both tests passed ==> equality
+  return true;
+}
+
+// Inequality comparison
+bool ModbusMessage::operator!=(const ModbusMessage& m) {
+  return (!(*this == m));
+}
+
+// Conversion to bool
+ModbusMessage::operator bool() {
+  if (MM_data.size() >= 2) return true;
+  return false;
+}
+
+// Exposed methods of std::vector
+const uint8_t *ModbusMessage::data() { return MM_data.data(); }
+uint16_t       ModbusMessage::size() { return MM_data.size(); }
+void           ModbusMessage::push_back(const uint8_t& val) { MM_data.push_back(val); }
+void           ModbusMessage::clear() { MM_data.clear(); }
+// provide restricted operator[] interface
+uint8_t  ModbusMessage::operator[](uint16_t index) const {
+  if (index < MM_data.size()) {
+    return MM_data[index];
+  }
+  LOG_W("Index %d out of bounds (>=%d).\n", index, MM_data.size());
+  return 0;
+}
+// Resize internal MM_data
+uint16_t ModbusMessage::resize(uint16_t newSize) { 
+  MM_data.resize(newSize); 
+  return MM_data.size(); 
+}
+
+// Add append() for two ModbusMessages or a std::vector<uint8_t> to be appended
+void ModbusMessage::append(ModbusMessage& m) { 
+  MM_data.reserve(size() + m.size()); 
+  MM_data.insert(MM_data.end(), m.begin(), m.end()); 
+}
+
+void ModbusMessage::append(std::vector<uint8_t>& m) { 
+  MM_data.reserve(size() + m.size()); 
+  MM_data.insert(MM_data.end(), m.begin(), m.end()); 
+}
+
+uint8_t ModbusMessage::getServerID() const {
+  // Only if we have data and it is at least as long to fit serverID and function code, return serverID
+  if (MM_data.size() >= 2) { return MM_data[0]; }
+  // Else return 0 - normally the Broadcast serverID, but we will not support that. Full stop. :-D
+  return 0;
+}
+
+// Get MM_data[0] (server ID) and MM_data[1] (function code)
+uint8_t ModbusMessage::getFunctionCode() const {
+  // Only if we have data and it is at least as long to fit serverID and function code, return FC
+  if (MM_data.size() >= 2) { return MM_data[1]; }
+  // Else return 0 - which is no valid Modbus FC.
+  return 0;
+}
+
+// getError() - returns error code
+Error ModbusMessage::getError() const {
+  // Do we have data long enough?
+  if (MM_data.size() > 2) {
+    // Yes. Does it indicate an error?
+    if (MM_data[1] & 0x80)
+    {
+      // Yes. Get it.
+      return static_cast<Modbus::Error>(MM_data[2]);
+    }
+  }
+  // Default: everything OK - SUCCESS
+  return SUCCESS;
+}
+
+// Modbus data manipulation
+void    ModbusMessage::setServerID(uint8_t serverID) {
+  // We accept here that [0] may allocate a byte!
+  if (MM_data.empty()) {
+    MM_data.reserve(3);  // At least an error message should fit
+  }
+  MM_data[0] = serverID;
+}
+
+void    ModbusMessage::setFunctionCode(uint8_t FC) {
+  // We accept here that [0], [1] may allocate bytes!
+  if (MM_data.empty()) {
+    MM_data.reserve(3);  // At least an error message should fit
+  }
+  // No serverID set yet? use a 0 to initialize it to an error-generating value
+  if (MM_data.size() < 2) MM_data[0] = 0; // intentional invalid server ID!
+  MM_data[1] = FC;
+}
+
+// add() variant to copy a buffer into MM_data. Returns updated size
+uint16_t ModbusMessage::add(const uint8_t *arrayOfBytes, uint16_t count) {
+  // Copy it
+  while (count--) {
+    MM_data.push_back(*arrayOfBytes++);
+  }
+  // Return updated size (logical length of message so far)
+  return MM_data.size();
+}
+
+// determineFloatOrder: calculate the sequence of bytes in a float value
+uint8_t ModbusMessage::determineFloatOrder() {
+  constexpr uint8_t floatSize = sizeof(float);
+  // Only do it if not done yet
+  if (floatOrder[0] == 0xFF) {
+    // We need to calculate it.
+    // This will only work for 32bit floats, so check that
+    if (floatSize != 4) {
+      // OOPS! we cannot proceed.
+      LOG_E("Oops. float seems to be %d bytes wide instead of 4.\n", floatSize);
+      return 0;
+    }
+
+    uint32_t i = 77230;                             // int value to go into a float without rounding error
+    float f = i;                                    // assign it
+    uint8_t *b = (uint8_t *)&f;                     // Pointer to bytes of f
+    uint8_t expect[floatSize] = { 0x47, 0x96, 0xd7, 0x00 }; // IEEE754 representation 
+    uint8_t matches = 0;                            // number of bytes successfully matched
+     
+    // Loop over the bytes of the expected sequence
+    for (uint8_t inx = 0; inx < floatSize; ++inx) {
+      // Loop over the real bytes of f
+      for (uint8_t trg = 0; trg < floatSize; ++trg) {
+        if (expect[inx] == b[trg]) {
+          floatOrder[inx] = trg;
+          matches++;
+          break;
+        }
+      }
+    }
+
+    // All bytes found?
+    if (matches != floatSize) {
+      // No! There is something fishy...
+      LOG_E("Unable to determine float byte order (matched=%d of %d)\n", matches, floatSize);
+      floatOrder[0] = 0xFF;
+      return 0;
+    } else {
+      HEXDUMP_V("floatOrder", floatOrder, floatSize);
+    }
+  }
+  return floatSize;
+}
+
+// determineDoubleOrder: calculate the sequence of bytes in a double value
+uint8_t ModbusMessage::determineDoubleOrder() {
+  constexpr uint8_t doubleSize = sizeof(double);
+  // Only do it if not done yet
+  if (doubleOrder[0] == 0xFF) {
+    // We need to calculate it.
+    // This will only work for 64bit doubles, so check that
+    if (doubleSize != 8) {
+      // OOPS! we cannot proceed.
+      LOG_E("Oops. double seems to be %d bytes wide instead of 8.\n", doubleSize);
+      return 0;
+    }
+
+    uint64_t i = 5791007487489389;                  // int64 value to go into a double without rounding error
+    double f = i;                                   // assign it
+    uint8_t *b = (uint8_t *)&f;                     // Pointer to bytes of f
+    uint8_t expect[doubleSize] = { 0x43, 0x34, 0x92, 0xE4, 0x00, 0x2E, 0xF5, 0x6D }; // IEEE754 representation 
+    uint8_t matches = 0;                            // number of bytes successfully matched
+     
+    // Loop over the bytes of the expected sequence
+    for (uint8_t inx = 0; inx < doubleSize; ++inx) {
+      // Loop over the real bytes of f
+      for (uint8_t trg = 0; trg < doubleSize; ++trg) {
+        if (expect[inx] == b[trg]) {
+          doubleOrder[inx] = trg;
+          matches++;
+          break;
+        }
+      }
+    }
+
+    // All bytes found?
+    if (matches != doubleSize) {
+      // No! There is something fishy...
+      LOG_E("Unable to determine double byte order (matched=%d of %d)\n", matches, doubleSize);
+      doubleOrder[0] = 0xFF;
+      return 0;
+    } else {
+      HEXDUMP_V("doubleOrder", doubleOrder, doubleSize);
+    }
+  }
+  return doubleSize;
+}
+
+// swapFloat() and swapDouble() will re-order the bytes of a float or double value
+// according a user-given pattern
+float ModbusMessage::swapFloat(float& f, int swapRule) {
+  LOG_V("swap float, swapRule=%02X\n", swapRule);
+  // Make a byte pointer to the given float
+  uint8_t *src = (uint8_t *)&f;
+  // Define a "work bench" float and byte pointer to it
+  float interim;
+  uint8_t *dst = (uint8_t *)&interim;
+  // Loop over all bytes of a float
+  for (uint8_t i = 0; i < sizeof(float); ++i) {
+    // Get i-th byte from the spot the swap table tells
+    // (only the first 4 tables are valid for floats)
+    LOG_V("dst[%d] = src[%d]\n", i, swapTables[swapRule & 0x03][i]);
+    dst[i] = src[swapTables[swapRule & 0x03][i]];
+    // Does the swar rule require nibble swaps?
+    if (swapRule & 0x08) {
+      // Yes, it does. 
+      uint8_t nib = ((dst[i] & 0x0f) << 4) | ((dst[i] >> 4) & 0x0F);
+      dst[i] = nib;
+    }
+  }
+  // Save and return result
+  f = interim;
+  return interim;
+}
+
+double ModbusMessage::swapDouble(double& f, int swapRule) {
+  LOG_V("swap double, swapRule=%02X\n", swapRule);
+  // Make a byte pointer to the given double
+  uint8_t *src = (uint8_t *)&f;
+  // Define a "work bench" double and byte pointer to it
+  double interim;
+  uint8_t *dst = (uint8_t *)&interim;
+  // Loop over all bytes of a double
+  for (uint8_t i = 0; i < sizeof(double); ++i) {
+    // Get i-th byte from the spot the swap table tells
+    LOG_V("dst[%d] = src[%d]\n", i, swapTables[swapRule & 0x07][i]);
+    dst[i] = src[swapTables[swapRule & 0x07][i]];
+    // Does the swar rule require nibble swaps?
+    if (swapRule & 0x08) {
+      // Yes, it does. 
+      uint8_t nib = ((dst[i] & 0x0f) << 4) | ((dst[i] >> 4) & 0x0F);
+      dst[i] = nib;
+    }
+  }
+  // Save and return result
+  f = interim;
+  return interim;
+}
+
+// add() variant for a vector of uint8_t
+uint16_t ModbusMessage::add(vector<uint8_t> v) {
+  for (auto& b: v) {
+    MM_data.push_back(b);
+  }
+  return MM_data.size();
+}
+
+// add() variants for float and double values
+// values will be added in IEEE754 byte sequence (MSB first)
+uint16_t ModbusMessage::add(float v, int swapRule) {
+  // First check if we need to determine byte order
+  LOG_V("add float, swapRule=%02X\n", swapRule);
+  HEXDUMP_V("float", (uint8_t *)&v, sizeof(float));
+  if (determineFloatOrder()) {
+    // If we get here, the floatOrder is known
+    float interim = 0;
+    uint8_t *dst = (uint8_t *)&interim;
+    uint8_t *src = (uint8_t *)&v;
+    // Put out the bytes of v in normalized sequence
+    for (uint8_t i = 0; i < sizeof(float); ++i) {
+      dst[i] = src[floatOrder[i]];
+    }
+    HEXDUMP_V("normalized float", (uint8_t *)&interim, sizeof(float));
+    // Do we need to apply a swap rule?
+    if (swapRule & 0x0B) {
+      // Yes, so do it.
+      swapFloat(interim, swapRule & 0x0B);
+    }
+    HEXDUMP_V("swapped float", (uint8_t *)&interim, sizeof(float));
+    // Put out the bytes of v in normalized (and swapped) sequence
+    for (uint8_t i = 0; i < sizeof(float); ++i) {
+      MM_data.push_back(dst[i]);
+    }
+  }
+
+  return MM_data.size();
+}
+
+uint16_t ModbusMessage::add(double v, int swapRule) {
+  // First check if we need to determine byte order
+  LOG_V("add double, swapRule=%02X\n", swapRule);
+  HEXDUMP_V("double", (uint8_t *)&v, sizeof(double));
+  if (determineDoubleOrder()) {
+    // If we get here, the doubleOrder is known
+    double interim = 0;
+    uint8_t *dst = (uint8_t *)&interim;
+    uint8_t *src = (uint8_t *)&v;
+    // Put out the bytes of v in normalized sequence
+    for (uint8_t i = 0; i < sizeof(double); ++i) {
+      dst[i] = src[doubleOrder[i]];
+    }
+    HEXDUMP_V("normalized double", (uint8_t *)&interim, sizeof(double));
+    // Do we need to apply a swap rule?
+    if (swapRule & 0x0F) {
+      // Yes, so do it.
+      swapDouble(interim, swapRule & 0x0F);
+    }
+    HEXDUMP_V("swapped double", (uint8_t *)&interim, sizeof(double));
+    // Put out the bytes of v in normalized (and swapped) sequence
+    for (uint8_t i = 0; i < sizeof(double); ++i) {
+      MM_data.push_back(dst[i]);
+    }
+  }
+
+  return MM_data.size();
+}
+
+// get() variants for float and double values
+// values will be read in IEEE754 byte sequence (MSB first)
+uint16_t ModbusMessage::get(uint16_t index, float& v, int swapRule) const {
+  // First check if we need to determine byte order
+  if (determineFloatOrder()) {
+    // If we get here, the floatOrder is known
+    // Will it fit?
+    if (index <= MM_data.size() - sizeof(float)) {
+      // Yes. Get the bytes of v in normalized sequence
+      uint8_t *bytes = (uint8_t *)&v;
+      for (uint8_t i = 0; i < sizeof(float); ++i) {
+        bytes[i] = MM_data[index + floatOrder[i]];
+      }
+      HEXDUMP_V("got float", (uint8_t *)&v, sizeof(float));
+      // Do we need to apply a swap rule?
+      if (swapRule & 0x0B) {
+        // Yes, so do it.
+        swapFloat(v, swapRule & 0x0B);
+      }
+      HEXDUMP_V("got float swapped", (uint8_t *)&v, sizeof(float));
+      index += sizeof(float);
+    }
+  }
+
+  return index;
+}
+
+uint16_t ModbusMessage::get(uint16_t index, double& v, int swapRule) const {
+  // First check if we need to determine byte order
+  if (determineDoubleOrder()) {
+    // If we get here, the doubleOrder is known
+    // Will it fit?
+    if (index <= MM_data.size() - sizeof(double)) {
+      // Yes. Get the bytes of v in normalized sequence
+      uint8_t *bytes = (uint8_t *)&v;
+      for (uint8_t i = 0; i < sizeof(double); ++i) {
+        bytes[i] = MM_data[index + doubleOrder[i]];
+      }
+      HEXDUMP_V("got double", (uint8_t *)&v, sizeof(double));
+      // Do we need to apply a swap rule?
+      if (swapRule & 0x0F) {
+        // Yes, so do it.
+        swapDouble(v, swapRule & 0x0F);
+      }
+      HEXDUMP_V("got double swapped", (uint8_t *)&v, sizeof(double));
+      index += sizeof(double);
+    }
+  }
+
+  return index;
+}
+
+// get() - read a byte array of a given size into a vector<uint8_t>. Returns updated index
+uint16_t ModbusMessage::get(uint16_t index, vector<uint8_t>& v, uint8_t count) const {
+  // Clean target vector
+  v.clear();
+  // Loop until required count is complete or the source is exhausted
+  while (index < MM_data.size() && count--) {
+    v.push_back(MM_data[index++]);
+  }
+  return index;
+}
+
+// Data validation methods for the different factory calls
+// 0. serverID and function code - used by all of the below
+Error ModbusMessage::checkServerFC(uint8_t serverID, uint8_t functionCode) {
+  if (serverID == 0)      return INVALID_SERVER;   // Broadcast - not supported here
+  if (serverID > 247)     return INVALID_SERVER;   // Reserved server addresses
+  if (FCT::getType(functionCode) == FCILLEGAL)  return ILLEGAL_FUNCTION; // FC 0 does not exist
+  return SUCCESS;
+}
+
+// 1. no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode) {
+  LOG_V("Check data #1\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FC07_TYPE && ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    }
+  }
+  return returnCode;
+}
+
+// 2. one uint16_t parameter (FC 0x18)
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1) {
+  LOG_V("Check data #2\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FC18_TYPE && ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    }
+  }
+  return returnCode;
+}
+
+// 3. two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2) {
+  LOG_V("Check data #3\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FC01_TYPE && ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    } else {
+      switch (functionCode) {
+      case 0x01:
+      case 0x02:
+        if ((p2 > 0x7d0) || (p2 == 0)) returnCode = PARAMETER_LIMIT_ERROR;
+        break;
+      case 0x03:
+      case 0x04:
+        if ((p2 > 0x7d) || (p2 == 0)) returnCode = PARAMETER_LIMIT_ERROR;
+        break;
+      case 0x05:
+        if ((p2 != 0) && (p2 != 0xff00)) returnCode = PARAMETER_LIMIT_ERROR;
+        break;
+      }
+    }
+  }
+  return returnCode;
+}
+
+// 4. three uint16_t parameters (FC 0x16)
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint16_t p3) {
+  LOG_V("Check data #4\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FC16_TYPE && ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    } 
+  }
+  return returnCode;
+}
+
+// 5. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of words (FC 0x10)
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint16_t *arrayOfWords) {
+  LOG_V("Check data #5\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FC10_TYPE && ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    } else {
+      if ((p2 == 0) || (p2 > 0x7b)) returnCode = PARAMETER_LIMIT_ERROR;
+      else if (count != (p2 * 2)) returnCode = ILLEGAL_DATA_VALUE;
+    }
+  }
+  return returnCode;
+}
+
+// 6. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of bytes (FC 0x0f)
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint8_t *arrayOfBytes) {
+  LOG_V("Check data #6\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FC0F_TYPE && ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    } else {
+      if ((p2 == 0) || (p2 > 0x7b0)) returnCode = PARAMETER_LIMIT_ERROR;
+      else if (count != ((p2 / 8 + (p2 % 8 ? 1 : 0)))) returnCode = ILLEGAL_DATA_VALUE;
+    }
+  }
+  return returnCode;
+}
+
+// 7. generic constructor for preformatted data ==> count is counting bytes!
+Error ModbusMessage::checkData(uint8_t serverID, uint8_t functionCode, uint16_t count, uint8_t *arrayOfBytes) {
+  LOG_V("Check data #7\n");
+  Error returnCode = checkServerFC(serverID, functionCode);
+  if (returnCode == SUCCESS)
+  {
+    FCType ft = FCT::getType(functionCode);
+    if (ft != FCUSER && ft != FCGENERIC) {
+      returnCode = PARAMETER_COUNT_ERROR;
+    } 
+  }
+  return returnCode;
+}
+
+// Factory methods to create valid Modbus messages from the parameters
+// 1. no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(2);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode);
+  }
+  return returnCode;
+}
+
+// 2. one uint16_t parameter (FC 0x18)
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode, p1);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(4);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode, p1);
+  }
+  return returnCode;
+}
+
+// 3. two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode, p1, p2);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(6);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode, p1, p2);
+  }
+  return returnCode;
+}
+
+// 4. three uint16_t parameters (FC 0x16)
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint16_t p3) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode, p1, p2, p3);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(8);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode, p1, p2, p3);
+  }
+  return returnCode;
+}
+
+// 5. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of words (FC 0x10)
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint16_t *arrayOfWords) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode, p1, p2, count, arrayOfWords);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(7 + count * 2);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode, p1, p2);
+    add(count);
+    for (uint8_t i = 0; i < (count >> 1); ++i) {
+      add(arrayOfWords[i]);
+    }
+  }
+  return returnCode;
+}
+
+// 6. two uint16_t parameters, a uint8_t length byte and a uint8_t* pointer to array of bytes (FC 0x0f)
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint8_t *arrayOfBytes) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode, p1, p2, count, arrayOfBytes);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(7 + count);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode, p1, p2);
+    add(count);
+    for (uint8_t i = 0; i < count; ++i) {
+      add(arrayOfBytes[i]);
+    }
+  }
+  return returnCode;
+}
+
+// 7. generic constructor for preformatted data ==> count is counting bytes!
+Error ModbusMessage::setMessage(uint8_t serverID, uint8_t functionCode, uint16_t count, uint8_t *arrayOfBytes) {
+  // Check parameter for validity
+  Error returnCode = checkData(serverID, functionCode, count, arrayOfBytes);
+  // No error? 
+  if (returnCode == SUCCESS)
+  {
+    // Yes, all fine. Create new ModbusMessage
+    MM_data.reserve(2 + count);
+    MM_data.shrink_to_fit();
+    MM_data.clear();
+    add(serverID, functionCode);
+    for (uint8_t i = 0; i < count; ++i) {
+      add(arrayOfBytes[i]);
+    }
+  }
+  return returnCode;
+}
+
+// 8. Error response generator
+Error ModbusMessage::setError(uint8_t serverID, uint8_t functionCode, Error errorCode) {
+  // No error checking for server ID or function code here, as both may be the cause for the message!? 
+  MM_data.reserve(3);
+  MM_data.shrink_to_fit();
+  MM_data.clear();
+  add(serverID, static_cast<uint8_t>((functionCode | 0x80) & 0xFF), static_cast<uint8_t>(errorCode));
+  return SUCCESS;
+}
+
+// Error output in case a message constructor will fail
+void ModbusMessage::printError(const char *file, int lineNo, Error e, uint8_t serverID, uint8_t functionCode) {
+  LOG_E("(%s, line %d) Error in constructor: %02X - %s (%02X/%02X)\n", file_name(file), lineNo, e, (const char *)(ModbusError(e)), serverID, functionCode);
+}
+
+uint8_t ModbusMessage::floatOrder[] = { 0xFF };
+uint8_t ModbusMessage::doubleOrder[] = { 0xFF };

lib/eModbus/src/ModbusMessage.h

@@ -0,0 +1,216 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_MESSAGE_H
+#define _MODBUS_MESSAGE_H
+#include "ModbusTypeDefs.h"
+#include "ModbusError.h"
+#include <type_traits>
+#include <vector>
+
+using Modbus::Error;
+using Modbus::FCType;
+using Modbus::FCT;
+using std::vector;
+
+class ModbusMessage {
+public:
+  // Default empty message Constructor - optionally takes expected size of MM_data
+  explicit ModbusMessage(uint16_t dataLen = 0);
+
+  // Special message Constructor - takes a std::vector<uint8_t>
+  explicit ModbusMessage(std::vector<uint8_t> s);
+
+  // Message constructors - internally setMessage() is called
+  // WARNING: if parameters are invalid, message will _NOT_ be set up!
+  template <typename... Args>
+  ModbusMessage(uint8_t serverID, uint8_t functionCode, Args&&... args) { // NOLINT
+    Error e = SUCCESS;
+    if ((e = setMessage(serverID, functionCode, std::forward<Args>(args) ...)) != SUCCESS) {
+      printError(__FILE__, __LINE__, e, serverID, functionCode);
+    }
+  }
+
+  // Destructor
+  ~ModbusMessage();
+
+  // Assignment operator
+  ModbusMessage& operator=(const ModbusMessage& m);
+  
+  // Copy constructor
+  ModbusMessage(const ModbusMessage& m);
+
+#ifndef NO_MOVE
+  // Move constructor
+	ModbusMessage(ModbusMessage&& m);
+  
+	// Move assignment
+	ModbusMessage& operator=(ModbusMessage&& m);
+#endif
+
+  // Comparison operators
+  bool operator==(const ModbusMessage& m);
+  bool operator!=(const ModbusMessage& m);
+  operator bool();
+  
+  // Exposed methods of std::vector
+  const uint8_t   *data();  // address of MM_data
+  uint16_t   size();  // used length in MM_data
+  uint8_t    operator[](uint16_t index) const; // provide restricted operator[] interface
+  void push_back(const uint8_t& val); // add a byte at the end of MM_data
+  void clear();             // delete message contents
+  uint16_t resize(uint16_t newSize);  // resize MM_data
+
+  // provide iterator interface on MM_data
+  typedef std::vector<uint8_t>::const_iterator const_iterator;
+  const_iterator begin() const { return MM_data.begin(); }
+  const_iterator end() const   { return MM_data.end(); }
+
+  // Add append() for two ModbusMessages or a std::vector<uint8_t> to be appended
+  void append(ModbusMessage& m);
+  void append(std::vector<uint8_t>& m);
+
+  // Modbus data extraction
+  uint8_t getServerID() const;      // returns Server ID or 0 if MM_data is shorter than 3
+  uint8_t getFunctionCode() const;  // returns FC or 0 if MM_data is shorter than 3
+  Error   getError() const;         // getError() - returns error code (MM_data[2], if MM_data[1] > 0x7F, else SUCCESS)
+
+  // Modbus data manipulation
+  void    setServerID(uint8_t serverID); // Change server ID
+  void    setFunctionCode(uint8_t FC);   // Change function code
+
+  // add() variant to copy a buffer into MM_data. Returns updated size
+  uint16_t add(const uint8_t *arrayOfBytes, uint16_t count);
+
+  // add() - add a single data element MSB first to MM_data. Returns updated size
+  template <class T> uint16_t add(T v) {
+    uint16_t sz = sizeof(T);    // Size of value to be added
+
+    // Copy it MSB first
+    while (sz) {
+      sz--;
+      MM_data.push_back((v >> (sz << 3)) & 0xFF);
+    }
+    // Return updated size (logical length of message so far)
+    return MM_data.size();
+  }
+
+  // Template function to extend add(A) to add(A, B, C, ...)
+  template <class T, class... Args> 
+  typename std::enable_if<!std::is_pointer<T>::value, uint16_t>::type
+  add(T v, Args... args) {
+      add(v);
+      return add(args...);
+  }
+
+// get() - read a byte array of a given size into a vector<uint8_t>. Returns updated index
+uint16_t get(uint16_t index, vector<uint8_t>& v, uint8_t count) const;
+
+// get() - recursion stopper for template function below
+inline uint16_t get(uint16_t index) const { return index; }
+
+// Template function to extend getOne(index, A&) to get(index, A&, B&, C&, ...)
+template <class T, class... Args>
+typename std::enable_if<!std::is_pointer<T>::value, uint16_t>::type
+get(uint16_t index, T& v, Args&... args) const {
+  uint16_t pos = getOne(index, v);
+  return get(pos, args...);
+}
+
+// add() variant for vectors of uint8_t
+uint16_t add(vector<uint8_t> v);
+
+// add() variants for float and double values
+uint16_t add(float v, int swapRules = 0);
+uint16_t add(double v, int swapRules = 0);
+
+// get() variants for float and double values
+uint16_t get(uint16_t index, float& v, int swapRules = 0) const;
+uint16_t get(uint16_t index, double& v, int swapRules = 0) const;
+
+  // Message generation methods
+  // 1. no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
+  Error setMessage(uint8_t serverID, uint8_t functionCode);
+
+  // 2. one uint16_t parameter (FC 0x18)
+  Error setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1);
+  
+  // 3. two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
+  Error setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2);
+  
+  // 4. three uint16_t parameters (FC 0x16)
+  Error setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint16_t p3);
+  
+  // 5. two uint16_t parameters, a uint8_t length byte and a uint8_t* pointer to array of words (FC 0x10)
+  Error setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint16_t *arrayOfWords);
+  
+  // 6. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of bytes (FC 0x0f)
+  Error setMessage(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint8_t *arrayOfBytes);
+
+  // 7. generic constructor for preformatted data ==> count is counting bytes!
+  Error setMessage(uint8_t serverID, uint8_t functionCode, uint16_t count, uint8_t *arrayOfBytes);
+
+  // 8. error response
+  Error setError(uint8_t serverID, uint8_t functionCode, Error errorCode);
+  
+protected:
+  // Data validation methods - used by the above!
+  // 0. serverID and function code - used by all of the below
+  static Error checkServerFC(uint8_t serverID, uint8_t functionCode);
+
+  // 1. no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
+  static Error checkData(uint8_t serverID, uint8_t functionCode);
+  
+  // 2. one uint16_t parameter (FC 0x18)
+  static Error checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1);
+  
+  // 3. two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
+  static Error checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2);
+  
+  // 4. three uint16_t parameters (FC 0x16)
+  static Error checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint16_t p3);
+  
+  // 5. two uint16_t parameters, a uint8_t length byte and a uint8_t* pointer to array of words (FC 0x10)
+  static Error checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint16_t *arrayOfWords);
+  
+  // 6. two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of bytes (FC 0x0f)
+  static Error checkData(uint8_t serverID, uint8_t functionCode, uint16_t p1, uint16_t p2, uint8_t count, uint8_t *arrayOfBytes);
+
+  // 7. generic constructor for preformatted data ==> count is counting bytes!
+  static Error checkData(uint8_t serverID, uint8_t functionCode, uint16_t count, uint8_t *arrayOfBytes);
+
+  // Error output in case a message constructor will fail
+  static void printError(const char *file, int lineNo, Error e, uint8_t serverID, uint8_t functionCode);
+
+  std::vector<uint8_t> MM_data;  // Message data buffer
+
+  static uint8_t floatOrder[sizeof(float)]; // order of bytes in a float variable
+  static uint8_t doubleOrder[sizeof(double)]; // order of bytes in a double variable
+
+  static uint8_t determineFloatOrder();
+  static uint8_t determineDoubleOrder();
+
+  static float swapFloat(float& f, int swapRule);
+  static double swapDouble(double& f, int swapRule);
+
+  // getOne() - read a MSB-first value starting at byte index. Returns updated index
+  template <typename T> uint16_t getOne(uint16_t index, T& retval) const {
+    uint16_t sz = sizeof(retval);    // Size of value to be read
+
+    retval = 0;                      // return value
+
+    // Will it fit?
+    if (index <= MM_data.size() - sz) {
+      // Yes. Copy it MSB first
+      while (sz) {
+        sz--;
+        retval <<= 8;
+        retval |= MM_data[index++];
+      }
+    }
+    return index;
+  }
+};
+
+#endif

lib/eModbus/src/ModbusServer.cpp

@@ -0,0 +1,175 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include <Arduino.h>
+#include "ModbusServer.h"
+
+#undef LOCAL_LOG_LEVEL
+// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+#include "Logging.h"
+
+// registerWorker: register a worker function for a certain serverID/FC combination
+// If there is one already, it will be overwritten!
+void ModbusServer::registerWorker(uint8_t serverID, uint8_t functionCode, MBSworker worker) {
+  workerMap[serverID][functionCode] = worker;
+  LOG_D("Registered worker for %02X/%02X\n", serverID, functionCode);
+}
+
+// getWorker: if a worker function is registered, return its address, nullptr otherwise
+MBSworker ModbusServer::getWorker(uint8_t serverID, uint8_t functionCode) {
+  // Search the FC map associated with the serverID
+  auto svmap = workerMap.find(serverID);
+  // Is there one?
+  if (svmap != workerMap.end()) {
+    // Yes. Now look for the function code in the inner map
+    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);
+      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);
+        return fcmap->second;
+      }
+    }
+  }
+  // No matching function pointer found
+  LOG_D("No matching worker found\n");
+  return nullptr;
+}
+
+// unregisterWorker; remove again all or part of the registered workers for a given server ID
+// Returns true if the worker was found and removed
+bool ModbusServer::unregisterWorker(uint8_t serverID, uint8_t functionCode) {
+  uint16_t numEntries = 0;    // Number of entries removed
+
+  // Is there at least one entry for the serverID?
+  auto svmap = workerMap.find(serverID);
+  // Is there one?
+  if (svmap != workerMap.end()) {
+    // Yes. we may proceed with it
+    // Are we to look for a single serverID/FC combination?
+    if (functionCode) {
+      // Yes. 
+      numEntries = svmap->second.erase(functionCode);
+    } else {
+      // No, the serverID shall be removed with all references
+      numEntries = workerMap.erase(serverID);
+    }
+  } 
+  LOG_D("Removed %d worker entries for %d/%d\n", numEntries, serverID, functionCode);
+  return (numEntries ? true : false);
+}
+
+// isServerFor: if any worker function is registered for the given serverID, return true
+bool ModbusServer::isServerFor(uint8_t serverID) {
+  // Search the FC map for the serverID
+  auto svmap = workerMap.find(serverID);
+  // Is it there? Then return true
+  if (svmap != workerMap.end()) return true;
+  // No, serverID was not found. Return false
+  return false;
+}
+
+// getMessageCount: read number of messages processed
+uint32_t ModbusServer::getMessageCount() { 
+  return messageCount;
+}
+
+// getErrorCount: read number of errors responded
+uint32_t ModbusServer::getErrorCount() { 
+  return errorCount;
+}
+
+// resetCounts: set both message and error counts to zero
+void ModbusServer::resetCounts() {
+  {
+    LOCK_GUARD(cntLock, m);
+    messageCount = 0;
+    errorCount = 0;
+  }
+}
+
+// LocalRequest: get response from locally running server.
+ModbusMessage ModbusServer::localRequest(ModbusMessage msg) {
+  ModbusMessage m;
+  uint8_t serverID = msg.getServerID();
+  uint8_t functionCode = msg.getFunctionCode();
+  LOG_D("Local request for %02X/%02X\n", serverID, functionCode);
+  HEXDUMP_V("Request", msg.data(), msg.size());
+  messageCount++;
+  // Try to get a worker for the request
+  MBSworker worker = getWorker(serverID, functionCode);
+  // Did we get one?
+  if (worker != nullptr) {
+    // Yes. call it and return the response
+    LOG_D("Call worker\n");
+    m = worker(msg);
+    LOG_D("Worker responded\n");
+    HEXDUMP_V("Worker response", m.data(), m.size());
+    // Process Response. Is it one of the predefined types?
+    if (m[0] == 0xFF && (m[1] == 0xF0 || m[1] == 0xF1)) {
+      // Yes. Check it
+      switch (m[1]) {
+      case 0xF0: // NIL
+        m.clear();
+        break;
+      case 0xF1: // ECHO
+        m.clear();
+        m.append(msg);
+        break;
+      default:   // Will not get here, but lint likes it!
+        break;
+      }
+    }
+    HEXDUMP_V("Response", m.data(), m.size());
+    if (m.getError() != SUCCESS) {
+      errorCount++;
+    }
+    return m;
+  } else {
+    LOG_D("No worker found. Error response.\n");
+    // No. Is there at least one worker for the serverID?
+    if (isServerFor(serverID)) {
+      // Yes. Respond with "illegal function code"
+      m.setError(serverID, functionCode, ILLEGAL_FUNCTION);
+    } else {
+      // No. Respond with "Invalid server ID"
+      m.setError(serverID, functionCode, INVALID_SERVER);
+    }
+    errorCount++;
+    return m;
+  }
+  // We should never get here...
+  LOG_C("Internal problem: should not get here!\n");
+  m.setError(serverID, functionCode, UNDEFINED_ERROR);
+  errorCount++;
+  return m;
+}
+
+// Constructor
+ModbusServer::ModbusServer() :
+  messageCount(0),
+  errorCount(0) { }
+
+// Destructor
+ModbusServer::~ModbusServer() {
+}
+
+// listServer: Print out all mapped server/FC combinations
+void ModbusServer::listServer() {
+  for (auto it = workerMap.begin(); it != workerMap.end(); ++it) {
+    LOG_N("Server %3d: ", it->first);
+    for (auto it2 = it->second.begin(); it2 != it->second.end(); it2++) {
+      LOGRAW_N(" %02X", it2->first);
+    }
+    LOGRAW_N("\n");
+  }
+}

lib/eModbus/src/ModbusServer.h

@@ -0,0 +1,86 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_SERVER_H
+#define _MODBUS_SERVER_H
+
+#include "options.h"
+
+#include <map>
+#include <vector>
+#include <functional>
+#if USE_MUTEX
+#include <mutex>      // NOLINT
+#endif
+#include "ModbusTypeDefs.h"
+#include "ModbusError.h"
+#include "ModbusMessage.h"
+
+#if USE_MUTEX
+using std::mutex;
+using std::lock_guard;
+#endif
+
+// Standard response variants for "no response" and "echo the request"
+const ModbusMessage NIL_RESPONSE (std::vector<uint8_t>{0xFF, 0xF0});
+const ModbusMessage ECHO_RESPONSE(std::vector<uint8_t>{0xFF, 0xF1});
+
+// MBSworker: function signature for worker functions to handle single serverID/functionCode combinations
+using MBSworker = std::function<ModbusMessage(ModbusMessage msg)>;
+
+class ModbusServer {
+public:
+  // registerWorker: register a worker function for a certain serverID/FC combination
+  // If there is one already, it will be overwritten!
+  void registerWorker(uint8_t serverID, uint8_t functionCode, MBSworker worker);
+  
+  // getWorker: if a worker function is registered, return its address, nullptr otherwise
+  MBSworker getWorker(uint8_t serverID, uint8_t functionCode);
+
+  // unregisterWorker; remove again all or part of the registered workers for a given server ID
+  // 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
+  bool isServerFor(uint8_t serverID);
+
+  // getMessageCount: read number of messages processed
+  uint32_t getMessageCount();
+
+  // getErrorCount: read number of errors responded
+  uint32_t getErrorCount();
+
+  // resetCounts: set both message and error counts to zero
+  void resetCounts();
+
+  // Local request to the server
+  ModbusMessage localRequest(ModbusMessage msg);
+
+  // listServer: print out all server/FC combinations served
+  void listServer();
+
+protected:
+  // Constructor
+  ModbusServer();
+
+  // Destructor
+  ~ModbusServer();
+
+  // Prevent copy construction or assignment
+  ModbusServer(ModbusServer& other) = delete;
+  ModbusServer& operator=(ModbusServer& other) = delete;
+
+  // Virtual function to prevent this class being instantiated
+  virtual void isInstance() = 0;
+
+  std::map<uint8_t, std::map<uint8_t, MBSworker>> workerMap;      // map on serverID->functionCode->worker function
+  uint32_t messageCount;         // Number of Requests processed
+  uint32_t errorCount;           // Number of errors responded
+  #if USE_MUTEX
+  mutex m;                       // mutex to cover changes to messageCount and errorCount
+  #endif
+};
+
+
+#endif

lib/eModbus/src/ModbusServerEthernet.h

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

lib/eModbus/src/ModbusServerRTU.cpp

@@ -0,0 +1,260 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusServerRTU.h"
+
+#if HAS_FREERTOS
+
+#undef LOG_LEVEL_LOCAL
+#include "Logging.h"
+
+// Init number of created ModbusServerRTU objects
+uint8_t ModbusServerRTU::instanceCounter = 0;
+
+// Constructor with RTS pin GPIO (or -1)
+ModbusServerRTU::ModbusServerRTU(uint32_t timeout, int rtsPin) :
+  ModbusServer(),
+  serverTask(nullptr),
+  serverTimeout(timeout),
+  MSRserial(nullptr),
+  MSRinterval(2000),     // will be calculated in begin()!
+  MSRlastMicros(0),
+  MSRrtsPin(rtsPin), 
+  MSRuseASCII(false),
+  MSRskipLeadingZeroByte(false),
+  listener(nullptr),
+  sniffer(nullptr) {
+  // Count instances one up
+  instanceCounter++;
+  // If we have a GPIO RE/DE pin, configure it.
+  if (MSRrtsPin >= 0) {
+    pinMode(MSRrtsPin, OUTPUT);
+    MRTSrts = [this](bool level) {
+      digitalWrite(MSRrtsPin, level);
+    };
+    MRTSrts(LOW);
+  } else {
+    MRTSrts = RTUutils::RTSauto;
+  }
+}
+
+// Constructor with RTS callback
+ModbusServerRTU::ModbusServerRTU(uint32_t timeout, RTScallback rts) :
+  ModbusServer(),
+  serverTask(nullptr),
+  serverTimeout(timeout),
+  MSRserial(nullptr),
+  MSRinterval(2000),     // will be calculated in begin()!
+  MSRlastMicros(0),
+  MRTSrts(rts), 
+  MSRuseASCII(false),
+  MSRskipLeadingZeroByte(false),
+  listener(nullptr),
+  sniffer(nullptr) {
+  // Count instances one up
+  instanceCounter++;
+  // Configure RTS callback
+  MSRrtsPin = -1;
+  MRTSrts(LOW);
+}
+
+// Destructor
+ModbusServerRTU::~ModbusServerRTU() {
+}
+
+// start: create task with RTU server - general version
+void ModbusServerRTU::begin(Stream& serial, uint32_t baudRate, int coreID) {
+  MSRserial = &serial;
+  doBegin(baudRate, coreID);
+}
+
+// start: create task with RTU server - HardwareSerial versions
+void ModbusServerRTU::begin(HardwareSerial& serial, int coreID) {
+  MSRserial = &serial;
+  uint32_t baudRate = serial.baudRate();
+  serial.setRxFIFOFull(1);
+  doBegin(baudRate, coreID);
+}
+
+void ModbusServerRTU::doBegin(uint32_t baudRate, int coreID) {
+  // Task already running? Stop it in case.
+  end();
+
+  // Set minimum interval time
+  MSRinterval = RTUutils::calculateInterval(baudRate);
+
+  // Create unique task name
+  char taskName[18];
+  snprintf(taskName, 18, "MBsrv%02XRTU", instanceCounter);
+
+  // Start task to handle the client
+  xTaskCreatePinnedToCore((TaskFunction_t)&serve, taskName, SERVER_TASK_STACK, this, 8, &serverTask, coreID >= 0 ? coreID : NULL);
+
+  LOG_D("Server task %d started. Interval=%d\n", (uint32_t)serverTask, MSRinterval);
+}
+
+// end: kill server task
+void ModbusServerRTU::end() {
+  if (serverTask != nullptr) {
+    vTaskDelete(serverTask);
+    LOG_D("Server task %d stopped.\n", (uint32_t)serverTask);
+    serverTask = nullptr;
+  }
+}
+
+// Toggle protocol to ModbusASCII
+void ModbusServerRTU::useModbusASCII(unsigned long timeout) {
+  MSRuseASCII = true;
+  serverTimeout = timeout; // Set timeout to ASCII's value
+  LOG_D("Protocol mode: ASCII\n");
+}
+
+// Toggle protocol to ModbusRTU
+void ModbusServerRTU::useModbusRTU() {
+  MSRuseASCII = false;
+  LOG_D("Protocol mode: RTU\n");
+}
+
+// Inquire protocol mode
+bool ModbusServerRTU::isModbusASCII() {
+  return MSRuseASCII;
+}
+
+// Toggle skipping of leading 0x00 byte
+void ModbusServerRTU::skipLeading0x00(bool onOff) {
+  MSRskipLeadingZeroByte = onOff;
+  LOG_D("Skip leading 0x00 mode = %s\n", onOff ? "ON" : "OFF");
+}
+
+// Special case: worker to react on broadcast requests
+void ModbusServerRTU::registerBroadcastWorker(MSRlistener worker) {
+  // If there is one already, it will be overwritten!
+  listener = worker;
+  LOG_D("Registered worker for broadcast requests\n");
+}
+
+// Even more special: register a sniffer worker
+void ModbusServerRTU::registerSniffer(MSRlistener worker) {
+  // If there is one already, it will be overwritten!
+  // This holds true for the broadcast worker as well, 
+  // so a sniffer never will do else but to sniff on broadcast requests!
+  sniffer = worker;
+  LOG_D("Registered sniffer\n");
+}
+
+// serve: loop until killed and receive messages from the RTU interface
+void ModbusServerRTU::serve(ModbusServerRTU *myServer) {
+  ModbusMessage request;                // received request message
+  ModbusMessage m;                      // Application's response data
+  ModbusMessage response;               // Response proper to be sent
+
+  // init microseconds timer
+  myServer->MSRlastMicros = micros();
+
+  while (true) {
+    // Initialize all temporary vectors
+    request.clear();
+    response.clear();
+    m.clear();
+
+    // Wait for and read an request
+    request = RTUutils::receive(
+      'S',
+      *(myServer->MSRserial), 
+      myServer->serverTimeout, 
+      myServer->MSRlastMicros, 
+      myServer->MSRinterval, 
+      myServer->MSRuseASCII, 
+      myServer->MSRskipLeadingZeroByte);
+
+    // Request longer than 1 byte (that will signal an error in receive())? 
+    if (request.size() > 1) {
+      LOG_D("Request received.\n");
+
+      // Yes. 
+      // Do we have a sniffer listening?
+      if (myServer->sniffer) {
+        // Yes. call it
+        myServer->sniffer(request);
+      }
+      // Is it a broadcast?
+      if (request[0] == 0) {
+        // Yes. Do we have a listener?
+        if (myServer->listener) {
+          // Yes. call it
+          myServer->listener(request);
+        }
+        // else we simply ignore it
+      } else {
+        // No Broadcast. 
+        // Do we have a callback function registered for it?
+        MBSworker callBack = myServer->getWorker(request[0], request[1]);
+        if (callBack) {
+          LOG_D("Callback found.\n");
+          // Yes, we do. Count the message
+          {
+            LOCK_GUARD(cntLock, myServer->m);
+            myServer->messageCount++;
+          }
+          // Get the user's response
+          LOG_D("Callback called.\n");
+          m = callBack(request);
+          HEXDUMP_V("Callback response", m.data(), m.size());
+
+          // Process Response. Is it one of the predefined types?
+          if (m[0] == 0xFF && (m[1] == 0xF0 || m[1] == 0xF1)) {
+            // Yes. Check it
+            switch (m[1]) {
+            case 0xF0: // NIL
+              response.clear();
+              break;
+            case 0xF1: // ECHO
+              response = request;
+              if (request.getFunctionCode() == WRITE_MULT_REGISTERS ||
+                  request.getFunctionCode() == WRITE_MULT_COILS) {
+                response.resize(6);
+              }
+              break;
+            default:   // Will not get here, but lint likes it!
+              break;
+            }
+          } else {
+            // No predefined. User provided data in free format
+            response = m;
+          }
+        } else {
+          // No callback. Is at least the serverID valid and no broadcast?
+          if (myServer->isServerFor(request[0]) && request[0] != 0x00) {
+            // Yes. Send back a ILLEGAL_FUNCTION error
+            response.setError(request.getServerID(), request.getFunctionCode(), ILLEGAL_FUNCTION);
+          }
+          // Else we will ignore the request, as it is not meant for us and we do not deal with broadcasts!
+        }
+        // Do we have gathered a valid response now?
+        if (response.size() >= 3) {
+          // Yes. send it back.
+          RTUutils::send(*(myServer->MSRserial), myServer->MSRlastMicros, myServer->MSRinterval, myServer->MRTSrts, response, myServer->MSRuseASCII);
+          LOG_D("Response sent.\n");
+          // Count it, in case we had an error response
+          if (response.getError() != SUCCESS) {
+            LOCK_GUARD(errorCntLock, myServer->m);
+            myServer->errorCount++;
+          }
+        }
+      }
+    } else {
+      // No, we got a 1-byte request, meaning an error has happened in receive()
+      // This is a server, so we will ignore TIMEOUT.
+      if (request[0] != TIMEOUT) {
+        // Any other error could be important for debugging, so print it
+        ModbusError me((Error)request[0]);
+        LOG_E("RTU receive: %02X - %s\n", (int)me, (const char *)me);
+      }
+    }
+    // Give scheduler room to breathe
+    delay(1);
+  }
+}
+
+#endif

lib/eModbus/src/ModbusServerRTU.h

@@ -0,0 +1,92 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_SERVER_RTU_H
+#define _MODBUS_SERVER_RTU_H
+
+#include "options.h"
+
+#if HAS_FREERTOS
+
+#include <Arduino.h>
+#include "Stream.h"
+#include "ModbusServer.h"
+#include "RTUutils.h"
+
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
+
+// Specal function signature for broadcast or sniffer listeners
+using MSRlistener = std::function<void(ModbusMessage msg)>;
+
+class ModbusServerRTU : public ModbusServer {
+public:
+  // Constructors
+  explicit ModbusServerRTU(uint32_t timeout, int rtsPin = -1);
+  ModbusServerRTU(uint32_t timeout, RTScallback rts);
+
+  // Destructor
+  ~ModbusServerRTU();
+
+  // begin: create task with RTU server to accept requests
+  void begin(Stream& serial, uint32_t baudRate, int coreID = -1);
+  void begin(HardwareSerial& serial, int coreID = -1);
+
+  // end: kill server task
+  void end();
+
+  // Toggle protocol to ModbusASCII
+  void useModbusASCII(unsigned long timeout = 1000);
+
+  // Toggle protocol to ModbusRTU
+  void useModbusRTU();
+
+  // Inquire protocol mode
+  bool isModbusASCII();
+
+  // Toggle skipping of leading 0x00 byte
+  void skipLeading0x00(bool onOff = true);
+
+  // Special case: worker to react on broadcast requests
+  void registerBroadcastWorker(MSRlistener worker);
+
+  // Even more special: register a sniffer worker
+  void registerSniffer(MSRlistener worker);
+
+protected:
+  // Prevent copy construction and assignment
+  ModbusServerRTU(ModbusServerRTU& m) = delete;
+  ModbusServerRTU& operator=(ModbusServerRTU& m) = delete;
+
+  inline void isInstance() { }           // Make class instantiable
+
+  // internal common begin function
+  void doBegin(uint32_t baudRate, int coreID);
+
+  static uint8_t instanceCounter;        // Number of RTU servers created (for task names)
+  TaskHandle_t serverTask;               // task of the started server
+  uint32_t serverTimeout;                // given timeout for receive. Does not really
+                                         // matter for a server, but is needed in 
+                                         // RTUutils. After timeout without any message
+                                         // the server will pause ~1ms and start 
+                                         // receive again.
+  Stream *MSRserial;                     // The serial interface to use
+  uint32_t MSRinterval;                  // Bus quiet time between messages
+  unsigned long MSRlastMicros;           // microsecond time stamp of last bus activity
+  int8_t MSRrtsPin;                      // GPIO number of the RS485 module's RE/DE line
+  RTScallback MRTSrts;                   // Callback to set the RTS line to HIGH/LOW
+  bool MSRuseASCII;                      // true=ModbusASCII, false=ModbusRTU
+  bool MSRskipLeadingZeroByte;           // true=first byte ignored if 0x00, false=all bytes accepted
+  MSRlistener listener;                  // Broadcast listener 
+  MSRlistener sniffer;                   // Sniffer listener 
+
+  // serve: loop function for server task
+  static void serve(ModbusServerRTU *myself);
+};
+
+#endif  // HAS_FREERTOS
+
+#endif // INCLUDE GUARD

lib/eModbus/src/ModbusServerTCPasync.cpp

@@ -0,0 +1,267 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+
+#include "ModbusServerTCPasync.h"
+#define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+// #undef LOCAL_LOG_LEVEL
+#include "Logging.h"
+
+ModbusServerTCPasync::mb_client::mb_client(ModbusServerTCPasync * s, AsyncClient * c)
+    : server(s)
+    , client(c)
+    , lastActiveTime(millis())
+    , message(nullptr)
+    , error(SUCCESS)
+    , 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->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->setNoDelay(true);
+}
+
+ModbusServerTCPasync::mb_client::~mb_client() {
+    // clear outbox, if data is left
+    while (!outbox.empty()) {
+        outbox.pop();
+    }
+
+    delete client; // will also close connection, if any
+}
+
+void ModbusServerTCPasync::mb_client::onData(uint8_t * data, size_t len) {
+    lastActiveTime = millis();
+    LOG_D("data len %d\n", len);
+
+    Error  error = SUCCESS;
+    size_t i     = 0;
+    while (i < len) {
+        // 0. start
+        if (!message) {
+            message = new ModbusMessage(8);
+            error   = SUCCESS;
+        }
+
+        //  1. get minimal 8 bytes to move on
+        while (message->size() < 8 && i < len) {
+            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
+        }
+
+        // 3. receive until request is complete
+        while (message->size() < messageLength && i < len) {
+            message->push_back(data[i++]);
+        }
+        if (message->size() == messageLength) {
+            LOG_D("request complete (len:%d)\n", message->size());
+        } else {
+            LOG_D("request incomplete (len:%d), waiting for next TCP packet\n", message->size());
+            continue;
+        }
+
+        // 4. request complete, process
+        ModbusMessage request(messageLength - 6); // create request without MBAP, with server ID
+        request.add(message->data() + 6, message->size() - 6);
+        ModbusMessage userData;
+        if (server->isServerFor(request.getServerID())) {
+            MBSworker callback = server->getWorker(request.getServerID(), request.getFunctionCode());
+            if (callback) {
+                // request is well formed and is being served by user API
+                userData = callback(request);
+                // Process Response
+                // One of the predefined types?
+                if (userData[0] == 0xFF && (userData[1] == 0xF0 || userData[1] == 0xF1)) {
+                    // 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
+            error = INVALID_SERVER;
+        }
+        if (error != SUCCESS) {
+            userData.setError(request.getServerID(), request.getFunctionCode(), error);
+        }
+        // Keep transaction id and protocol id
+        message->resize(4);
+        // 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);
+    handleOutbox();
+    if (server->idle_timeout > 0 && millis() - lastActiveTime > server->idle_timeout) {
+        LOG_D("client idle, closing\n");
+        client->close();
+    }
+}
+
+void ModbusServerTCPasync::mb_client::onDisconnect() {
+    LOG_D("client disconnected\n");
+    server->onClientDisconnect(this);
+}
+
+void ModbusServerTCPasync::mb_client::addResponseToOutbox(ModbusMessage * response) {
+    if (response->size() > 0) {
+        LOCK_GUARD(lock1, obLock);
+        outbox.push(response);
+        handleOutbox();
+    }
+}
+
+void ModbusServerTCPasync::mb_client::handleOutbox() {
+    while (!outbox.empty()) {
+        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->send();
+            delete m;
+            outbox.pop();
+        } else {
+            return;
+        }
+    }
+}
+
+ModbusServerTCPasync::ModbusServerTCPasync()
+    : server(nullptr)
+    , clients()
+    , maxNoClients(5)
+    , idle_timeout(60000) {
+    // setup will be done in 'start'
+}
+
+
+ModbusServerTCPasync::~ModbusServerTCPasync() {
+    stop();
+    delete server;
+}
+
+
+uint16_t ModbusServerTCPasync::activeClients() {
+    LOCK_GUARD(lock1, cListLock);
+    return clients.size();
+}
+
+
+bool ModbusServerTCPasync::start(uint16_t port, uint8_t max_clients, uint32_t timeout, int coreID) {
+    // don't restart if already running
+    if (server) {
+        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;
+}
+
+bool ModbusServerTCPasync::stop() {
+    if (!server) {
+        LOG_W("Server not running.\n");
+        return false;
+    }
+
+    // stop server to prevent new clients connecting
+    server->end();
+
+    // now close existing clients
+    LOCK_GUARD(lock1, cListLock);
+    while (!clients.empty()) {
+        // prevent onDisconnect handler to be called, resulting in deadlock
+        clients.front()->client->onDisconnect(nullptr, nullptr);
+        delete clients.front();
+        clients.pop_front();
+    }
+    delete server;
+    server = nullptr;
+    LOG_D("Modbus server stopped\n");
+    return true;
+}
+
+bool ModbusServerTCPasync::isRunning() {
+    if (server)
+        return true;
+    else
+        return false;
+}
+
+void ModbusServerTCPasync::onClientConnect(AsyncClient * client) {
+    LOG_D("new client\n");
+    LOCK_GUARD(lock1, cListLock);
+    if (clients.size() < maxNoClients) {
+        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());
+}

lib/eModbus/src/ModbusServerTCPasync.h

@@ -0,0 +1,92 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_SERVER_TCP_ASYNC_H
+#define _MODBUS_SERVER_TCP_ASYNC_H
+
+#include "options.h"
+
+#include <list>
+#include <queue>
+#if USE_MUTEX
+#include <mutex> // NOLINT
+#endif
+#include <vector>
+
+#include <Arduino.h> // for millis()
+
+#if defined(ESP32)
+#include <AsyncTCP.h>
+#elif defined(ESP8266)
+#include <ESPAsyncTCP.h>
+#endif
+
+#include "ModbusServer.h"
+
+#if USE_MUTEX
+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();
+
+      private:
+        void                        onData(uint8_t * data, size_t len);
+        void                        onPoll();
+        void                        onDisconnect();
+        void                        addResponseToOutbox(ModbusMessage * response);
+        void                        handleOutbox();
+        ModbusServerTCPasync *      server;
+        AsyncClient *               client;
+        uint32_t                    lastActiveTime;
+        ModbusMessage *             message;
+        Modbus::Error               error;
+        std::queue<ModbusMessage *> outbox;
+#if USE_MUTEX
+        std::mutex obLock; // outbox protection
+#endif
+    };
+
+
+  public:
+    // Constructor
+    ModbusServerTCPasync();
+
+    // Destructor: closes the connections
+    ~ModbusServerTCPasync();
+
+    // activeClients: return number of clients currently employed
+    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);
+
+    // stop: drop all connections and kill server task
+    bool stop();
+
+    // isRunning: return true is server is running
+    bool isRunning();
+
+  protected:
+    inline void isInstance() {
+    }
+    void onClientConnect(AsyncClient * client);
+    void onClientDisconnect(mb_client * client);
+
+    AsyncServer *          server;
+    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

@@ -0,0 +1,433 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_SERVER_TCP_TEMP_H
+#define _MODBUS_SERVER_TCP_TEMP_H
+
+#include <Arduino.h>
+#include <mutex> // NOLINT
+#include "ModbusServer.h"
+#undef LOCAL_LOG_LEVEL
+// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+#include "Logging.h"
+
+extern "C" {
+#include <freertos/FreeRTOS.h>
+#include <freertos/task.h>
+}
+
+using std::lock_guard;
+using std::mutex;
+using std::vector;
+
+template <typename ST, typename CT>
+class ModbusServerTCP : public ModbusServer {
+  public:
+    // Constructor
+    ModbusServerTCP();
+
+    // Destructor: closes the connections
+    ~ModbusServerTCP();
+
+    // activeClients: return number of clients currently employed
+    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);
+
+    // stop: drop all connections and kill server task
+    bool stop();
+
+  protected:
+    // Prevent copy construction and assignment
+    ModbusServerTCP(ModbusServerTCP & m)             = delete;
+    ModbusServerTCP & operator=(ModbusServerTCP & m) = delete;
+
+    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;
+
+    // serve: loop function for server task
+    static void serve(ModbusServerTCP<ST, CT> * myself);
+
+    // worker: loop function for client tasks
+    static void worker(ClientData * myData);
+
+    // receive: read data from TCP
+    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);
+
+    // 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()
+    : ModbusServer()
+    , numClients(0)
+    , serverTask(nullptr)
+    , serverPort(502)
+    , serverTimeout(20000)
+    , serverGoDown(false) {
+    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) {
+        if (clients[i] != nullptr) {
+            delete clients[i];
+        }
+    }
+    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;
+    for (uint8_t i = 0; i < numClients; ++i) {
+        // Current slot could have been previously used - look for cleared task handles
+        if (clients[i] != nullptr) {
+            // Empty task handle?
+            if (clients[i]->task == nullptr) {
+                // Yes. Delete entry and init client pointer
+                lock_guard<mutex> cL(clientLock);
+                delete clients[i];
+                LOG_V("Delete client %d\n", i);
+                clients[i] = nullptr;
+            }
+        }
+        if (clients[i] != nullptr)
+            cnt++;
+    }
+    return cnt;
+}
+
+// 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) {
+    // Task already running?
+    if (serverTask != nullptr) {
+        // Yes. stop it first
+        stop();
+    }
+    // Does the required number of slots fit?
+    if (numClients != max_clients) {
+        // No. Drop array and allocate a new one
+        delete[] clients;
+        // Now allocate a new one
+        numClients = max_clients;
+        clients    = new ClientData *[numClients]();
+    }
+    serverPort    = port;
+    serverTimeout = timeout;
+    serverGoDown  = false;
+
+    // Create unique task name
+    char taskName[18];
+    snprintf(taskName, 18, "MBserve%04X", port);
+
+    // Start task to handle the client
+    xTaskCreatePinnedToCore((TaskFunction_t)&serve, taskName, SERVER_TASK_STACK, this, 5, &serverTask, coreID >= 0 ? coreID : NULL);
+    LOG_D("Server task %s started (%d).\n", taskName, (uint32_t)serverTask);
+
+    // Wait two seconds for it to establish
+    delay(2000);
+
+    return true;
+}
+
+// stop: drop all connections and kill server task
+template <typename ST, typename CT>
+bool ModbusServerTCP<ST, CT>::stop() {
+    // Check for clients still connected
+    for (uint8_t i = 0; i < numClients; ++i) {
+        // Client is alive?
+        if (clients[i] != nullptr) {
+            // Yes. Close the connection
+            delete clients[i];
+            clients[i] = nullptr;
+        }
+    }
+    if (serverTask != nullptr) {
+        // Signal server task to stop
+        serverGoDown = true;
+        delay(5000);
+        LOG_D("Killed server task %d\n", (uint32_t)(serverTask));
+        serverTask   = nullptr;
+        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) {
+    // Look for an empty client slot
+    for (uint8_t i = 0; i < numClients; ++i) {
+        // Empty slot?
+        if (clients[i] == nullptr) {
+            // Yes. allocate new client data in slot
+            clients[i] = new ClientData(0, client, timeout, this);
+
+            // Create unique task name
+            char taskName[18];
+            snprintf(taskName, 18, "MBsrv%02Xclnt", i);
+
+            // Start task to handle the client
+            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));
+
+            return true;
+        }
+    }
+    LOG_D("No client slot available.\n");
+    return false;
+}
+
+template <typename ST, typename CT>
+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
+        ST server(myself->serverPort);
+
+        // Start it
+        server.begin();
+
+        // Loop until being killed
+        while (!myself->serverGoDown) {
+            // Do we have clients left to use?
+            if (myself->clientAvailable()) {
+                // Yes. accept one, when it connects
+                CT ec = server.accept();
+                // Did we get a connection?
+                if (ec) {
+                    // Yes. Forward it to the Modbus server
+                    myself->accept(ec, myself->serverTimeout, 0);
+                    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
+        SERVER_END;
+    }
+    vTaskDelete(NULL);
+}
+
+template <typename ST, typename CT>
+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;
+    unsigned long             myLastMessage = millis();
+
+    LOG_D("Worker started, timeout=%d\n", myTimeOut);
+
+    // loop forever, if timeout is 0, or until timeout was hit
+    while (myClient.connected() && (!myTimeOut || (millis() - myLastMessage < myTimeOut))) {
+        ModbusMessage response; // Data buffer to hold prepared response
+        // Get a request
+        if (myClient.available()) {
+            response.clear();
+            ModbusMessage m = myParent->receive(myClient, 100);
+
+            // has it the minimal length (6 bytes TCP header plus serverID plus FC)?
+            if (m.size() >= 8) {
+                {
+                    LOCK_GUARD(cntLock, myParent->m);
+                    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);
+    }
+
+    if (millis() - myLastMessage >= myTimeOut) {
+        // Timeout!
+        LOG_D("Worker stopping due to timeout.\n");
+    } else {
+        // Disconnected!
+        LOG_D("Worker stopping due to client disconnect.\n");
+    }
+
+    // Read away all that may still hang in the buffer
+    while (myClient.read() != -1) {
+    }
+    // Now stop the client
+    myClient.stop();
+
+    {
+        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) {
+    unsigned long  lastMillis = millis(); // Timer to check for timeout
+    ModbusMessage  m;                     // to take read data
+    uint16_t       lengthVal = 0;
+    uint16_t       cnt       = 0;
+    const uint16_t BUFFERSIZE(300);
+    uint8_t        buffer[BUFFERSIZE];
+
+    // wait for sufficient packet data or timeout
+    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)
+                lengthVal = buffer[cnt] << 8;
+            // Are we at the TCP header length field byte #2?
+            if (cnt == 5) {
+                lengthVal |= buffer[cnt];
+                lengthVal += 6;
+            }
+            cnt++;
+            // Rewind EOT and timeout timers
+            lastMillis = millis();
+        } else {
+            delay(1); // Give scheduler room to breathe
+        }
+    }
+    // Did we receive some data?
+    if (cnt) {
+        // Yes. Is it too much?
+        if (cnt >= BUFFERSIZE) {
+            // Yes, likely a buffer overflow of some sort
+            // Adjust message size in TCP header
+            buffer[4] = (cnt >> 8) & 0xFF;
+            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;
+}
+
+#endif

lib/eModbus/src/ModbusServerWiFi.h

@@ -0,0 +1,16 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_SERVER_WIFI_H
+#define _MODBUS_SERVER_WIFI_H
+#include "options.h"
+#include <WiFi.h>
+
+#undef SERVER_END
+#define SERVER_END server.end();
+
+#include "ModbusServerTCPtemp.h"
+using ModbusServerWiFi = ModbusServerTCP<WiFiServer, WiFiClient>;
+
+#endif

lib/eModbus/src/ModbusTypeDefs.cpp

@@ -0,0 +1,66 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "ModbusTypeDefs.h"
+
+#ifndef MINIMAL
+
+using Modbus::FCType;
+using Modbus::FCT;
+
+// Initialize function code type table
+FCType FCT::table[] = {
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FC01_TYPE, FC01_TYPE, FC01_TYPE, FC01_TYPE, FC01_TYPE, FC01_TYPE, FC07_TYPE,  // 0x0.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCGENERIC, FCILLEGAL, FCILLEGAL, FC07_TYPE, FC07_TYPE, FCILLEGAL, FCILLEGAL, FC0F_TYPE,  // 0x0.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FC10_TYPE, FC07_TYPE, FCILLEGAL, FCILLEGAL, FCGENERIC, FCGENERIC, FC16_TYPE, FCGENERIC,  // 0x1.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FC18_TYPE, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x1.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x2.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCGENERIC, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x2.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x3.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x3.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCUSER,     // 0x4.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCUSER,    FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x4.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x5.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x5.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCUSER,    FCUSER,    FCUSER,    FCUSER,     // 0x6.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCUSER,    FCILLEGAL,  // 0x6.
+// 0x.0      0x.1       0x.2       0x.3       0x.4       0x.5       0x.6       0x.7
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x7.
+// 0x.8      0x.9       0x.A       0x.B       0x.C       0x.D       0x.E       0x.F
+  FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL, FCILLEGAL,  // 0x7.
+};
+
+// FCT::getType: get the function code type for a given function code
+FCType FCT::getType(uint8_t functionCode) {
+  return table[functionCode & 0x7F];
+}
+
+// setType: 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) {
+  uint8_t fc = functionCode & 0x7F;
+  
+  // Allow modifications for yet undefined codes only
+  if (table[fc] == FCILLEGAL) {
+    table[fc] = type;
+  }
+  return table[fc];
+}
+
+#endif

lib/eModbus/src/ModbusTypeDefs.h

@@ -0,0 +1,139 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _MODBUS_TYPEDEFS_H
+#define _MODBUS_TYPEDEFS_H
+#include <stdint.h>
+#include <stddef.h>
+#include <cstdint>
+
+namespace Modbus {
+
+enum FunctionCode : uint8_t {
+  ANY_FUNCTION_CODE       = 0x00, // Only valid for server to register function codes
+  READ_COIL               = 0x01,
+  READ_DISCR_INPUT        = 0x02,
+  READ_HOLD_REGISTER      = 0x03,
+  READ_INPUT_REGISTER     = 0x04,
+  WRITE_COIL              = 0x05,
+  WRITE_HOLD_REGISTER     = 0x06,
+  READ_EXCEPTION_SERIAL   = 0x07,
+  DIAGNOSTICS_SERIAL      = 0x08,
+  READ_COMM_CNT_SERIAL    = 0x0B,
+  READ_COMM_LOG_SERIAL    = 0x0C,
+  WRITE_MULT_COILS        = 0x0F,
+  WRITE_MULT_REGISTERS    = 0x10,
+  REPORT_SERVER_ID_SERIAL = 0x11,
+  READ_FILE_RECORD        = 0x14,
+  WRITE_FILE_RECORD       = 0x15,
+  MASK_WRITE_REGISTER     = 0x16,
+  R_W_MULT_REGISTERS      = 0x17,
+  READ_FIFO_QUEUE         = 0x18,
+  ENCAPSULATED_INTERFACE  = 0x2B,
+  USER_DEFINED_41         = 0x41,
+  USER_DEFINED_42         = 0x42,
+  USER_DEFINED_43         = 0x43,
+  USER_DEFINED_44         = 0x44,
+  USER_DEFINED_45         = 0x45,
+  USER_DEFINED_46         = 0x46,
+  USER_DEFINED_47         = 0x47,
+  USER_DEFINED_48         = 0x48,
+  USER_DEFINED_64         = 0x64,
+  USER_DEFINED_65         = 0x65,
+  USER_DEFINED_66         = 0x66,
+  USER_DEFINED_67         = 0x67,
+  USER_DEFINED_68         = 0x68,
+  USER_DEFINED_69         = 0x69,
+  USER_DEFINED_6A         = 0x6A,
+  USER_DEFINED_6B         = 0x6B,
+  USER_DEFINED_6C         = 0x6C,
+  USER_DEFINED_6D         = 0x6D,
+  USER_DEFINED_6E         = 0x6E,
+};
+
+enum Error : uint8_t {
+  SUCCESS                = 0x00,
+  ILLEGAL_FUNCTION       = 0x01,
+  ILLEGAL_DATA_ADDRESS   = 0x02,
+  ILLEGAL_DATA_VALUE     = 0x03,
+  SERVER_DEVICE_FAILURE  = 0x04,
+  ACKNOWLEDGE            = 0x05,
+  SERVER_DEVICE_BUSY     = 0x06,
+  NEGATIVE_ACKNOWLEDGE   = 0x07,
+  MEMORY_PARITY_ERROR    = 0x08,
+  GATEWAY_PATH_UNAVAIL   = 0x0A,
+  GATEWAY_TARGET_NO_RESP = 0x0B,
+  TIMEOUT                = 0xE0,
+  INVALID_SERVER         = 0xE1,
+  CRC_ERROR              = 0xE2, // only for Modbus-RTU
+  FC_MISMATCH            = 0xE3,
+  SERVER_ID_MISMATCH     = 0xE4,
+  PACKET_LENGTH_ERROR    = 0xE5,
+  PARAMETER_COUNT_ERROR  = 0xE6,
+  PARAMETER_LIMIT_ERROR  = 0xE7,
+  REQUEST_QUEUE_FULL     = 0xE8,
+  ILLEGAL_IP_OR_PORT     = 0xE9,
+  IP_CONNECTION_FAILED   = 0xEA,
+  TCP_HEAD_MISMATCH      = 0xEB,
+  EMPTY_MESSAGE          = 0xEC,
+  ASCII_FRAME_ERR        = 0xED,
+  ASCII_CRC_ERR          = 0xEE,
+  ASCII_INVALID_CHAR     = 0xEF,
+  BROADCAST_ERROR        = 0xF0,
+  UNDEFINED_ERROR        = 0xFF  // otherwise uncovered communication error
+};
+
+#ifndef MINIMAL
+
+// Constants for float and double re-ordering
+#define SWAP_BYTES     0x01
+#define SWAP_REGISTERS 0x02
+#define SWAP_WORDS     0x04
+#define SWAP_NIBBLES   0x08
+
+const uint8_t swapTables[8][8] = {
+  { 0, 1, 2, 3, 4, 5, 6, 7 },  // no swap
+  { 1, 0, 3, 2, 5, 4, 7, 6 },  // bytes only
+  { 2, 3, 0, 1, 6, 7, 4, 5 },  // registers only
+  { 3, 2, 1, 0, 7, 6, 5, 4 },  // registers and bytes
+  { 4, 5, 6, 7, 0, 1, 2, 3 },  // words only (double)
+  { 5, 4, 7, 6, 1, 0, 3, 2 },  // words and bytes (double)
+  { 6, 7, 4, 5, 2, 3, 0, 1 },  // words and registers (double)
+  { 7, 6, 5, 4, 3, 2, 1, 0 }   // Words, registers and bytes (double)
+};
+
+enum FCType : uint8_t {
+  FC01_TYPE,         // Two uint16_t parameters (FC 0x01, 0x02, 0x03, 0x04, 0x05, 0x06)
+  FC07_TYPE,         // no additional parameter (FCs 0x07, 0x0b, 0x0c, 0x11)
+  FC0F_TYPE,         // two uint16_t parameters, a uint8_t length byte and a uint16_t* pointer to array of bytes (FC 0x0f)
+  FC10_TYPE,         // two uint16_t parameters, a uint8_t length byte and a uint8_t* pointer to array of words (FC 0x10)
+  FC16_TYPE,         // three uint16_t parameters (FC 0x16)
+  FC18_TYPE,         // one uint16_t parameter (FC 0x18)
+  FCGENERIC,         // for FCs not yet explicitly coded (or too complex)
+  FCUSER,            // No checks except the server ID
+  FCILLEGAL,         // not allowed function codes
+};
+
+// FCT: static class to hold the types of function codes
+class FCT {
+protected:
+  static FCType table[128];     // data table
+  FCT() = delete;                // No instances allowed
+  FCT(const FCT&) = delete;      // No copy constructor
+  FCT& operator=(const FCT& other) = delete; // No assignment either
+public:
+  // getType: get the function code type for a given function code
+  static FCType getType(uint8_t functionCode);
+
+  // setType: change the type of a function code.
+  // This is possible only for the codes undefined yet and will return
+  // the effective type
+  static FCType redefineType(uint8_t functionCode, const FCType type = FCUSER);
+};
+
+#endif
+
+}  // namespace Modbus
+
+#endif

lib/eModbus/src/RTUutils.cpp

@@ -0,0 +1,467 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#include "options.h"
+#include "ModbusMessage.h"
+#include "RTUutils.h"
+#undef LOCAL_LOG_LEVEL
+// #define LOCAL_LOG_LEVEL LOG_LEVEL_VERBOSE
+#include "Logging.h"
+
+#if HAS_FREERTOS
+// calcCRC: calculate Modbus CRC16 on a given array of bytes
+uint16_t RTUutils::calcCRC(const uint8_t *data, uint16_t len) {
+  // CRC16 pre-calculated tables
+  const uint8_t crcHiTable[] = {
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
+    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81,
+    0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
+    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01,
+    0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0,
+    0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01,
+    0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81, 0x40, 0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41,
+    0x00, 0xC1, 0x81, 0x40, 0x01, 0xC0, 0x80, 0x41, 0x01, 0xC0, 0x80, 0x41, 0x00, 0xC1, 0x81,
+    0x40
+  };
+
+  const uint8_t crcLoTable[] = {
+    0x00, 0xC0, 0xC1, 0x01, 0xC3, 0x03, 0x02, 0xC2, 0xC6, 0x06, 0x07, 0xC7, 0x05, 0xC5, 0xC4,
+    0x04, 0xCC, 0x0C, 0x0D, 0xCD, 0x0F, 0xCF, 0xCE, 0x0E, 0x0A, 0xCA, 0xCB, 0x0B, 0xC9, 0x09,
+    0x08, 0xC8, 0xD8, 0x18, 0x19, 0xD9, 0x1B, 0xDB, 0xDA, 0x1A, 0x1E, 0xDE, 0xDF, 0x1F, 0xDD,
+    0x1D, 0x1C, 0xDC, 0x14, 0xD4, 0xD5, 0x15, 0xD7, 0x17, 0x16, 0xD6, 0xD2, 0x12, 0x13, 0xD3,
+    0x11, 0xD1, 0xD0, 0x10, 0xF0, 0x30, 0x31, 0xF1, 0x33, 0xF3, 0xF2, 0x32, 0x36, 0xF6, 0xF7,
+    0x37, 0xF5, 0x35, 0x34, 0xF4, 0x3C, 0xFC, 0xFD, 0x3D, 0xFF, 0x3F, 0x3E, 0xFE, 0xFA, 0x3A,
+    0x3B, 0xFB, 0x39, 0xF9, 0xF8, 0x38, 0x28, 0xE8, 0xE9, 0x29, 0xEB, 0x2B, 0x2A, 0xEA, 0xEE,
+    0x2E, 0x2F, 0xEF, 0x2D, 0xED, 0xEC, 0x2C, 0xE4, 0x24, 0x25, 0xE5, 0x27, 0xE7, 0xE6, 0x26,
+    0x22, 0xE2, 0xE3, 0x23, 0xE1, 0x21, 0x20, 0xE0, 0xA0, 0x60, 0x61, 0xA1, 0x63, 0xA3, 0xA2,
+    0x62, 0x66, 0xA6, 0xA7, 0x67, 0xA5, 0x65, 0x64, 0xA4, 0x6C, 0xAC, 0xAD, 0x6D, 0xAF, 0x6F,
+    0x6E, 0xAE, 0xAA, 0x6A, 0x6B, 0xAB, 0x69, 0xA9, 0xA8, 0x68, 0x78, 0xB8, 0xB9, 0x79, 0xBB,
+    0x7B, 0x7A, 0xBA, 0xBE, 0x7E, 0x7F, 0xBF, 0x7D, 0xBD, 0xBC, 0x7C, 0xB4, 0x74, 0x75, 0xB5,
+    0x77, 0xB7, 0xB6, 0x76, 0x72, 0xB2, 0xB3, 0x73, 0xB1, 0x71, 0x70, 0xB0, 0x50, 0x90, 0x91,
+    0x51, 0x93, 0x53, 0x52, 0x92, 0x96, 0x56, 0x57, 0x97, 0x55, 0x95, 0x94, 0x54, 0x9C, 0x5C,
+    0x5D, 0x9D, 0x5F, 0x9F, 0x9E, 0x5E, 0x5A, 0x9A, 0x9B, 0x5B, 0x99, 0x59, 0x58, 0x98, 0x88,
+    0x48, 0x49, 0x89, 0x4B, 0x8B, 0x8A, 0x4A, 0x4E, 0x8E, 0x8F, 0x4F, 0x8D, 0x4D, 0x4C, 0x8C,
+    0x44, 0x84, 0x85, 0x45, 0x87, 0x47, 0x46, 0x86, 0x82, 0x42, 0x43, 0x83, 0x41, 0x81, 0x80,
+    0x40
+  };
+
+  uint8_t crcHi = 0xFF;
+  uint8_t crcLo = 0xFF;
+  uint8_t index;
+
+  while (len--) {
+    index = crcLo ^ *data++;
+    crcLo = crcHi ^ crcHiTable[index];
+    crcHi = crcLoTable[index];
+  }
+  return (crcHi << 8 | crcLo);
+}
+
+// calcCRC: calculate Modbus CRC16 on a given message
+uint16_t RTUutils::calcCRC(ModbusMessage msg) {
+  return calcCRC(msg.data(), msg.size());
+}
+
+
+// validCRC #1: check the given CRC in a block of data for correctness
+bool RTUutils::validCRC(const uint8_t *data, uint16_t len) {
+  return validCRC(data, len - 2, data[len - 2] | (data[len - 1] << 8));
+}
+
+// validCRC #2: check the CRC of a block of data against a given one for equality
+bool RTUutils::validCRC(const uint8_t *data, uint16_t len, uint16_t CRC) {
+  uint16_t crc16 = calcCRC(data, len);
+  if (CRC == crc16) return true;
+  return false;
+}
+
+// validCRC #3: check the given CRC in a message for correctness
+bool RTUutils::validCRC(ModbusMessage msg) {
+  return validCRC(msg.data(), msg.size() - 2, msg[msg.size() - 2] | (msg[msg.size() - 1] << 8));
+}
+
+// validCRC #4: check the CRC of a message against a given one for equality
+bool RTUutils::validCRC(ModbusMessage msg, uint16_t CRC) {
+  return validCRC(msg.data(), msg.size(), CRC);
+}
+
+// addCRC: calculate the CRC for a given RTUMessage and add it to the end
+void RTUutils::addCRC(ModbusMessage& raw) {
+  uint16_t crc16 = calcCRC(raw.data(), raw.size());
+  raw.push_back(crc16 & 0xff);
+  raw.push_back((crc16 >> 8) & 0xFF);
+}
+
+// calculateInterval: determine the minimal gap time between messages
+uint32_t RTUutils::calculateInterval(uint32_t baudRate) {
+  uint32_t interval = 0;
+
+  // silent interval is at least 3.5x character time
+  interval = 35000000UL / baudRate;  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
+  if (interval < 1750) interval = 1750;       // lower limit according to Modbus RTU standard
+  LOG_V("Calc interval(%u)=%u\n", baudRate, interval);
+  return interval;
+}
+
+// send: send a message via Serial, watching interval times - including CRC!
+void RTUutils::send(Stream& serial, unsigned long& lastMicros, uint32_t interval, RTScallback rts, const uint8_t *data, uint16_t len, bool ASCIImode) {
+  // Clear serial buffers
+  while (serial.available()) serial.read();
+  
+  // Treat ASCII differently
+  if (ASCIImode) {
+    // Toggle rtsPin, if necessary
+    rts(HIGH);
+    // Yes, ASCII mode. Send lead-in
+    serial.write(':');
+
+    uint16_t cnt = len;
+    uint8_t crc = 0;
+    uint8_t *cp = (uint8_t *)data;
+
+    // Loop over all bytes of the message
+    while (cnt--) {
+      // Write two nibbles as ASCII characters
+      serial.write(ASCIIwrite[(*cp >> 4) & 0x0F]);
+      serial.write(ASCIIwrite[*cp & 0x0F]);
+      // Advance CRC
+      crc += *cp;
+      // Next byte
+      cp++;
+    }
+    // Finalize CRC (2's complement)
+    crc = ~crc;
+    crc++;
+    // Write ist - two nibbles as ASCII characters
+    serial.write(ASCIIwrite[(crc >> 4) & 0x0F]);
+    serial.write(ASCIIwrite[crc & 0x0F]);
+    
+    // Send lead-out
+    serial.write("\r\n");
+    serial.flush();
+    // Toggle rtsPin, if necessary
+    rts(LOW);
+  } else {
+    // RTU mode
+    uint16_t crc16 = calcCRC(data, len);
+
+    // Respect interval - we must not toggle rtsPin before
+    if (micros() - lastMicros < interval) delayMicroseconds(interval - (micros() - lastMicros));
+
+    // Toggle rtsPin, if necessary
+    rts(HIGH);
+    // Write message
+    serial.write(data, len);
+    // Write CRC in LSB order
+    serial.write(crc16 & 0xff);
+    serial.write((crc16 >> 8) & 0xFF);
+    serial.flush();
+    // Toggle rtsPin, if necessary
+    rts(LOW);
+  }
+
+  HEXDUMP_D("Sent packet", data, len);
+
+  // Mark end-of-message time for next interval
+  lastMicros = micros();
+}
+
+// send: send a message via Serial, watching interval times - including CRC!
+void RTUutils::send(Stream& serial, unsigned long& lastMicros, uint32_t interval, RTScallback rts, ModbusMessage raw, bool ASCIImode) {
+  send(serial, lastMicros, interval, rts, raw.data(), raw.size(), ASCIImode);
+}
+
+// receive: get (any) message from Serial, taking care of timeout and interval
+ModbusMessage RTUutils::receive(uint8_t caller, Stream& serial, uint32_t timeout, unsigned long& lastMicros, uint32_t interval, bool ASCIImode, bool skipLeadingZeroBytes) {
+  // Allocate initial receive buffer size: 1 block of BUFBLOCKSIZE bytes
+  const uint16_t BUFBLOCKSIZE(512);
+  uint8_t *buffer = new uint8_t[BUFBLOCKSIZE];
+  ModbusMessage rv;
+
+  // Index into buffer
+  uint16_t bufferPtr = 0;
+  // Byte read
+  int b = 0; 
+
+  // State machine states, RTU mode
+  enum STATES : uint8_t { WAIT_DATA = 0, IN_PACKET, DATA_READ, FINISHED };
+
+  // State machine states, ASCII mode
+  enum ASTATES : uint8_t { A_WAIT_DATA = 0, A_DATA, A_WAIT_LEAD_OUT, A_FINISHED };
+
+  uint8_t state;
+
+  // Timeout tracker
+  unsigned long TimeOut = millis();
+
+  // RTU mode?
+  if (!ASCIImode) {
+    // Yes.
+    state = WAIT_DATA;
+    // interval tracker 
+    lastMicros = micros();
+  
+    while (state != FINISHED) {
+      switch (state) {
+      // WAIT_DATA: await first data byte, but watch timeout
+      case WAIT_DATA:
+        // Blindly try to read a byte
+        b = serial.read();
+        // Did we get one?
+        if (b >= 0) {
+          // Yes. Note the time.
+          lastMicros = micros();
+          // Do we need to skip it, if it is zero?
+          if (b > 0 || !skipLeadingZeroBytes) {
+            // No, we can go process it regularly
+            buffer[bufferPtr++] = b;
+            state = IN_PACKET;
+          } 
+        } else {
+          // No, we had no byte. Just check the timeout period
+          if (millis() - TimeOut >= timeout) {
+            rv.push_back(TIMEOUT);
+            state = FINISHED;
+          }
+          delay(1);
+        }
+        break;
+      // IN_PACKET: read data until a gap of at least _interval time passed without another byte arriving
+      case IN_PACKET:
+        // tight loop until finished reading or error
+        while (state == IN_PACKET) {
+          // Is there a byte?
+          while (serial.available()) {
+            // Yes, collect it
+            buffer[bufferPtr++] = serial.read();
+            // Mark time of last byte
+            lastMicros = micros();
+            // Buffer full?
+            if (bufferPtr >= BUFBLOCKSIZE) {
+              // Yes. Something fishy here - bail out!
+              rv.push_back(PACKET_LENGTH_ERROR);
+              state = FINISHED;
+              break;
+            }
+          } 
+          // No more byte read
+          if (state == IN_PACKET) {
+            // Are we past the interval gap?
+            if (micros() - lastMicros >= interval) {
+              // Yes, terminate reading
+              LOG_V("%c/%ldus without data after %u\n", (const char)caller, micros() - lastMicros, bufferPtr);
+              state = DATA_READ;
+              break;
+            }
+          }
+        }
+        break;
+      // DATA_READ: successfully gathered some data. Prepare return object.
+      case DATA_READ:
+        // Did we get a sensible buffer length?
+        LOG_V("%c/", (const char)caller);
+        HEXDUMP_V("Raw buffer received", buffer, bufferPtr);
+        if (bufferPtr >= 4)
+        {
+          // Yes. Check CRC
+          if (!validCRC(buffer, bufferPtr)) {
+            // Ooops. CRC is wrong.
+            rv.push_back(CRC_ERROR);
+          } else {
+            // CRC was fine, Now allocate response object without the CRC
+            for (uint16_t i = 0; i < bufferPtr - 2; ++i) {
+              rv.push_back(buffer[i]);
+            }
+          }
+        } else {
+          // No, packet was too short for anything usable. Return error
+          rv.push_back(PACKET_LENGTH_ERROR);
+        }
+        state = FINISHED;
+        break;
+      // FINISHED: we are done, clean up.
+      case FINISHED:
+        // CLear serial buffer in case something is left trailing
+        // May happen with servers too slow!
+        while (serial.available()) serial.read();
+        break;
+      }
+    }
+  } else {
+    // We are in ASCII mode.
+    state = A_WAIT_DATA;
+
+    // Track nibbles in a byte
+    bool byteComplete = true; 
+
+    // Track bytes read
+    bool hadBytes = false;
+
+    // ASCII crc byte
+    uint8_t crc = 0;
+
+    while (state != A_FINISHED) {
+      // Always watch timeout - 1s
+      if (millis() - TimeOut >= timeout) {
+        // Timeout! Bail out with error
+        rv.push_back(TIMEOUT);
+        state = A_FINISHED;
+      } else {
+        // Still in time. Check for another byte on serial
+        if (!hadBytes && serial.available()) {
+          b = serial.read();
+          if (b >= 0) {
+            hadBytes = true;
+          }
+        }
+        // Only use state machine with new data arrived
+        if (hadBytes) {
+          // First reset timeout
+          TimeOut = millis();
+          // Is it a valid character?
+          if ((b & 0x80) || ASCIIread[b] == 0xFF) {
+            // No. Report error and leave.
+            rv.clear();
+            rv.push_back(ASCII_INVALID_CHAR);
+            hadBytes = false;
+            state = A_FINISHED;
+          } else {
+            // Yes, is valid. Furtheron use interpreted byte
+            b = ASCIIread[b];
+            switch (state) {
+            // A_WAIT_DATA: await lead-in byte ':'
+            case A_WAIT_DATA:
+              // Is it the lead-in?
+              if (b == 0xF0) {
+                // Yes, proceed to data read state
+                state = A_DATA;
+              }
+              // byte was consumed in any case
+              hadBytes = false;
+              break;
+            // A_DATA: read data as it comes
+            case A_DATA:
+              // Lead-out byte 1 received?
+              if (b == 0xF1) {
+                // Yes. Was last buffer byte completed?
+                if (byteComplete) {
+                  // Yes. Move to final state
+                  state = A_WAIT_LEAD_OUT;
+                } else {
+                  // No, signal with error
+                  rv.push_back(PACKET_LENGTH_ERROR);
+                  state = A_FINISHED;
+                }
+              } else {
+                // No lead-out, must be data byte.
+                // Is it valid?
+                if (b < 0xF0) {
+                  // Yes. Add it into current buffer byte
+                  buffer[bufferPtr] <<= 4;
+                  buffer[bufferPtr] += (b & 0x0F);
+                  // Advance nibble
+                  byteComplete = !byteComplete;
+                  // Was it the second of the byte?
+                  if (byteComplete) {
+                    // Yes. Advance CRC and move buffer pointer by one
+                    crc += buffer[bufferPtr];
+                    bufferPtr++;
+                    buffer[bufferPtr] = 0;
+                  }
+                } else {
+                  // No, garbage. report error
+                  rv.push_back(ASCII_INVALID_CHAR);
+                  state = A_FINISHED;
+                }
+              }
+              hadBytes = false;
+              break;
+            // A_WAIT_LEAD_OUT: await \n
+            case A_WAIT_LEAD_OUT:
+              if (b == 0xF2) {
+                // Lead-out byte 2 received. Transfer buffer to returned message
+                LOG_V("%c/", (const char)caller);
+                HEXDUMP_V("Raw buffer received", buffer, bufferPtr);
+                // Did we get a sensible buffer length?
+                if (bufferPtr >= 3)
+                {
+                  // Yes. Was the CRC calculated correctly?
+                  if (crc == 0) {
+                    // Yes, reduce buffer by 1 to get rid of CRC byte...
+                    bufferPtr--;
+                    // Move data into returned message
+                    for (uint16_t i = 0; i < bufferPtr; ++i) {
+                      rv.push_back(buffer[i]);
+                    }
+                  } else {
+                    // No, CRC calculation seems to have failed
+                    rv.push_back(ASCII_CRC_ERR);
+                  }
+                } else {
+                  // No, packet was too short for anything usable. Return error
+                  rv.push_back(PACKET_LENGTH_ERROR);
+                }
+              } else {
+                // No lead out byte 2, but something else - report error.
+                rv.push_back(ASCII_FRAME_ERR);
+              }
+              state = A_FINISHED;
+              break;
+            // A_FINISHED: Message completed
+            case A_FINISHED:
+              // Clean up serial buffer
+              while (serial.available()) serial.read();
+              break;
+            }
+          }
+        } else {
+          // No data received, so give the task scheduler room to breathe
+          delay(1);
+        }
+      }
+    }
+  }
+  // Deallocate buffer
+  delete[] buffer;
+
+  LOG_D("%c/", (const char)caller);
+  HEXDUMP_D("Received packet", rv.data(), rv.size());
+
+  return rv;
+}
+
+// Lower 7 bit ASCII characters - all invalid are set to 0xFF
+const char RTUutils::ASCIIread[] = { 
+  /* 00-07 */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 08-0F */ 0xFF, 0xFF, 0xF2, 0xFF, 0xFF, 0xF1, 0xFF, 0xFF,  // LF + CR
+  /* 10-17 */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 18-1F */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 20-27 */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 28-2F */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 30-37 */    0,    1,    2,    3,    4,    5,    6,    7,  // digits 0-7
+  /* 38-3F */    8,    9, 0xF0, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF,  // digits 8 + 9, :
+  /* 40-47 */ 0xFF,   10,   11,   12,   13,   14,   15, 0xFF,  // digits A-F
+  /* 48-4F */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 50-57 */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 58-5F */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 60-67 */ 0xFF,   10,   11,   12,   13,   14,   15, 0xFF,  // digits a-f
+  /* 68-6F */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 70-77 */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 
+  /* 78-7F */ 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF 
+};
+
+// Writable ASCII chars for hex digits
+const char RTUutils::ASCIIwrite[] = { 0x30, 0x31, 0x32, 0x33, 0x34, 0x35, 0x36, 0x37, 
+                                      0x38, 0x39, 0x41, 0x42, 0x43, 0x44, 0x45, 0x46 
+};
+#endif

lib/eModbus/src/RTUutils.h

@@ -0,0 +1,76 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _RTU_UTILS_H
+#define _RTU_UTILS_H
+#include <stdint.h>
+#if NEED_UART_PATCH
+#include <soc/uart_struct.h>
+#endif
+#include <vector>
+#include "Stream.h"
+#include "ModbusTypeDefs.h"
+#include <functional> 
+
+typedef std::function<void(bool level)> RTScallback;
+
+using namespace Modbus;  // NOLINT
+
+// RTUutils is bundling the send, receive and CRC functions for Modbus RTU communications.
+// RTU server and client will make use of it. 
+// All functions are static!
+class RTUutils {
+public:
+  friend class ModbusClientRTU;
+  friend class ModbusServerRTU;
+
+// calcCRC: calculate the CRC16 value for a given block of data
+  static uint16_t calcCRC(const uint8_t *data, uint16_t len);
+
+// calcCRC: calculate the CRC16 value for a given block of data
+  static uint16_t calcCRC(ModbusMessage msg);
+
+// validCRC #1: check the CRC in a block of data for validity
+  static bool validCRC(const uint8_t *data, uint16_t len);
+
+// validCRC #2: check the CRC of a block of data against a given one
+  static bool validCRC(const uint8_t *data, uint16_t len, uint16_t CRC);
+
+// validCRC #1: check the CRC in a message for validity
+  static bool validCRC(ModbusMessage msg);
+
+// validCRC #2: check the CRC of a message against a given one
+  static bool validCRC(ModbusMessage msg, uint16_t CRC);
+
+// addCRC: extend a RTUMessage by a valid CRC
+  static void addCRC(ModbusMessage& raw);
+
+// calculateInterval: determine the minimal gap time between messages
+  static uint32_t calculateInterval(uint32_t baudRate);
+
+// RTSauto: dummy callback for auto half duplex RS485 boards
+  inline static void RTSauto(bool level) { return; } // NOLINT
+
+// Necessary preparations for a HardwareSerial
+static void prepareHardwareSerial(HardwareSerial& s, uint16_t bufferSize = 260) {
+  s.setRxBufferSize(bufferSize);
+  s.setTxBufferSize(bufferSize);
+}
+
+protected:
+// Printable characters for ASCII protocol: 012345678ABCDEF
+  static const char ASCIIwrite[];
+  static const char ASCIIread[];
+
+  RTUutils() = delete;
+
+// receive: get a Modbus message from serial, maintaining timeouts etc.
+  static ModbusMessage receive(uint8_t caller, Stream& serial, uint32_t timeout, unsigned long& lastMicros, uint32_t interval, bool ASCIImode, bool skipLeadingZeroBytes = false);
+
+// send: send a Modbus message in either format (ModbusMessage or data/len)
+  static void send(Stream& serial, unsigned long& lastMicros, uint32_t interval, RTScallback r, const uint8_t *data, uint16_t len, bool ASCIImode);
+  static void send(Stream& serial, unsigned long& lastMicros, uint32_t interval, RTScallback r, ModbusMessage raw, bool ASCIImode);
+};
+
+#endif

lib/eModbus/src/options.h

@@ -0,0 +1,61 @@
+// =================================================================================================
+// eModbus: Copyright 2020 by Michael Harwerth, Bert Melis and the contributors to eModbus
+//               MIT license - see license.md for details
+// =================================================================================================
+#ifndef _EMODBUS_OPTIONS_H
+#define _EMODBUS_OPTIONS_H
+
+/* === ESP32 DEFINITIONS AND MACROS === */
+#if defined(ESP32) 
+#include <Arduino.h>
+#define USE_MUTEX 1
+#define HAS_FREERTOS 1
+#define HAS_ETHERNET 1
+#define IS_LINUX 0
+#define NEED_UART_PATCH 1
+const unsigned int SERVER_TASK_STACK = 4096;
+const unsigned int CLIENT_TASK_STACK = 4096;
+
+/* === ESP8266 DEFINITIONS AND MACROS === */
+#elif defined(ESP8266)
+#include <Arduino.h>
+#define USE_MUTEX 0
+#define HAS_FREERTOS 0
+#define HAS_ETHERNET 0
+#define IS_LINUX 0
+#define NEED_UART_PATCH 0
+
+/* === LINUX DEFINITIONS AND MACROS === */
+#elif defined(__linux__)
+#define USE_MUTEX 1
+#define HAS_FREERTOS 0
+#define HAS_ETHERNET 0
+#define IS_LINUX 1
+#define NEED_UART_PATCH 0
+#include <cstdio>  // for printf()
+#include <cstring> // for memcpy(), strlen() etc.
+#include <cinttypes> // for uint32_t etc.
+#if IS_RASPBERRY
+#include <wiringPi.h>
+#else
+#include <chrono>  // NOLINT
+// Use nanosleep() to avoid problems with pthreads (std::this_thread::sleep_for would interfere!)
+#define delay(x)  nanosleep((const struct timespec[]){{x/1000, (x%1000)*1000000L}}, NULL);
+typedef std::chrono::steady_clock clk;
+#define millis() std::chrono::duration_cast<std::chrono::milliseconds>(clk::now().time_since_epoch()).count()
+#define micros() std::chrono::duration_cast<std::chrono::microseconds>(clk::now().time_since_epoch()).count()
+#endif
+
+/* === INVALID TARGET === */
+#else
+#error Define target in options.h
+#endif
+
+/* === COMMON MACROS === */
+#if USE_MUTEX
+#define LOCK_GUARD(x,y) std::lock_guard<std::mutex> x(y);
+#else
+#define LOCK_GUARD(x,y)
+#endif
+
+#endif // _EMODBUS_OPTIONS_H