Server: Fix callback called twice on register read

Author: emelianov

README.md

@@ -63,6 +63,8 @@ V1.02](http://www.modbus.org/docs/Modbus_over_serial_line_V1_02.pdf)
 ## Last Changes
 
 ```diff
+// 3.0.6
++ Slave/Server: Fix callback called twice on register read
 // 3.0.5
 + ModbusRTU: Fix early bus release if no callback
 + ModbusRTU ESP32: Fix potential responce loss

library.properties

@@ -1,5 +1,5 @@
 name=modbus-esp8266
-version=3.0.5
+version=3.0.6
 author=Andre Sarmento Barbosa, Alexander Emelianov
 maintainer=Alexander Emelianov<a.m.emelianov@gmail.com>
 sentence=Modbus RTU and Modbus TCP Library for ESP8266/ESP32

src/Modbus.cpp

@@ -224,7 +224,7 @@ void Modbus::getMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numregs
 
 void Modbus::getMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs) {
     for (uint8_t i = 0; i < numregs; i++) {
-        frame[i] = __bswap_16(Reg(startreg + i));
+        frame[i] = __swap_16(Reg(startreg + i));
     }
 }
 
@@ -298,7 +298,7 @@ void Modbus::setMultipleBits(uint8_t* frame, TAddress startreg, uint16_t numoutp
 
 void Modbus::setMultipleWords(uint16_t* frame, TAddress startreg, uint16_t numregs) {
     for (uint8_t i = 0; i < numregs; i++) {
-        Reg(startreg + i, __bswap_16(frame[i]));
+        Reg(startreg + i, __swap_16(frame[i]));
     }
 }
 
@@ -405,7 +405,7 @@ bool Modbus::writeSlaveWords(TAddress startreg, uint16_t to, uint16_t numregs, F
         if (data) {
             uint16_t* frame = (uint16_t*)(_frame + 6);
             for (uint8_t i = 0; i < numregs; i++) {
-                frame[i] = __bswap_16(data[i]);
+                frame[i] = __swap_16(data[i]);
             }
         } else {
             getMultipleWords((uint16_t*)(_frame + 6), startreg, numregs);
@@ -469,7 +469,7 @@ void Modbus::masterPDU(uint8_t* frame, uint8_t* sourceFrame, TAddress startreg,
             if (output) {
                 frame += 2;
                 while(field2) {
-                    *((uint16_t*)output) = __bswap_16(*((uint16_t*)frame));
+                    *((uint16_t*)output) = __swap_16(*((uint16_t*)frame));
                     frame += 2;
                     output += 2;
                     field2--;

src/Modbus.h

@@ -12,10 +12,7 @@
  #include <byteswap.h>
 #endif
 
-#ifndef __bswap_16
- #define __bswap_16(num) (((uint16_t)num>>8) | ((uint16_t)num<<8))
-#endif
-
+static inline uint16_t __swap_16(uint16_t num) { return (num >> 8) | (num << 8); }
 
 #define MB_GLOBAL_REGS
 //#define MB_MAX_REGS     32

src/ModbusIP_ESP8266.cpp

@@ -81,12 +81,12 @@ void ModbusIP::task() {
 		while (millis() - readStart < MODBUSIP_MAX_READMS &&  tcpclient[n]->available() > sizeof(_MBAP)) {
 			tcpclient[n]->readBytes(_MBAP.raw, sizeof(_MBAP.raw));	// Get MBAP
 
-			if (__bswap_16(_MBAP.protocolId) != 0) {   // Check if MODBUSIP packet. __bswap is usless there.
+			if (__swap_16(_MBAP.protocolId) != 0) {   // Check if MODBUSIP packet. __bswap is usless there.
 				while (tcpclient[n]->available())	// Drop all incoming if wrong packet
 					tcpclient[n]->read();
 					continue;
 			}
-			_len = __bswap_16(_MBAP.length);
+			_len = __swap_16(_MBAP.length);
 			_len--; // Do not count with last byte from MBAP
 			if (_len > MODBUSIP_MAXFRAME) {	// Length is over MODBUSIP_MAXFRAME
 				exceptionResponse((FunctionCode)tcpclient[n]->read(), EX_SLAVE_FAILURE);
@@ -112,7 +112,7 @@ void ModbusIP::task() {
 						} else {
 							// Process reply to master request
 							_reply = EX_SUCCESS;
-							TTransaction* trans = searchTransaction(__bswap_16(_MBAP.transactionId));
+							TTransaction* trans = searchTransaction(__swap_16(_MBAP.transactionId));
 							if (trans) { // if valid transaction id
 								if ((_frame[0] & 0x7F) == trans->_frame[0]) { // Check if function code the same as requested
 									// Procass incoming frame as master
@@ -135,7 +135,7 @@ void ModbusIP::task() {
 			}
 			if (tcpclient[n]->localPort() != serverPort) _reply = REPLY_OFF;	// No replay if it was responce to master
 			if (_reply != REPLY_OFF) {
-				_MBAP.length = __bswap_16(_len+1);     // _len+1 for last byte from MBAP					
+				_MBAP.length = __swap_16(_len+1);     // _len+1 for last byte from MBAP					
 				size_t send_len = (uint16_t)_len + sizeof(_MBAP.raw);
 				uint8_t sbuf[send_len];				
 				memcpy(sbuf, _MBAP.raw, sizeof(_MBAP.raw));
@@ -164,9 +164,9 @@ uint16_t ModbusIP::send(IPAddress ip, TAddress startreg, cbTransaction cb, uint8
 		return autoConnectMode?connect(ip):false;
 	transactionId++;
 	if (!transactionId) transactionId = 1;
-	_MBAP.transactionId	= __bswap_16(transactionId);
-	_MBAP.protocolId	= __bswap_16(0);
-	_MBAP.length		= __bswap_16(_len+1);     //_len+1 for last byte from MBAP
+	_MBAP.transactionId	= __swap_16(transactionId);
+	_MBAP.protocolId	= __swap_16(0);
+	_MBAP.length		= __swap_16(_len+1);     //_len+1 for last byte from MBAP
 	_MBAP.unitId		= unit;
 	size_t send_len = _len + sizeof(_MBAP.raw);
 	uint8_t sbuf[send_len];