From 57d2e9c016c1948648710589ec9a0fc95ea83363 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Fri, 14 Aug 2020 16:48:42 +0200
Subject: [PATCH 01/25] Token implementation /3

---
 src/ModbusMessage.cpp     | 24 +++++++++----
 src/ModbusMessage.h       | 12 ++++---
 src/esp32ModbusRTU.cpp    | 74 +++++++++++++++++++++++++++++----------
 src/esp32ModbusRTU.h      | 16 +++++----
 src/esp32ModbusTypeDefs.h |  4 +++
 5 files changed, 93 insertions(+), 37 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 085aeba..8bc2f77 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -103,7 +103,8 @@ uint16_t make_word(uint8_t high, uint8_t low) {
 ModbusMessage::ModbusMessage(uint8_t length) :
   _buffer(nullptr),
   _length(length),
-  _index(0) {
+  _index(0),
+  _token(0) {
   if (length < 5) _length = 5;  // minimum for Modbus Exception codes
   _buffer = new uint8_t[_length];
   for (uint8_t i = 0; i < _length; ++i) {
@@ -123,6 +124,10 @@ uint8_t ModbusMessage::getSize() {
   return _index;
 }
 
+uint32_t ModbusMessage::getToken() {
+  return _token;
+}
+
 void ModbusMessage::add(uint8_t value) {
   if (_index < _length) _buffer[_index++] = value;
 }
@@ -138,12 +143,13 @@ ModbusRequest::ModbusRequest(uint8_t length) :
   return _address;
 }
 
-ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils) :
+ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_DISCR_INPUT;
   _address = address;
   _byteCount = numberCoils / 8 + 1;
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
   add(high(_address));
@@ -159,12 +165,13 @@ size_t ModbusRequest02::responseLength() {
   return 5 + _byteCount;
 }
 
-ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) :
+ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) :
   ModbusRequest(12) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_HOLD_REGISTER;
   _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
   add(high(_address));
@@ -180,12 +187,13 @@ size_t ModbusRequest03::responseLength() {
   return 5 + _byteCount;
 }
 
-ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) :
+ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_INPUT_REGISTER;
   _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
   add(high(_address));
@@ -202,12 +210,13 @@ size_t ModbusRequest04::responseLength() {
   return 5 + _byteCount;
 }
 
-ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data) :
+ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::WRITE_HOLD_REGISTER;
   _address = address;
   _byteCount = 2;  // 1 register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
   add(high(_address));
@@ -223,12 +232,13 @@ size_t ModbusRequest06::responseLength() {
   return 8;
 }
 
-ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data) :
+ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token) :
   ModbusRequest(9 + (numberRegisters * 2)) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::WRITE_MULT_REGISTERS;
   _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
+  _token = token;
   add(_slaveAddress);
   add(_functionCode);
   add(high(_address));
@@ -251,7 +261,7 @@ size_t ModbusRequest16::responseLength() {
 ModbusResponse::ModbusResponse(uint8_t length, ModbusRequest* request) :
   ModbusMessage(length),
   _request(request),
-  _error(esp32Modbus::SUCCES) {}
+  _error(esp32Modbus::SUCCES) { _token = request->getToken(); }
 
 bool ModbusResponse::isComplete() {
   if (_buffer[1] > 0x80 && _index == 5) {  // 5: slaveAddress(1), errorCode(1), CRC(2) + indexed
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index 4fdbaaa..511a8b3 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -37,6 +37,7 @@ class ModbusMessage {
   virtual ~ModbusMessage();
   uint8_t* getMessage();
   uint8_t getSize();
+  uint32_t getToken();
   void add(uint8_t value);
 
  protected:
@@ -44,6 +45,7 @@ class ModbusMessage {
   uint8_t* _buffer;
   uint8_t _length;
   uint8_t _index;
+  uint32_t _token;
 };
 
 class ModbusResponse;  // forward declare for use in ModbusRequest
@@ -64,35 +66,35 @@ class ModbusRequest : public ModbusMessage {
 // read discrete coils
 class ModbusRequest02 : public ModbusRequest {
  public:
-  explicit ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils);
+  explicit ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token = 0);
   size_t responseLength();
 };
 
 // read holding registers
 class ModbusRequest03 : public ModbusRequest {
  public:
-  explicit ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
+  explicit ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
   size_t responseLength();
 };
 
 // read input registers
 class ModbusRequest04 : public ModbusRequest {
  public:
-  explicit ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
+  explicit ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
   size_t responseLength();
 };
 
 // write single holding registers
 class ModbusRequest06 : public ModbusRequest {
  public:
-  explicit ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data);
+  explicit ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token = 0);
   size_t responseLength();
 };
 
 // write multiple holding registers
 class ModbusRequest16 : public ModbusRequest {
  public:
-  explicit ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data);
+  explicit ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token = 0);
   size_t responseLength();
 };
 
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 9500a0f..c26c2ae 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -31,11 +31,15 @@ using namespace esp32ModbusRTUInternals;  // NOLINT
 esp32ModbusRTU::esp32ModbusRTU(HardwareSerial* serial, int8_t rtsPin) :
   TimeOutValue(TIMEOUT_MS),
   _serial(serial),
-  _lastMillis(0),
+  _lastMicros(0),
   _interval(0),
   _rtsPin(rtsPin),
   _task(nullptr),
-  _queue(nullptr)  {
+  _queue(nullptr),
+  _onData(nullptr),
+  _onError(nullptr),
+  _onDataToken(nullptr),
+  _onErrorToken(nullptr)  {
     _queue = xQueueCreate(QUEUE_SIZE, sizeof(ModbusRequest*));
 }
 
@@ -51,31 +55,35 @@ void esp32ModbusRTU::begin(int coreID /* = -1 */) {
   }
   xTaskCreatePinnedToCore((TaskFunction_t)&_handleConnection, "esp32ModbusRTU", 4096, this, 5, &_task, coreID >= 0 ? coreID : NULL);
   // silent interval is at least 3.5x character time
-  _interval = 40000 / _serial->baudRate();  // 4 * 1000 * 10 / baud
-  if (_interval == 0) _interval = 1;  // minimum of 1msec interval
+  _interval = 35000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
+
+  // The following is okay for sending at any baud rate, but problematic at receiving with baud rates above 35000,
+  // since the calculated interval will be below 1000µs!
+  // f.i. 115200bd ==> interval=304µs
+  if (_interval < 1000) _interval = 1000;  // minimum of 1msec interval
 }
 
-bool esp32ModbusRTU::readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils) {
-  ModbusRequest* request = new ModbusRequest02(slaveAddress, address, numberCoils);
+bool esp32ModbusRTU::readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest02(slaveAddress, address, numberCoils, token);
   return _addToQueue(request);
 }
-bool esp32ModbusRTU::readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) {
-  ModbusRequest* request = new ModbusRequest03(slaveAddress, address, numberRegisters);
+bool esp32ModbusRTU::readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest03(slaveAddress, address, numberRegisters, token);
   return _addToQueue(request);
 }
 
-bool esp32ModbusRTU::readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters) {
-  ModbusRequest* request = new ModbusRequest04(slaveAddress, address, numberRegisters);
+bool esp32ModbusRTU::readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest04(slaveAddress, address, numberRegisters, token);
   return _addToQueue(request);
 }
 
-bool esp32ModbusRTU::writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data) {
-  ModbusRequest* request = new ModbusRequest06(slaveAddress, address, data);
+bool esp32ModbusRTU::writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest06(slaveAddress, address, data, token);
   return _addToQueue(request);
 }
 
-bool esp32ModbusRTU::writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data) {
-  ModbusRequest* request = new ModbusRequest16(slaveAddress, address, numberRegisters, data);
+bool esp32ModbusRTU::writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token) {
+  ModbusRequest* request = new ModbusRequest16(slaveAddress, address, numberRegisters, data, token);
   return _addToQueue(request);
 }
 
@@ -87,6 +95,14 @@ void esp32ModbusRTU::onError(esp32Modbus::MBRTUOnError handler) {
   _onError = handler;
 }
 
+void esp32ModbusRTU::onDataToken(esp32Modbus::MBRTUOnDataToken handler) {
+  _onDataToken = handler;
+}
+
+void esp32ModbusRTU::onErrorToken(esp32Modbus::MBRTUOnErrorToken handler) {
+  _onErrorToken = handler;
+}
+
 bool esp32ModbusRTU::_addToQueue(ModbusRequest* request) {
   if (!request) {
     return false;
@@ -105,9 +121,28 @@ void esp32ModbusRTU::_handleConnection(esp32ModbusRTU* instance) {
       instance->_send(request->getMessage(), request->getSize());
       ModbusResponse* response = instance->_receive(request);
       if (response->isSucces()) {
-        if (instance->_onData) instance->_onData(response->getSlaveAddress(), response->getFunctionCode(), request->getAddress(), response->getData(), response->getByteCount());
+        // if the non-token onData handler is set, call it
+        if (instance->_onData)
+          instance->_onData(
+            response->getSlaveAddress(),
+            response->getFunctionCode(),
+            request->getAddress(),
+            response->getData(),
+            response->getByteCount());
+        // else, if the token onData handler is set, call that
+        else if (instance->_onDataToken)
+          instance->_onDataToken(
+            response->getSlaveAddress(),
+            response->getFunctionCode(),
+            request->getAddress(),
+            response->getData(),
+            response->getByteCount(),
+            response->getToken());
       } else {
+        // Same for error responses. non-token onError set?
         if (instance->_onError) instance->_onError(response->getError());
+        // No, but token onError instead?
+        else if (instance->_onErrorToken) instance->_onErrorToken(response->getError(), response->getToken());
       }
       delete request;  // object created in public methods
       delete response;  // object created in _receive()
@@ -116,14 +151,14 @@ void esp32ModbusRTU::_handleConnection(esp32ModbusRTU* instance) {
 }
 
 void esp32ModbusRTU::_send(uint8_t* data, uint8_t length) {
-  while (millis() - _lastMillis < _interval) delay(1);  // respect _interval
+  while (micros() - _lastMicros < _interval) delayMicroseconds(1);  // respect _interval
   // Toggle rtsPin, if necessary
   if (_rtsPin >= 0) digitalWrite(_rtsPin, HIGH);
   _serial->write(data, length);
   _serial->flush();
   // Toggle rtsPin, if necessary
   if (_rtsPin >= 0) digitalWrite(_rtsPin, LOW);
-  _lastMillis = millis();
+  _lastMicros = micros();
 }
 
 // Adjust timeout on MODBUS - some slaves require longer/allow for shorter times
@@ -133,15 +168,16 @@ void esp32ModbusRTU::setTimeOutValue(uint32_t tov) {
 
 ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
   ModbusResponse* response = new ModbusResponse(request->responseLength(), request);
+  uint32_t lastMillis = millis();
   while (true) {
     while (_serial->available()) {
       response->add(_serial->read());
     }
     if (response->isComplete()) {
-      _lastMillis = millis();
+      lastMillis = millis();
       break;
     }
-    if (millis() - _lastMillis > TimeOutValue) {
+    if (millis() - lastMillis > TimeOutValue) {
       break;
     }
     delay(1);  // take care of watchdog
diff --git a/src/esp32ModbusRTU.h b/src/esp32ModbusRTU.h
index 1ee4cdd..b3c8764 100644
--- a/src/esp32ModbusRTU.h
+++ b/src/esp32ModbusRTU.h
@@ -52,13 +52,15 @@ class esp32ModbusRTU {
   explicit esp32ModbusRTU(HardwareSerial* serial, int8_t rtsPin = -1);
   ~esp32ModbusRTU();
   void begin(int coreID = -1);
-  bool readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils);
-  bool readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
-  bool readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters);
-  bool writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data);
-  bool writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data);
+  bool readDiscreteInputs(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token = 0);
+  bool readHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
+  bool readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
+  bool writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token = 0);
+  bool writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token = 0);
   void onData(esp32Modbus::MBRTUOnData handler);
   void onError(esp32Modbus::MBRTUOnError handler);
+  void onDataToken(esp32Modbus::MBRTUOnDataToken handler);
+  void onErrorToken(esp32Modbus::MBRTUOnErrorToken handler);
   void setTimeOutValue(uint32_t tov);
 
  private:
@@ -70,13 +72,15 @@ class esp32ModbusRTU {
  private:
   uint32_t TimeOutValue;
   HardwareSerial* _serial;
-  uint32_t _lastMillis;
+  uint32_t _lastMicros;
   uint32_t _interval;
   int8_t _rtsPin;
   TaskHandle_t _task;
   QueueHandle_t _queue;
   esp32Modbus::MBRTUOnData _onData;
   esp32Modbus::MBRTUOnError _onError;
+  esp32Modbus::MBRTUOnDataToken _onDataToken;
+  esp32Modbus::MBRTUOnErrorToken _onErrorToken;
 };
 
 #endif
diff --git a/src/esp32ModbusTypeDefs.h b/src/esp32ModbusTypeDefs.h
index 7af6eb8..f5f65c0 100644
--- a/src/esp32ModbusTypeDefs.h
+++ b/src/esp32ModbusTypeDefs.h
@@ -64,6 +64,10 @@ typedef std::function<void(uint16_t, uint8_t, esp32Modbus::FunctionCode, uint8_t
 typedef std::function<void(uint8_t, esp32Modbus::FunctionCode, uint16_t, uint8_t*, uint16_t)> MBRTUOnData;
 typedef std::function<void(uint16_t, esp32Modbus::Error)> MBTCPOnError;
 typedef std::function<void(esp32Modbus::Error)> MBRTUOnError;
+typedef std::function<void(uint16_t, uint8_t, esp32Modbus::FunctionCode, uint8_t*, uint16_t, uint32_t)> MBTCPOnDataToken;
+typedef std::function<void(uint8_t, esp32Modbus::FunctionCode, uint16_t, uint8_t*, uint16_t, uint32_t)> MBRTUOnDataToken;
+typedef std::function<void(uint16_t, esp32Modbus::Error, uint32_t)> MBTCPOnErrorToken;
+typedef std::function<void(esp32Modbus::Error, uint32_t)> MBRTUOnErrorToken;
 
 }  // namespace esp32Modbus
 

From bf4dc4975045ddf72c7353a8f0c5d8345c659652 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Tue, 18 Aug 2020 18:42:33 +0200
Subject: [PATCH 02/25] First shot at new receive function

---
 src/ModbusMessage.cpp  |  28 +++++-----
 src/ModbusMessage.h    |   3 +-
 src/esp32ModbusRTU.cpp | 115 +++++++++++++++++++++++++++++++++++++----
 3 files changed, 118 insertions(+), 28 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 8bc2f77..ed2369f 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -139,10 +139,18 @@ ModbusRequest::ModbusRequest(uint8_t length) :
   _address(0),
   _byteCount(0) {}
 
-  uint16_t ModbusRequest::getAddress() {
+uint16_t ModbusRequest::getAddress() {
   return _address;
 }
 
+uint8_t ModbusRequest::getSlaveAddress() {
+  return _slaveAddress;
+}
+
+uint8_t ModbusRequest::getFunctionCode() {
+  return _functionCode;
+}
+
 ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
@@ -263,30 +271,18 @@ ModbusResponse::ModbusResponse(uint8_t length, ModbusRequest* request) :
   _request(request),
   _error(esp32Modbus::SUCCES) { _token = request->getToken(); }
 
-bool ModbusResponse::isComplete() {
-  if (_buffer[1] > 0x80 && _index == 5) {  // 5: slaveAddress(1), errorCode(1), CRC(2) + indexed
-    return true;
-  }
-  if (_index == _request->responseLength()) return true;
-  return false;
-}
-
 bool ModbusResponse::isSucces() {
-  if (!isComplete()) {
-    _error = esp32Modbus::TIMEOUT;
-  } else if (_buffer[1] > 0x80) {
+  _error = esp32Modbus::SUCCES;
+  if (_buffer[1] > 0x80) {
     _error = static_cast<esp32Modbus::Error>(_buffer[2]);
   } else if (!checkCRC()) {
     _error = esp32Modbus::CRC_ERROR;
   // TODO(bertmelis): add other checks
-  } else {
-    _error = esp32Modbus::SUCCES;
   }
   if (_error == esp32Modbus::SUCCES) {
     return true;
-  } else {
-    return false;
   }
+  return false;
 }
 
 bool ModbusResponse::checkCRC() {
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index 511a8b3..ce53da5 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -54,6 +54,8 @@ class ModbusRequest : public ModbusMessage {
  public:
   virtual size_t responseLength() = 0;
   uint16_t getAddress();
+  uint8_t getFunctionCode();
+  uint8_t getSlaveAddress();
 
  protected:
   explicit ModbusRequest(uint8_t length);
@@ -101,7 +103,6 @@ class ModbusRequest16 : public ModbusRequest {
 class ModbusResponse : public ModbusMessage {
  public:
   explicit ModbusResponse(uint8_t length, ModbusRequest* request);
-  bool isComplete();
   bool isSucces();
   bool checkCRC();
   esp32Modbus::Error getError() const;
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index c26c2ae..356e52b 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -167,21 +167,114 @@ void esp32ModbusRTU::setTimeOutValue(uint32_t tov) {
 }
 
 ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
-  ModbusResponse* response = new ModbusResponse(request->responseLength(), request);
-  uint32_t lastMillis = millis();
-  while (true) {
-    while (_serial->available()) {
-      response->add(_serial->read());
-    }
-    if (response->isComplete()) {
-      lastMillis = millis();
+  // Allocate initial buffer size
+  const uint16_t BUFBLOCKSIZE(128);
+  uint8_t *buffer = new uint8_t[BUFBLOCKSIZE];
+  uint8_t bufferBlocks = 1;
+
+  // Index into buffer
+  register uint16_t bufferPtr = 0;
+
+  // State machine states
+  typedef enum STATES : uint8_t { WAIT_INTERVAL=0, WAIT_DATA, IN_PACKET, DATA_READ, ERROR_EXIT, FINISHED };
+  register STATES state = WAIT_INTERVAL;
+
+  // Timeout tracker
+  uint32_t TimeOut = millis();
+
+  // Error code
+  esp32Modbus::Error errorCode = esp32Modbus::SUCCES;
+
+  // Return data object
+  ModbusResponse* response = nullptr;
+
+  while(state!=FINISHED)
+  {
+    switch(state)
+    {
+    // WAIT_INTERVAL: spend the remainder of the bus quiet time waiting
+    case WAIT_INTERVAL:
+      // Time passed?
+      if(micros()-_lastMicros>=_interval) {
+        // Yes, proceed to reading data
+        state = WAIT_DATA;
+      }
+      else {
+        // No, wait a little longer
+        delayMicroseconds(1);
+      }
       break;
-    }
-    if (millis() - lastMillis > TimeOutValue) {
+    // WAIT_DATA: await first data byte, but watch timeout
+    case WAIT_DATA:
+      if(_serial.available()) {
+        state = IN_PACKET;
+        _lastMicros = micros();
+      }
+      else if(millis() - TimeOut >= TimeOutValue) {
+        errorCode = esp32Modbus::TIMEOUT;
+        state = ERROR_EXIT;
+      }
+      break;
+    // IN_PACKET: read data until a gap of at least _interval time passed without another byte arriving
+    case IN_PACKET:
+      // Data waiting and space left in buffer?
+      while (bufferPtr<128 && _serial->available()) {
+        // Yes. Catch the byte
+        buffer[bufferPtr++] = _serial.read();
+        // Rewind timer
+        _lastMicros = micros();
+      }
+      // Buffer full?
+      if(bufferPtr>=128) {
+        // Yes. Extend it by another block
+        bufferBlocks++;
+        uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
+        memcpy(temp, buffer, (bufferBlocks-1) * BUFBLOCKSIZE);
+        delete buffer;
+        buffer = temp;
+      }
+      // Gap of at least _interval micro seconds passed without data?
+      if(micros() - _lastMicros <= _interval) {
+        state = DATA_READ;
+      }
+      break;
+    // DATA_READ: successfully gathered some data. Prepare return object.
+    case DATA_READ:
+      // Check response for validity
+      // 1) Did the right slave answer?
+      if(buffer[0] != request->getSlaveAddress()) {
+        errorCode = esp32Modbus::INVALID_SLAVE;
+        state = ERROR_EXIT;
+      }
+      // More to be added here
+      else {
+        // Allocate response object
+        response = new ModbusResponse(bufferPtr, request);
+        // Move gathered data into it
+        for(uint16_t tPtr = 0; tPtr < bufferPtr; tPtr++) {
+          response->add(buffer[tPtr]);
+        }
+        state = FINISHED;
+      }
+      break;
+    // ERROR_EXIT: We had a timeout. Prepare error return object
+    case ERROR_EXIT:
+      response = new ModbusResponse(5, request);
+      response->add(request->getSlaveAddress());
+      response->add(request->getFunctionCode() | 0x80);
+      response->add(errorCode);
+      uint16_t CRC = CRC16(_buffer, 3);
+      response->add(low(CRC));
+      response->add(high(CRC));
+      state = FINISHED;
       break;
     }
-    delay(1);  // take care of watchdog
   }
+
+  // Deallocate buffer
+  delete buffer;
+  _lastMicros = micros();
+
   return response;
 }
 

From 5dac6eee63be3b834a7c9d054498e59588d376d1 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Tue, 18 Aug 2020 19:10:42 +0200
Subject: [PATCH 03/25] Modified getData and getByteCount to fit to new
 function codes

---
 src/ModbusMessage.cpp | 20 ++++++++++++++++----
 src/ModbusMessage.h   |  2 +-
 2 files changed, 17 insertions(+), 5 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index ed2369f..4fe01ff 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -302,14 +302,26 @@ uint8_t ModbusResponse::getSlaveAddress() {
   return _buffer[0];
 }
 
-esp32Modbus::FunctionCode ModbusResponse::getFunctionCode() {
-  return static_cast<esp32Modbus::FunctionCode>(_buffer[1]);
+uint8_t ModbusResponse::getFunctionCode() {
+  return _buffer[1];
 }
 
 uint8_t* ModbusResponse::getData() {
-  return &_buffer[3];
+  uint8_t fc = _request->getFunctionCode();
+  if(fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
+    return &_buffer[3];
+  }
+  else {
+    return &_buffer[2];
+  }
 }
 
 uint8_t ModbusResponse::getByteCount() {
-  return _buffer[2];
+  uint8_t fc = _request->getFunctionCode();
+  if(fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
+    return _buffer[2];
+  }
+  else {
+    return _index - 2;
+  }
 }
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index ce53da5..6cc700b 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -108,7 +108,7 @@ class ModbusResponse : public ModbusMessage {
   esp32Modbus::Error getError() const;
 
   uint8_t getSlaveAddress();
-  esp32Modbus::FunctionCode getFunctionCode();
+  uint8_t getFunctionCode();
   uint8_t* getData();
   uint8_t getByteCount();
 

From ac459098cb328bbcc6250563d9478b38e4a60f7e Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Wed, 19 Aug 2020 18:30:46 +0200
Subject: [PATCH 04/25] Add dynamic buffer sizes in receive

---
 src/ModbusMessage.cpp  |  3 ++-
 src/esp32ModbusRTU.cpp | 32 +++++++++++---------------------
 2 files changed, 13 insertions(+), 22 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 4fe01ff..667bdfc 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -272,11 +272,12 @@ ModbusResponse::ModbusResponse(uint8_t length, ModbusRequest* request) :
   _error(esp32Modbus::SUCCES) { _token = request->getToken(); }
 
 bool ModbusResponse::isSucces() {
-  _error = esp32Modbus::SUCCES;
   if (_buffer[1] > 0x80) {
     _error = static_cast<esp32Modbus::Error>(_buffer[2]);
   } else if (!checkCRC()) {
     _error = esp32Modbus::CRC_ERROR;
+  } else if (_buffer[0] != _request->getSlaveAddress()) {
+    _error = esp32Modbus::INVALID_SLAVE;
   // TODO(bertmelis): add other checks
   }
   if (_error == esp32Modbus::SUCCES) {
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 356e52b..3f1eeb1 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -188,14 +188,14 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
   // Return data object
   ModbusResponse* response = nullptr;
 
-  while(state!=FINISHED)
+  while(state != FINISHED)
   {
     switch(state)
     {
     // WAIT_INTERVAL: spend the remainder of the bus quiet time waiting
     case WAIT_INTERVAL:
       // Time passed?
-      if(micros()-_lastMicros>=_interval) {
+      if(micros() - _lastMicros >= _interval) {
         // Yes, proceed to reading data
         state = WAIT_DATA;
       }
@@ -218,18 +218,18 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
     // IN_PACKET: read data until a gap of at least _interval time passed without another byte arriving
     case IN_PACKET:
       // Data waiting and space left in buffer?
-      while (bufferPtr<128 && _serial->available()) {
+      while (bufferPtr < bufferBlocks * BUFBLOCKSIZE && _serial->available()) {
         // Yes. Catch the byte
         buffer[bufferPtr++] = _serial.read();
         // Rewind timer
         _lastMicros = micros();
       }
       // Buffer full?
-      if(bufferPtr>=128) {
+      if(bufferPtr >= bufferBlocks * BUFBLOCKSIZE) {
         // Yes. Extend it by another block
         bufferBlocks++;
         uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
-        memcpy(temp, buffer, (bufferBlocks-1) * BUFBLOCKSIZE);
+        memcpy(temp, buffer, (bufferBlocks - 1) * BUFBLOCKSIZE);
         delete buffer;
         buffer = temp;
       }
@@ -240,22 +240,12 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       break;
     // DATA_READ: successfully gathered some data. Prepare return object.
     case DATA_READ:
-      // Check response for validity
-      // 1) Did the right slave answer?
-      if(buffer[0] != request->getSlaveAddress()) {
-        errorCode = esp32Modbus::INVALID_SLAVE;
-        state = ERROR_EXIT;
-      }
-      // More to be added here
-      else {
-        // Allocate response object
-        response = new ModbusResponse(bufferPtr, request);
-        // Move gathered data into it
-        for(uint16_t tPtr = 0; tPtr < bufferPtr; tPtr++) {
-          response->add(buffer[tPtr]);
-        }
-        state = FINISHED;
-      }
+      // Allocate response object
+      response = new ModbusResponse(bufferPtr, request);
+      // Move gathered data into it
+      memcpy(_buffer, buffer, bufferPtr);
+      _index = bufferPtr;
+      state = FINISHED;
       break;
     // ERROR_EXIT: We had a timeout. Prepare error return object
     case ERROR_EXIT:

From 176e59d157adb8de4dddab7544c35f1926b9fb4f Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Wed, 19 Aug 2020 18:50:14 +0200
Subject: [PATCH 05/25] Add rawRequest

---
 src/ModbusMessage.cpp  | 30 ++++++++++++++++++++++++++++++
 src/ModbusMessage.h    |  7 +++++++
 src/esp32ModbusRTU.cpp |  5 +++++
 src/esp32ModbusRTU.h   |  1 +
 4 files changed, 43 insertions(+)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 667bdfc..e4fc9d6 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -266,6 +266,36 @@ size_t ModbusRequest16::responseLength() {
   return 8;
 }
 
+ModbusRequestRaw::ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token) :
+  ModbusRequest(dataLength + 4) {
+  _slaveAddress = slaveAddress;
+  _functionCode = functionCode;
+  _address = 0;
+  _byteCount = dataLength + 2;
+  _token = token;
+  add(_slaveAddress);
+  add(_functionCode);
+  for (int i = 0; i < dataLength; i++) {
+      add(data[i]);
+    }
+  uint16_t CRC = CRC16(_buffer, _byteCount);
+  add(low(CRC));
+  add(high(CRC));
+  /*
+  Serial.print("rawRequest: ");
+  for(uint8_t i=0; i<_byteCount; ++i)
+  {
+    Serial.print(_buffer[i], HEX);
+    Serial.print(" ");
+  }
+  Serial.println("");
+  */
+}
+
+size_t ModbusRequestRaw::responseLength() {
+  return 6;
+}
+
 ModbusResponse::ModbusResponse(uint8_t length, ModbusRequest* request) :
   ModbusMessage(length),
   _request(request),
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index 6cc700b..40c0df8 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -100,6 +100,13 @@ class ModbusRequest16 : public ModbusRequest {
   size_t responseLength();
 };
 
+// "raw" request based on a request packet obtained as is.
+class ModbusRequestRaw : public ModbusRequest {
+ public:
+  explicit ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token=0);
+  size_t responseLength();
+};
+
 class ModbusResponse : public ModbusMessage {
  public:
   explicit ModbusResponse(uint8_t length, ModbusRequest* request);
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 3f1eeb1..fb66dc2 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -87,6 +87,11 @@ bool esp32ModbusRTU::writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t ad
   return _addToQueue(request);
 }
 
+bool esp32ModbusRTU::rawRequest(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token) {
+  ModbusRequest* request = new ModbusRequestRaw(slaveAddress, functionCode, dataLength, data, token);
+  return _addToQueue(request);
+}
+
 void esp32ModbusRTU::onData(esp32Modbus::MBRTUOnData handler) {
   _onData = handler;
 }
diff --git a/src/esp32ModbusRTU.h b/src/esp32ModbusRTU.h
index b3c8764..56bb751 100644
--- a/src/esp32ModbusRTU.h
+++ b/src/esp32ModbusRTU.h
@@ -57,6 +57,7 @@ class esp32ModbusRTU {
   bool readInputRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
   bool writeSingleHoldingRegister(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token = 0);
   bool writeMultHoldingRegisters(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token = 0);
+  bool rawRequest(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t *data, uint32_t token = 0);
   void onData(esp32Modbus::MBRTUOnData handler);
   void onError(esp32Modbus::MBRTUOnError handler);
   void onDataToken(esp32Modbus::MBRTUOnDataToken handler);

From 3c10f02ccbe21d64616f9723d14bbebdc442a555 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Wed, 19 Aug 2020 18:59:14 +0200
Subject: [PATCH 06/25] Fix typos

---
 src/esp32ModbusRTU.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index fb66dc2..4681f6c 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -181,7 +181,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
   register uint16_t bufferPtr = 0;
 
   // State machine states
-  typedef enum STATES : uint8_t { WAIT_INTERVAL=0, WAIT_DATA, IN_PACKET, DATA_READ, ERROR_EXIT, FINISHED };
+  enum STATES : uint8_t { WAIT_INTERVAL=0, WAIT_DATA, IN_PACKET, DATA_READ, ERROR_EXIT, FINISHED };
   register STATES state = WAIT_INTERVAL;
 
   // Timeout tracker
@@ -211,7 +211,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       break;
     // WAIT_DATA: await first data byte, but watch timeout
     case WAIT_DATA:
-      if(_serial.available()) {
+      if(_serial->available()) {
         state = IN_PACKET;
         _lastMicros = micros();
       }
@@ -225,7 +225,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       // Data waiting and space left in buffer?
       while (bufferPtr < bufferBlocks * BUFBLOCKSIZE && _serial->available()) {
         // Yes. Catch the byte
-        buffer[bufferPtr++] = _serial.read();
+        buffer[bufferPtr++] = _serial->read();
         // Rewind timer
         _lastMicros = micros();
       }
@@ -263,6 +263,9 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       response->add(high(CRC));
       state = FINISHED;
       break;
+    // FINISHED: we are done, keep the compiler happy by pseudo-treating it.
+    case FINISHED:
+      break;
     }
   }
 

From 59c9445e0e55914e32f0b002ec03e02ff0f1032c Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Wed, 19 Aug 2020 19:18:15 +0200
Subject: [PATCH 07/25] Remove esp32Modbus::FunctionCode from onData function
 typedefs

---
 src/esp32ModbusTypeDefs.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/esp32ModbusTypeDefs.h b/src/esp32ModbusTypeDefs.h
index f5f65c0..dac4275 100644
--- a/src/esp32ModbusTypeDefs.h
+++ b/src/esp32ModbusTypeDefs.h
@@ -60,12 +60,12 @@ enum Error : uint8_t {
   COMM_ERROR            = 0xE4  // general communication error
 };
 
-typedef std::function<void(uint16_t, uint8_t, esp32Modbus::FunctionCode, uint8_t*, uint16_t)> MBTCPOnData;
-typedef std::function<void(uint8_t, esp32Modbus::FunctionCode, uint16_t, uint8_t*, uint16_t)> MBRTUOnData;
+typedef std::function<void(uint16_t, uint8_t, uint8_t, uint8_t*, uint16_t)> MBTCPOnData;
+typedef std::function<void(uint8_t, uint8_t, uint16_t, uint8_t*, uint16_t)> MBRTUOnData;
 typedef std::function<void(uint16_t, esp32Modbus::Error)> MBTCPOnError;
 typedef std::function<void(esp32Modbus::Error)> MBRTUOnError;
-typedef std::function<void(uint16_t, uint8_t, esp32Modbus::FunctionCode, uint8_t*, uint16_t, uint32_t)> MBTCPOnDataToken;
-typedef std::function<void(uint8_t, esp32Modbus::FunctionCode, uint16_t, uint8_t*, uint16_t, uint32_t)> MBRTUOnDataToken;
+typedef std::function<void(uint16_t, uint8_t, uint8_t, uint8_t*, uint16_t, uint32_t)> MBTCPOnDataToken;
+typedef std::function<void(uint8_t, uint8_t, uint16_t, uint8_t*, uint16_t, uint32_t)> MBRTUOnDataToken;
 typedef std::function<void(uint16_t, esp32Modbus::Error, uint32_t)> MBTCPOnErrorToken;
 typedef std::function<void(esp32Modbus::Error, uint32_t)> MBRTUOnErrorToken;
 

From e9d8760be93fc5a6f7f523df3818187488d39658 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Wed, 19 Aug 2020 19:24:58 +0200
Subject: [PATCH 08/25] _buffer, _index not visible outside response object

---
 src/esp32ModbusRTU.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 4681f6c..f4b49ed 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -248,8 +248,8 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       // Allocate response object
       response = new ModbusResponse(bufferPtr, request);
       // Move gathered data into it
-      memcpy(_buffer, buffer, bufferPtr);
-      _index = bufferPtr;
+      memcpy(response->_buffer, buffer, bufferPtr);
+      response->_index = bufferPtr;
       state = FINISHED;
       break;
     // ERROR_EXIT: We had a timeout. Prepare error return object

From 0de99d35874b6d9fd2cffe5cb99e876487732308 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Wed, 19 Aug 2020 19:30:17 +0200
Subject: [PATCH 09/25] remove _buffer and _index from receive completely

---
 src/esp32ModbusRTU.cpp | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index f4b49ed..9f1e0a7 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -248,8 +248,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       // Allocate response object
       response = new ModbusResponse(bufferPtr, request);
       // Move gathered data into it
-      memcpy(response->_buffer, buffer, bufferPtr);
-      response->_index = bufferPtr;
+      for(uint16_t fPtr = 0; fPtr < bufferPtr; fPtr++) response->add(buffer[fPtr]);
       state = FINISHED;
       break;
     // ERROR_EXIT: We had a timeout. Prepare error return object

From 8c8aac7d5143109d16b42364e60785723904b4ed Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Thu, 20 Aug 2020 15:50:21 +0200
Subject: [PATCH 10/25] Remove responseLength, add setErrorResponse, setData

---
 src/ModbusMessage.cpp  | 57 +++++++++++++++++-------------------------
 src/ModbusMessage.h    |  9 ++-----
 src/esp32ModbusRTU.cpp |  9 ++-----
 3 files changed, 27 insertions(+), 48 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index e4fc9d6..d0c17fd 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -169,10 +169,6 @@ ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_
   add(high(CRC));
 }
 
-size_t ModbusRequest02::responseLength() {
-  return 5 + _byteCount;
-}
-
 ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) :
   ModbusRequest(12) {
   _slaveAddress = slaveAddress;
@@ -191,10 +187,6 @@ ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_
   add(high(CRC));
 }
 
-size_t ModbusRequest03::responseLength() {
-  return 5 + _byteCount;
-}
-
 ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
@@ -213,11 +205,6 @@ ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_
   add(high(CRC));
 }
 
-size_t ModbusRequest04::responseLength() {
-  // slaveAddress (1) + functionCode (1) + byteCount (1) + length x 2 + CRC (2)
-  return 5 + _byteCount;
-}
-
 ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token) :
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
@@ -236,10 +223,6 @@ ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_
   add(high(CRC));
 }
 
-size_t ModbusRequest06::responseLength() {
-  return 8;
-}
-
 ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token) :
   ModbusRequest(9 + (numberRegisters * 2)) {
   _slaveAddress = slaveAddress;
@@ -262,10 +245,6 @@ ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_
   add(high(CRC));
 }
 
-size_t ModbusRequest16::responseLength() {
-  return 8;
-}
-
 ModbusRequestRaw::ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token) :
   ModbusRequest(dataLength + 4) {
   _slaveAddress = slaveAddress;
@@ -281,19 +260,6 @@ ModbusRequestRaw::ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, u
   uint16_t CRC = CRC16(_buffer, _byteCount);
   add(low(CRC));
   add(high(CRC));
-  /*
-  Serial.print("rawRequest: ");
-  for(uint8_t i=0; i<_byteCount; ++i)
-  {
-    Serial.print(_buffer[i], HEX);
-    Serial.print(" ");
-  }
-  Serial.println("");
-  */
-}
-
-size_t ModbusRequestRaw::responseLength() {
-  return 6;
 }
 
 ModbusResponse::ModbusResponse(uint8_t length, ModbusRequest* request) :
@@ -356,3 +322,26 @@ uint8_t ModbusResponse::getByteCount() {
     return _index - 2;
   }
 }
+
+void ModbusResponse::setErrorResponse(uint8_t slaveAddress, uint8_t functionCode, uint8_t errorCode) {
+  if(_length != 5) {
+    delete _buffer;
+    _buffer = new uint8_t[5];
+  }
+  _index = 0;
+  add(request->getSlaveAddress());
+  add(request->getFunctionCode() | 0x80);
+  add(errorCode);
+  uint16_t CRC = CRC16(_buffer, 3);
+  add(low(CRC));
+  add(high(CRC));
+}
+
+void ModbusResponse::setData(uint16_t dataLength, uint8_t *data) {
+  if(_length != dataLength) {
+    delete _buffer;
+    _buffer = new uint8_t[dataLength];
+  }
+  _index = dataLength;
+  memcpy(_buffer, data, dataLength);
+}
\ No newline at end of file
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index 40c0df8..8cbd573 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -52,7 +52,6 @@ class ModbusResponse;  // forward declare for use in ModbusRequest
 
 class ModbusRequest : public ModbusMessage {
  public:
-  virtual size_t responseLength() = 0;
   uint16_t getAddress();
   uint8_t getFunctionCode();
   uint8_t getSlaveAddress();
@@ -69,42 +68,36 @@ class ModbusRequest : public ModbusMessage {
 class ModbusRequest02 : public ModbusRequest {
  public:
   explicit ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_t numberCoils, uint32_t token = 0);
-  size_t responseLength();
 };
 
 // read holding registers
 class ModbusRequest03 : public ModbusRequest {
  public:
   explicit ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
-  size_t responseLength();
 };
 
 // read input registers
 class ModbusRequest04 : public ModbusRequest {
  public:
   explicit ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint32_t token = 0);
-  size_t responseLength();
 };
 
 // write single holding registers
 class ModbusRequest06 : public ModbusRequest {
  public:
   explicit ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_t data, uint32_t token = 0);
-  size_t responseLength();
 };
 
 // write multiple holding registers
 class ModbusRequest16 : public ModbusRequest {
  public:
   explicit ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_t numberRegisters, uint8_t* data, uint32_t token = 0);
-  size_t responseLength();
 };
 
 // "raw" request based on a request packet obtained as is.
 class ModbusRequestRaw : public ModbusRequest {
  public:
   explicit ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token=0);
-  size_t responseLength();
 };
 
 class ModbusResponse : public ModbusMessage {
@@ -118,6 +111,8 @@ class ModbusResponse : public ModbusMessage {
   uint8_t getFunctionCode();
   uint8_t* getData();
   uint8_t getByteCount();
+  void setErrorResponse(uint8_t slaveAddress, uint8_t functionCode, uint8_t errorCode);
+  void setData(uint16_t dataLength, uint8_t *data);
 
  private:
   ModbusRequest* _request;
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 9f1e0a7..a226e47 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -248,18 +248,13 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       // Allocate response object
       response = new ModbusResponse(bufferPtr, request);
       // Move gathered data into it
-      for(uint16_t fPtr = 0; fPtr < bufferPtr; fPtr++) response->add(buffer[fPtr]);
+      response->setData(bufferPtr, buffer);
       state = FINISHED;
       break;
     // ERROR_EXIT: We had a timeout. Prepare error return object
     case ERROR_EXIT:
       response = new ModbusResponse(5, request);
-      response->add(request->getSlaveAddress());
-      response->add(request->getFunctionCode() | 0x80);
-      response->add(errorCode);
-      uint16_t CRC = CRC16(_buffer, 3);
-      response->add(low(CRC));
-      response->add(high(CRC));
+      response->setErrorResponse(request->getSlaveAddress(), request->getFunctionCode(), errorCode);
       state = FINISHED;
       break;
     // FINISHED: we are done, keep the compiler happy by pseudo-treating it.

From 6830b420847b7bcdec02e7d5d1ad5c392acfab3c Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Thu, 20 Aug 2020 16:01:15 +0200
Subject: [PATCH 11/25] Simplify setError

---
 src/ModbusMessage.cpp  | 6 +++---
 src/ModbusMessage.h    | 3 ++-
 src/esp32ModbusRTU.cpp | 2 +-
 3 files changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index d0c17fd..6cb8dc9 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -323,14 +323,14 @@ uint8_t ModbusResponse::getByteCount() {
   }
 }
 
-void ModbusResponse::setErrorResponse(uint8_t slaveAddress, uint8_t functionCode, uint8_t errorCode) {
+void ModbusResponse::setErrorResponse(uint8_t errorCode) {
   if(_length != 5) {
     delete _buffer;
     _buffer = new uint8_t[5];
   }
   _index = 0;
-  add(request->getSlaveAddress());
-  add(request->getFunctionCode() | 0x80);
+  add(_request->getSlaveAddress());
+  add(_request->getFunctionCode() | 0x80);
   add(errorCode);
   uint16_t CRC = CRC16(_buffer, 3);
   add(low(CRC));
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index 8cbd573..d890a4a 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -27,6 +27,7 @@ SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
 #include <stdint.h>  // for uint*_t
 #include <stddef.h>  // for size_t
+#include <cstring>  // for memcpy
 
 #include "esp32ModbusTypeDefs.h"
 
@@ -111,7 +112,7 @@ class ModbusResponse : public ModbusMessage {
   uint8_t getFunctionCode();
   uint8_t* getData();
   uint8_t getByteCount();
-  void setErrorResponse(uint8_t slaveAddress, uint8_t functionCode, uint8_t errorCode);
+  void setErrorResponse(uint8_t errorCode);
   void setData(uint16_t dataLength, uint8_t *data);
 
  private:
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index a226e47..5d0eec3 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -254,7 +254,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
     // ERROR_EXIT: We had a timeout. Prepare error return object
     case ERROR_EXIT:
       response = new ModbusResponse(5, request);
-      response->setErrorResponse(request->getSlaveAddress(), request->getFunctionCode(), errorCode);
+      response->setErrorResponse(errorCode);
       state = FINISHED;
       break;
     // FINISHED: we are done, keep the compiler happy by pseudo-treating it.

From 37ccf602e345b31bb361312dc0780c0f27ba03c9 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Thu, 20 Aug 2020 16:19:54 +0200
Subject: [PATCH 12/25] Relax bus timing a bit

---
 src/esp32ModbusRTU.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 5d0eec3..7d6b584 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -55,7 +55,8 @@ void esp32ModbusRTU::begin(int coreID /* = -1 */) {
   }
   xTaskCreatePinnedToCore((TaskFunction_t)&_handleConnection, "esp32ModbusRTU", 4096, this, 5, &_task, coreID >= 0 ? coreID : NULL);
   // silent interval is at least 3.5x character time
-  _interval = 35000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
+  // _interval = 35000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
+  _interval = 40000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
 
   // The following is okay for sending at any baud rate, but problematic at receiving with baud rates above 35000,
   // since the calculated interval will be below 1000µs!

From 3b028eb68a80a660cf965c6ce9c6d298cf9d508d Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Fri, 21 Aug 2020 15:37:52 +0200
Subject: [PATCH 13/25] Missing _length in setErrorResponse and setData

---
 src/ModbusMessage.cpp  | 2 ++
 src/esp32ModbusRTU.cpp | 2 +-
 2 files changed, 3 insertions(+), 1 deletion(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 6cb8dc9..d30dcb8 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -327,6 +327,7 @@ void ModbusResponse::setErrorResponse(uint8_t errorCode) {
   if(_length != 5) {
     delete _buffer;
     _buffer = new uint8_t[5];
+    _length = 5;
   }
   _index = 0;
   add(_request->getSlaveAddress());
@@ -341,6 +342,7 @@ void ModbusResponse::setData(uint16_t dataLength, uint8_t *data) {
   if(_length != dataLength) {
     delete _buffer;
     _buffer = new uint8_t[dataLength];
+    _length = dataLength;
   }
   _index = dataLength;
   memcpy(_buffer, data, dataLength);
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 7d6b584..37b8799 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -240,7 +240,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
         buffer = temp;
       }
       // Gap of at least _interval micro seconds passed without data?
-      if(micros() - _lastMicros <= _interval) {
+      if(micros() - _lastMicros >= _interval) {
         state = DATA_READ;
       }
       break;

From d4a4fe32ba0e0d3f2a56681ea06909e3a68814c8 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Fri, 21 Aug 2020 18:40:22 +0200
Subject: [PATCH 14/25] Set gap detection interval to 16ms to cope with an
 ESP32-Arduino core implementation weakness

---
 src/esp32ModbusRTU.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 37b8799..b74e411 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -240,7 +240,11 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
         buffer = temp;
       }
       // Gap of at least _interval micro seconds passed without data?
-      if(micros() - _lastMicros >= _interval) {
+      // if(micros() - _lastMicros >= _interval) {
+      // The above line is the theory - in practice the FIFO copy process into
+      // the Serial buffer takes much longer than that. In lieu of a better
+      // solution we will use 16ms(!) instead of _interval :(
+      if(micros() - _lastMicros >= 16000) {
         state = DATA_READ;
       }
       break;

From 143ceacf24655459078e14d728ec64e1bc0ba177 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 15:55:49 +0200
Subject: [PATCH 15/25] Clarify FIFO handling issue in ESP32-Arduino core code.

---
 src/esp32ModbusRTU.cpp | 41 +++++++++++++++++++++++++++--------------
 1 file changed, 27 insertions(+), 14 deletions(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index b74e411..b594c53 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -224,27 +224,40 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
     // IN_PACKET: read data until a gap of at least _interval time passed without another byte arriving
     case IN_PACKET:
       // Data waiting and space left in buffer?
-      while (bufferPtr < bufferBlocks * BUFBLOCKSIZE && _serial->available()) {
+      while (_serial->available()) {
         // Yes. Catch the byte
         buffer[bufferPtr++] = _serial->read();
+        // Buffer full?
+        if(bufferPtr >= bufferBlocks * BUFBLOCKSIZE) {
+          // Yes. Extend it by another block
+          bufferBlocks++;
+          uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
+          memcpy(temp, buffer, (bufferBlocks - 1) * BUFBLOCKSIZE);
+          delete buffer;
+          buffer = temp;
+        }
         // Rewind timer
         _lastMicros = micros();
       }
-      // Buffer full?
-      if(bufferPtr >= bufferBlocks * BUFBLOCKSIZE) {
-        // Yes. Extend it by another block
-        bufferBlocks++;
-        uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
-        memcpy(temp, buffer, (bufferBlocks - 1) * BUFBLOCKSIZE);
-        delete buffer;
-        buffer = temp;
-      }
       // Gap of at least _interval micro seconds passed without data?
-      // if(micros() - _lastMicros >= _interval) {
-      // The above line is the theory - in practice the FIFO copy process into
-      // the Serial buffer takes much longer than that. In lieu of a better
-      // solution we will use 16ms(!) instead of _interval :(
+      // ***********************************************
+      // Important notice!
+      // Due to an implementation decision done in the ESP32 Arduino core code,
+      // the correct time to detect a gap of _interval µs is not effective, as
+      // the core FIFO handling takes much longer than that.
+      //
+      // Workaround: uncomment the following line to wait for 16ms(!) for the handling to finish:
       if(micros() - _lastMicros >= 16000) {
+      //
+      // Alternate solution: is to modify the uartEnableInterrupt() function in
+      // the core implementation file 'esp32-hal-uart.c', to have the line
+      //    'uart->dev->conf1.rxfifo_full_thrhd = 1; // 112;'
+      // This will change the number of bytes received to trigger the copy interrupt 
+      // from 112 (as is implemented in the core) to 1, effectively firing the interrupt
+      // for any single byte.
+      // Then you may uncomment the line below instead:
+      // if(micros() - _lastMicros >= _interval) {
+      //
         state = DATA_READ;
       }
       break;

From 3da0a524455d46b895ec63c4ade9a3b508452aa6 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 16:18:20 +0200
Subject: [PATCH 16/25] Fix typo on _interval comment in esp32ModbusRTU.begin()
 and add update of _error in setErrorResponse()

---
 src/ModbusMessage.cpp  | 1 +
 src/esp32ModbusRTU.cpp | 2 +-
 2 files changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index d30dcb8..417a7ca 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -330,6 +330,7 @@ void ModbusResponse::setErrorResponse(uint8_t errorCode) {
     _length = 5;
   }
   _index = 0;
+  _error = static_cast<esp32Modbus::Error>(errorCode);
   add(_request->getSlaveAddress());
   add(_request->getFunctionCode() | 0x80);
   add(errorCode);
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index b594c53..310c569 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -56,7 +56,7 @@ void esp32ModbusRTU::begin(int coreID /* = -1 */) {
   xTaskCreatePinnedToCore((TaskFunction_t)&_handleConnection, "esp32ModbusRTU", 4096, this, 5, &_task, coreID >= 0 ? coreID : NULL);
   // silent interval is at least 3.5x character time
   // _interval = 35000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
-  _interval = 40000000UL / _serial->baudRate();  // 3.5 * 10 bits * 1000 µs * 1000 ms / baud
+  _interval = 40000000UL / _serial->baudRate();  // 4 * 10 bits * 1000 µs * 1000 ms / baud
 
   // The following is okay for sending at any baud rate, but problematic at receiving with baud rates above 35000,
   // since the calculated interval will be below 1000µs!

From 6d216718ea903930fa97feea1882614909b8ab45 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 16:47:06 +0200
Subject: [PATCH 17/25] Fix cpplint grieves

---
 src/ModbusMessage.cpp  | 16 +++++++---------
 src/ModbusMessage.h    |  2 +-
 src/esp32ModbusRTU.cpp | 26 +++++++++++---------------
 3 files changed, 19 insertions(+), 25 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 417a7ca..079d6a2 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -305,26 +305,24 @@ uint8_t ModbusResponse::getFunctionCode() {
 
 uint8_t* ModbusResponse::getData() {
   uint8_t fc = _request->getFunctionCode();
-  if(fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
+  if (fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
     return &_buffer[3];
-  }
-  else {
+  } else {
     return &_buffer[2];
   }
 }
 
 uint8_t ModbusResponse::getByteCount() {
   uint8_t fc = _request->getFunctionCode();
-  if(fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
+  if (fc == 0x01 || fc == 0x02 || fc == 0x03 || fc == 0x04) {
     return _buffer[2];
-  }
-  else {
+  } else {
     return _index - 2;
   }
 }
 
 void ModbusResponse::setErrorResponse(uint8_t errorCode) {
-  if(_length != 5) {
+  if (_length != 5) {
     delete _buffer;
     _buffer = new uint8_t[5];
     _length = 5;
@@ -340,11 +338,11 @@ void ModbusResponse::setErrorResponse(uint8_t errorCode) {
 }
 
 void ModbusResponse::setData(uint16_t dataLength, uint8_t *data) {
-  if(_length != dataLength) {
+  if (_length != dataLength) {
     delete _buffer;
     _buffer = new uint8_t[dataLength];
     _length = dataLength;
   }
   _index = dataLength;
   memcpy(_buffer, data, dataLength);
-}
\ No newline at end of file
+}
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index d890a4a..779b38c 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -98,7 +98,7 @@ class ModbusRequest16 : public ModbusRequest {
 // "raw" request based on a request packet obtained as is.
 class ModbusRequestRaw : public ModbusRequest {
  public:
-  explicit ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token=0);
+  explicit ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, uint16_t dataLength, uint8_t* data, uint32_t token = 0);
 };
 
 class ModbusResponse : public ModbusMessage {
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 310c569..4c35dfa 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -182,7 +182,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
   register uint16_t bufferPtr = 0;
 
   // State machine states
-  enum STATES : uint8_t { WAIT_INTERVAL=0, WAIT_DATA, IN_PACKET, DATA_READ, ERROR_EXIT, FINISHED };
+  enum STATES : uint8_t { WAIT_INTERVAL = 0, WAIT_DATA, IN_PACKET, DATA_READ, ERROR_EXIT, FINISHED };
   register STATES state = WAIT_INTERVAL;
 
   // Timeout tracker
@@ -194,29 +194,25 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
   // Return data object
   ModbusResponse* response = nullptr;
 
-  while(state != FINISHED)
-  {
-    switch(state)
-    {
+  while (state != FINISHED) {
+    switch (state) {
     // WAIT_INTERVAL: spend the remainder of the bus quiet time waiting
     case WAIT_INTERVAL:
       // Time passed?
-      if(micros() - _lastMicros >= _interval) {
+      if (micros() - _lastMicros >= _interval) {
         // Yes, proceed to reading data
         state = WAIT_DATA;
-      }
-      else {
+      } else {
         // No, wait a little longer
         delayMicroseconds(1);
       }
       break;
     // WAIT_DATA: await first data byte, but watch timeout
     case WAIT_DATA:
-      if(_serial->available()) {
+      if (_serial->available()) {
         state = IN_PACKET;
         _lastMicros = micros();
-      }
-      else if(millis() - TimeOut >= TimeOutValue) {
+      } else if(millis() - TimeOut >= TimeOutValue) {
         errorCode = esp32Modbus::TIMEOUT;
         state = ERROR_EXIT;
       }
@@ -228,7 +224,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
         // Yes. Catch the byte
         buffer[bufferPtr++] = _serial->read();
         // Buffer full?
-        if(bufferPtr >= bufferBlocks * BUFBLOCKSIZE) {
+        if (bufferPtr >= bufferBlocks * BUFBLOCKSIZE) {
           // Yes. Extend it by another block
           bufferBlocks++;
           uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
@@ -247,16 +243,16 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       // the core FIFO handling takes much longer than that.
       //
       // Workaround: uncomment the following line to wait for 16ms(!) for the handling to finish:
-      if(micros() - _lastMicros >= 16000) {
+      if (micros() - _lastMicros >= 16000) {
       //
       // Alternate solution: is to modify the uartEnableInterrupt() function in
       // the core implementation file 'esp32-hal-uart.c', to have the line
       //    'uart->dev->conf1.rxfifo_full_thrhd = 1; // 112;'
-      // This will change the number of bytes received to trigger the copy interrupt 
+      // This will change the number of bytes received to trigger the copy interrupt
       // from 112 (as is implemented in the core) to 1, effectively firing the interrupt
       // for any single byte.
       // Then you may uncomment the line below instead:
-      // if(micros() - _lastMicros >= _interval) {
+      // if (micros() - _lastMicros >= _interval) {
       //
         state = DATA_READ;
       }

From ba4b0e9cbca8070a520bca9b45530391f44c7a7f Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 16:49:48 +0200
Subject: [PATCH 18/25] Another blank missing for cpplint...

---
 src/esp32ModbusRTU.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 4c35dfa..9bbb58f 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -212,7 +212,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
       if (_serial->available()) {
         state = IN_PACKET;
         _lastMicros = micros();
-      } else if(millis() - TimeOut >= TimeOutValue) {
+      } else if (millis() - TimeOut >= TimeOutValue) {
         errorCode = esp32Modbus::TIMEOUT;
         state = ERROR_EXIT;
       }

From 256c37c26860b0560e98acf8fe6a505b733e7d1f Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 16:54:24 +0200
Subject: [PATCH 19/25] Use intermediate memory pointer to keep cppcheck happy

---
 src/esp32ModbusRTU.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 9bbb58f..d392809 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -229,8 +229,11 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
           bufferBlocks++;
           uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
           memcpy(temp, buffer, (bufferBlocks - 1) * BUFBLOCKSIZE);
-          delete buffer;
-          buffer = temp;
+          // Use intermediate pointer temp2 to keep cppcheck happy
+          uint8_t temp2 = temp;
+          temp = buffer;
+          buffer = temp2;
+          delete temp;
         }
         // Rewind timer
         _lastMicros = micros();

From 78ffbcf79abf97000efcea7d1eaca6b9564b6999 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 17:04:42 +0200
Subject: [PATCH 20/25] Another attempt to ease cppcheck

---
 src/esp32ModbusRTU.cpp | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index d392809..4d07e76 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -230,10 +230,8 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
           uint8_t *temp = new uint8_t[bufferBlocks * BUFBLOCKSIZE];
           memcpy(temp, buffer, (bufferBlocks - 1) * BUFBLOCKSIZE);
           // Use intermediate pointer temp2 to keep cppcheck happy
-          uint8_t temp2 = temp;
-          temp = buffer;
-          buffer = temp2;
-          delete temp;
+          delete[] buffer;
+          buffer = temp;
         }
         // Rewind timer
         _lastMicros = micros();
@@ -281,7 +279,7 @@ ModbusResponse* esp32ModbusRTU::_receive(ModbusRequest* request) {
   }
 
   // Deallocate buffer
-  delete buffer;
+  delete[] buffer;
   _lastMicros = micros();
 
   return response;

From 60783fb494e58f381dbf4ddc5baefbd11b46fd6a Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 17:12:24 +0200
Subject: [PATCH 21/25] Fix Test cases (removed responseLength())

---
 tests/Test_ModbusMessage.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/tests/Test_ModbusMessage.cpp b/tests/Test_ModbusMessage.cpp
index c582efd..2acca8e 100644
--- a/tests/Test_ModbusMessage.cpp
+++ b/tests/Test_ModbusMessage.cpp
@@ -17,7 +17,7 @@ TEST_CASE("Read input registers", "[FC04]") {
   REQUIRE(request->getSize() == sizeof(stdMessage));
   REQUIRE_THAT(request->getMessage(), ByteArrayEqual(stdMessage, sizeof(stdMessage)));
 
-  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(request->responseLength(), request);
+  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(7, request);
 
   SECTION("normal response") {
     for (uint8_t i = 0; i < sizeof(stdResponse); ++i) {
@@ -48,7 +48,7 @@ TEST_CASE("Write multiple holding registers", "[FC16]") {
   REQUIRE(request->getSize() == sizeof(stdMessage));
   REQUIRE_THAT(request->getMessage(), ByteArrayEqual(stdMessage, sizeof(stdMessage)));
 
-  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(request->responseLength(), request);
+  esp32ModbusRTUInternals::ModbusResponse* response = new esp32ModbusRTUInternals::ModbusResponse(8, request);
 
   SECTION("normal response") {
     for (uint8_t i = 0; i < sizeof(stdResponse); ++i) {

From a1297bbb4e5306dfe8654569c73afcbf7230d61a Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 17:20:00 +0200
Subject: [PATCH 22/25] Update example sketch to accept all function codes

---
 examples/SDM630/SDM630.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/SDM630/SDM630.ino b/examples/SDM630/SDM630.ino
index e43db58..7770efb 100644
--- a/examples/SDM630/SDM630.ino
+++ b/examples/SDM630/SDM630.ino
@@ -18,7 +18,7 @@ void setup() {
   Serial.begin(115200);  // Serial output
   Serial1.begin(9600, SERIAL_8N1, 17, 4, true);  // Modbus connection
 
-  modbus.onData([](uint8_t serverAddress, esp32Modbus::FunctionCode fc, uint16_t address, uint8_t* data, size_t length) {
+  modbus.onData([](uint8_t serverAddress, uint8_t fc, uint16_t address, uint8_t* data, size_t length) {
     Serial.printf("id 0x%02x fc 0x%02x len %u: 0x", serverAddress, fc, length);
     for (size_t i = 0; i < length; ++i) {
       Serial.printf("%02x", data[i]);

From 8e2c32126abc8d92eeb38fc4eff2b398d586e918 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Sat, 22 Aug 2020 17:34:52 +0200
Subject: [PATCH 23/25] Update Readme

---
 README.md | 21 ++++++++++++++++++---
 1 file changed, 18 insertions(+), 3 deletions(-)

diff --git a/README.md b/README.md
index 9f8267a..b479356 100644
--- a/README.md
+++ b/README.md
@@ -12,6 +12,9 @@ This is a non blocking Modbus client (master) for ESP32.
   -  read discrete inputs (02)
   -  read holding registers (03)
   -  read input registers (04)
+  -  write single holding register (06)
+  -  write multiple holding register (16)
+  -  "raw" requests for arbitrary function codes
 -  similar API as my [esp32ModbusTCP](https://github.com/bertmelis/esp32ModbusTCP) implementation
 
 ## Developement status
@@ -64,7 +67,8 @@ is connected via a 100 Ohms resistor to limit possible ground loop currents.
 
 The API is quite lightweight. It takes minimal 3 steps to get going.
 
-First create the ModbusRTU object. The constructor takes two arguments: HardwareSerial object and pin number of DE/RS.
+First create the ModbusRTU object. The constructor takes two arguments: HardwareSerial object and pin number of DE/RS (or -1, if 
+your module does auto half duplex).
 
 ```C++
 esp32ModbusRTU myModbus(&Serial, DE_PIN);
@@ -119,12 +123,23 @@ The request queue holds maximum 20 items. So a 21st request will fail until the
 #define QUEUE_SIZE 20
 ```
 
-The waiting time before a timeout error is returned can also be changed by a `#define` variable:
+The waiting time before a timeout error is returned can also be changed by a method call:
 
 ```C++
-#define TIMEOUT_MS 5000
+myModbus.setTimeOutValue(5000);
 ```
 
+## Caveat
+
+The ESP32 Arduino core implementation of the handling of Serial interfaces has a design decision built in that prevents real time
+Serial communications for data packets received larger than 112 bytes. the underlying FIFO buffer is only copied into the Serial's buffer 
+when 112 bytes have been received. The copy process then takes longer than the MODBUS timeout lasts, so the remainder of the packet is lost. 
+
+The library has a workaround built in that covers the issue by waiting a loooong time (16 milliseconds) until determining a bus timeout (= end
+of packet). This is possible since the library implements a MODBUS master device, thus controlling the bus timing.
+
+But note that this is not according to MODBUS standards.
+
 ## Issues
 
 Please file a Github issue ~~if~~ when you find a bug. You can also use the issue tracker for feature requests.

From 889fe93d8bcc0c7eb11eeb1f2369a0d89aa52ef9 Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Mon, 24 Aug 2020 16:04:49 +0200
Subject: [PATCH 24/25] Remove (now obsolete) address parameter for onData and
 onError handlers

---
 src/ModbusMessage.cpp     | 31 ++++++++++---------------------
 src/ModbusMessage.h       |  2 --
 src/esp32ModbusRTU.cpp    |  2 --
 src/esp32ModbusTypeDefs.h |  8 ++------
 4 files changed, 12 insertions(+), 31 deletions(-)

diff --git a/src/ModbusMessage.cpp b/src/ModbusMessage.cpp
index 079d6a2..543f8c0 100644
--- a/src/ModbusMessage.cpp
+++ b/src/ModbusMessage.cpp
@@ -136,13 +136,8 @@ ModbusRequest::ModbusRequest(uint8_t length) :
   ModbusMessage(length),
   _slaveAddress(0),
   _functionCode(0),
-  _address(0),
   _byteCount(0) {}
 
-uint16_t ModbusRequest::getAddress() {
-  return _address;
-}
-
 uint8_t ModbusRequest::getSlaveAddress() {
   return _slaveAddress;
 }
@@ -155,13 +150,12 @@ ModbusRequest02::ModbusRequest02(uint8_t slaveAddress, uint16_t address, uint16_
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_DISCR_INPUT;
-  _address = address;
   _byteCount = numberCoils / 8 + 1;
   _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberCoils));
   add(low(numberCoils));
   uint16_t CRC = CRC16(_buffer, 6);
@@ -173,13 +167,12 @@ ModbusRequest03::ModbusRequest03(uint8_t slaveAddress, uint16_t address, uint16_
   ModbusRequest(12) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_HOLD_REGISTER;
-  _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
   _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberRegisters));
   add(low(numberRegisters));
   uint16_t CRC = CRC16(_buffer, 6);
@@ -191,13 +184,12 @@ ModbusRequest04::ModbusRequest04(uint8_t slaveAddress, uint16_t address, uint16_
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::READ_INPUT_REGISTER;
-  _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
   _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberRegisters));
   add(low(numberRegisters));
   uint16_t CRC = CRC16(_buffer, 6);
@@ -209,13 +201,12 @@ ModbusRequest06::ModbusRequest06(uint8_t slaveAddress, uint16_t address, uint16_
   ModbusRequest(8) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::WRITE_HOLD_REGISTER;
-  _address = address;
   _byteCount = 2;  // 1 register is 2 bytes wide
   _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(data));
   add(low(data));
   uint16_t CRC = CRC16(_buffer, 6);
@@ -227,13 +218,12 @@ ModbusRequest16::ModbusRequest16(uint8_t slaveAddress, uint16_t address, uint16_
   ModbusRequest(9 + (numberRegisters * 2)) {
   _slaveAddress = slaveAddress;
   _functionCode = esp32Modbus::WRITE_MULT_REGISTERS;
-  _address = address;
   _byteCount = numberRegisters * 2;  // register is 2 bytes wide
   _token = token;
   add(_slaveAddress);
   add(_functionCode);
-  add(high(_address));
-  add(low(_address));
+  add(high(address));
+  add(low(address));
   add(high(numberRegisters));
   add(low(numberRegisters));
   add(_byteCount);
@@ -249,7 +239,6 @@ ModbusRequestRaw::ModbusRequestRaw(uint8_t slaveAddress, uint8_t functionCode, u
   ModbusRequest(dataLength + 4) {
   _slaveAddress = slaveAddress;
   _functionCode = functionCode;
-  _address = 0;
   _byteCount = dataLength + 2;
   _token = token;
   add(_slaveAddress);
diff --git a/src/ModbusMessage.h b/src/ModbusMessage.h
index 779b38c..4f3fae7 100644
--- a/src/ModbusMessage.h
+++ b/src/ModbusMessage.h
@@ -53,7 +53,6 @@ class ModbusResponse;  // forward declare for use in ModbusRequest
 
 class ModbusRequest : public ModbusMessage {
  public:
-  uint16_t getAddress();
   uint8_t getFunctionCode();
   uint8_t getSlaveAddress();
 
@@ -61,7 +60,6 @@ class ModbusRequest : public ModbusMessage {
   explicit ModbusRequest(uint8_t length);
   uint8_t _slaveAddress;
   uint8_t _functionCode;
-  uint16_t _address;
   uint16_t _byteCount;
 };
 
diff --git a/src/esp32ModbusRTU.cpp b/src/esp32ModbusRTU.cpp
index 4d07e76..6b50506 100644
--- a/src/esp32ModbusRTU.cpp
+++ b/src/esp32ModbusRTU.cpp
@@ -132,7 +132,6 @@ void esp32ModbusRTU::_handleConnection(esp32ModbusRTU* instance) {
           instance->_onData(
             response->getSlaveAddress(),
             response->getFunctionCode(),
-            request->getAddress(),
             response->getData(),
             response->getByteCount());
         // else, if the token onData handler is set, call that
@@ -140,7 +139,6 @@ void esp32ModbusRTU::_handleConnection(esp32ModbusRTU* instance) {
           instance->_onDataToken(
             response->getSlaveAddress(),
             response->getFunctionCode(),
-            request->getAddress(),
             response->getData(),
             response->getByteCount(),
             response->getToken());
diff --git a/src/esp32ModbusTypeDefs.h b/src/esp32ModbusTypeDefs.h
index dac4275..97a3542 100644
--- a/src/esp32ModbusTypeDefs.h
+++ b/src/esp32ModbusTypeDefs.h
@@ -60,13 +60,9 @@ enum Error : uint8_t {
   COMM_ERROR            = 0xE4  // general communication error
 };
 
-typedef std::function<void(uint16_t, uint8_t, uint8_t, uint8_t*, uint16_t)> MBTCPOnData;
-typedef std::function<void(uint8_t, uint8_t, uint16_t, uint8_t*, uint16_t)> MBRTUOnData;
-typedef std::function<void(uint16_t, esp32Modbus::Error)> MBTCPOnError;
+typedef std::function<void(uint8_t, uint8_t, uint8_t*, uint16_t)> MBRTUOnData;
 typedef std::function<void(esp32Modbus::Error)> MBRTUOnError;
-typedef std::function<void(uint16_t, uint8_t, uint8_t, uint8_t*, uint16_t, uint32_t)> MBTCPOnDataToken;
-typedef std::function<void(uint8_t, uint8_t, uint16_t, uint8_t*, uint16_t, uint32_t)> MBRTUOnDataToken;
-typedef std::function<void(uint16_t, esp32Modbus::Error, uint32_t)> MBTCPOnErrorToken;
+typedef std::function<void(uint8_t, uint8_t, uint8_t*, uint16_t, uint32_t)> MBRTUOnDataToken;
 typedef std::function<void(esp32Modbus::Error, uint32_t)> MBRTUOnErrorToken;
 
 }  // namespace esp32Modbus

From d9c0758d149194bd35cb62f8b4c0303b542677ea Mon Sep 17 00:00:00 2001
From: Miq1 <miq1@gmx.de>
Date: Mon, 24 Aug 2020 16:18:59 +0200
Subject: [PATCH 25/25] Missed the example sketch when removing the parameter

---
 examples/SDM630/SDM630.ino | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/examples/SDM630/SDM630.ino b/examples/SDM630/SDM630.ino
index 7770efb..be9d76d 100644
--- a/examples/SDM630/SDM630.ino
+++ b/examples/SDM630/SDM630.ino
@@ -18,7 +18,7 @@ void setup() {
   Serial.begin(115200);  // Serial output
   Serial1.begin(9600, SERIAL_8N1, 17, 4, true);  // Modbus connection
 
-  modbus.onData([](uint8_t serverAddress, uint8_t fc, uint16_t address, uint8_t* data, size_t length) {
+  modbus.onData([](uint8_t serverAddress, uint8_t fc, uint8_t* data, size_t length) {
     Serial.printf("id 0x%02x fc 0x%02x len %u: 0x", serverAddress, fc, length);
     for (size_t i = 0; i < length; ++i) {
       Serial.printf("%02x", data[i]);