From 7704769468fecc73aa6fa62921b8f034ed0df55e Mon Sep 17 00:00:00 2001
From: Jonny007-MKD <me@jonny007-mkd.de>
Date: Tue, 15 Jul 2014 12:42:05 +0000
Subject: [PATCH] ModbusClient.cin   Fix some issues with sending and receiving
 packets

ModbusTcp.cin
  Don't assume the connection is ready when TcpOpen says so. Wait for OnTcpOpen() to confirm
---
 .../include/CAPL/include/ModbusClient.cin     | 25 +++++++++++++------
 .../include/CAPL/include/ModbusTcp.cin        | 10 +++-----
 2 files changed, 21 insertions(+), 14 deletions(-)

diff --git a/Modbus-CAPL/include/CAPL/include/ModbusClient.cin b/Modbus-CAPL/include/CAPL/include/ModbusClient.cin
index 4f614da..9694cf2 100644
--- a/Modbus-CAPL/include/CAPL/include/ModbusClient.cin
+++ b/Modbus-CAPL/include/CAPL/include/ModbusClient.cin
@@ -586,7 +586,7 @@ void ModbusWriteRegisters(word address, long count, word values[])
 
 	// FC16: Write Multiple Registers (AOs)
 	offset = 0;
-	while (count > 0 && (address + offset) < devEndAddr)
+	while (count > 0 && (address + offset) > devEndAddr)
 	{
 		addressO = address + offset;
 		curCount = count > gMaxRegsPerWrite ? gMaxRegsPerWrite : count;
@@ -786,6 +786,15 @@ void _OnModbusReceive(dword socket, long result, dword address, dword port, byte
 			// Size of zero indicates that the socket was closed by the communication peer.
 			writeDbg(ConnWarning, "OnModbusReceive: Socket closed by peer");
 			_ModbusDisconnect();
+			
+			for (long TxID : gQueueSent)
+			{
+				writeDbg(ConnWarning, "OnModbusReceive: Moving 0x%04X back to pending queue", TxID);
+				memcpy(gQueuePending[TxID], gQueueSent[TxID]);	// Move back to pending queue
+				gQueueSent.Remove(TxID);
+				gQueuePending[TxID].Timeouts = 0;
+				gQueuePending[TxID].TimeoutTicks = 0;
+			}
 		}
 		else
 		{
@@ -975,8 +984,7 @@ void _ModbusSend(byte buffer[], word length, word TxID)
 	memcpy(gQueuePending[TxID], qe);
 	writeDbg(ConnDebug, "Appended packet 0x%04X to pending queue", TxID);
 
-	if (gQueuePending.Size() == 1 && gQueueSent.Size() == 0 && gSocketState == OK)	// start timer if connection established
-		_ModbusStartQueue();
+	_ModbusStartQueue();
 }
 
 // This method will start the timer. Nothing special :)
@@ -984,8 +992,11 @@ void _ModbusSend(byte buffer[], word length, word TxID)
 // It gets called by _ModbusSend() and OnEthReceivePacket() in ModbusEil.cin
 void _ModbusStartQueue()
 {
-	writeDbg(ConnDebug, "Starting Timer gtModbusRobin");
-	setTimerCyclic(gtModbusRobin, 1);
+	if (gSocketState >= CLOSED && (gQueuePending.Size() > 0 || gQueueSent.Size() > 0))
+	{
+		writeDbg(ConnDebug, "Starting Timer gtModbusRobin");
+		setTimerCyclic(gtModbusRobin, 1);
+	}
 }
 
 /// <-ModbusSend>
@@ -1047,9 +1058,9 @@ on timer gtModbusRobin
 	}
 
 	// Stop timer to reduce load and latency of first new packet
-	if (gSocketState == ERROR || gQueueSent.Size() == 0 && gQueuePending.Size() == 0)
+	if (gSocketState != OK || gQueueSent.Size() == 0 && gQueuePending.Size() == 0)
 	{
-		writeDbg(ConnDebug, "gtModbusRobin: Stopping Timer");
+		writeDbg(ConnDebug, "gtModbusRobin: Stopping Timer. Queue Sent: %d, Queue Pending: %d", gQueueSent.Size(), gQueuePending.Size());
 		this.Cancel();
 	}
 }
diff --git a/Modbus-CAPL/include/CAPL/include/ModbusTcp.cin b/Modbus-CAPL/include/CAPL/include/ModbusTcp.cin
index 56710bd..8c7b373 100644
--- a/Modbus-CAPL/include/CAPL/include/ModbusTcp.cin
+++ b/Modbus-CAPL/include/CAPL/include/ModbusTcp.cin
@@ -105,12 +105,8 @@ word _ModbusConnectTo(dword remoteIp, word remotePort)
 		}
 		return 0;
 	}
-	else
-	{
-		writeDbg(ConnInfo, "_ModbusConnectTo: Successfully connected to server");
-		gSocketState = OK;
-		return 0;
-	}
+	gSocketState = CONNECTING;		// Don't set state OK because the Stack doesn't tell us WSAEWOULDBLOCK
+	return 0;
 }
 
 // This method will be called when TcpSocket.Connect() had to defer the result (and returned WSAEWOULDBLOCK).
@@ -208,7 +204,6 @@ word _ModbusSnd(byte buffer[], word length)
 	}
 
 	bin_to_strhex(buffer, str);
-	writeDbg(ConnDebug, "_ModbusSnd: %s (Länge: %d)", str, length);
 	
 	if (gSocket.Send(buffer, length) != 0)
 	{
@@ -223,6 +218,7 @@ word _ModbusSnd(byte buffer[], word length)
 		}
 		// else: tough luck!
 	}
+	writeDbg(ConnDebug, "_ModbusSnd: %s (Länge: %d)", str, length);
 	return 0;
 }