From 608ec48e471ccba88a5156134b2aa73d992c7dc3 Mon Sep 17 00:00:00 2001
From: Jonny007-MKD <me@jonny007-mkd.de>
Date: Mon, 14 Jul 2014 10:20:47 +0000
Subject: [PATCH] Implemented checks in ModbusWriteBits that the upper bound of
 process image is not exceeded.

---
 .../include/CAPL/include/ModbusClient.cin     | 45 +++++++++++--------
 1 file changed, 26 insertions(+), 19 deletions(-)

diff --git a/Modbus-CAPL/include/CAPL/include/ModbusClient.cin b/Modbus-CAPL/include/CAPL/include/ModbusClient.cin
index 709cd68..6df9024 100644
--- a/Modbus-CAPL/include/CAPL/include/ModbusClient.cin
+++ b/Modbus-CAPL/include/CAPL/include/ModbusClient.cin
@@ -435,38 +435,55 @@ void ModbusWriteBits(word address, long count, byte values[])
 	enum ModbusFuncCode funcCode = WriteBits;
 	byte buffer[maxLength];
 	struct ModbusReqWriteBits mbreq;
-	word curCount;
+	word overallLength, curCount, addressO;
 	byte dataLength;
-	word overallLength;
 	word i;
 	long offset;
+	word devStartAddr, devEndAddr;
+
+	devStartAddr = funcCode == thisDev.Addr.Write.OutputBits;		// The start address of the bits
+	devEndAddr = devStartAddr + thisDev.MaxBitCount;				// The address behind the last bit
+	if (address < devStartAddr)										// Oh, reading at the wrong address?
+	{
+		writeDbg(MbError, "ModbusWriteBits: The given start address 0x%04X is smaller than the obligatory start address 0x%04X", address, devStartAddr);
+		OnModbusClientPanics(AddressFailure);
+		return;
+	}
 
 	// FC15: Write Multiple Bits (DOs)
 	offset = 0;
-	while (count > 0)
+	while (count > 0 && (address + offset) < devEndAddr)
 	{
-		if (count > gMaxBitsPerWrite)
+		addressO = address + offset;
+		if (count > gMaxBitsPerWrite)										// divide packets that are too large
 		{
 			curCount = gMaxBitsPerWrite;
 			dataLength = gMaxBitsPerWrite/8;
 			overallLength = maxLength;
 		}
-		else
+		else																// Bits fit in one packet
 		{
 			curCount = count;
 			dataLength = _ceil(curCount / 8.0);
 			overallLength = maxLength - gMaxBitsPerWrite/8 + dataLength;
 		}
+		if (addressO + curCount > devEndAddr)								// upper bound in process image
+		{
+			writeDbg(MbWarning, "ModbusWriteBits: Impossible to write %d bits at 0x%04X. Changed count to %d.", curCount, addressO, devEndAddr - addressO);
+			curCount = devEndAddr - addressO;
+			dataLength = _ceil(curCount / 8.0);
+			overallLength = maxLength - gMaxBitsPerWrite/8 + dataLength;
+		}
 
 		_ModbusMakeHeader(mbreq.Header, overallLength, funcCode);
 
-		mbreq.Address = address+offset;			// [2] Output address
+		mbreq.Address = addressO;				// [2] Output address
 		mbreq.Count = curCount;					// [2] Number of items; 1:max 1968=0x7B0
 		mbreq.ByteCount = dataLength;			// [1] Number of bytes; = ceil(count/8)
 		for (i = 0; i < dataLength; i++)		// [264] Byte status, 8 per byte
-			mbreq.Data[i] = values[i+offset/8];
+			mbreq.Data[i] = values[i + offset/8];
 
-		writeDbg(MbDebug, "Sending 'Write Bits' (0x0F) command. TxID: 0x%04X, Addr: 0x%04X, Count: %d", mbreq.Header.TxID, address+offset, curCount);
+		writeDbg(MbDebug, "Sending 'Write Bits' (0x0F) command. TxID: 0x%04X, Addr: 0x%04X, Count: %d", mbreq.Header.TxID, addressO, curCount);
 
 		memcpy_h2n(buffer, mbreq);
 		_ModbusSend(buffer, overallLength, mbreq.Header.TxID);
@@ -877,10 +894,6 @@ void _OnModbusReceive2Exceptions(byte exCode, struct ModbusApHeader mbap)
 		case 0x80+ReadWriteRegisters:
 			_OnModbusReceiveConfirmRegistersException(mbap, ex);
 			break;
-		default:
-			writeDbg(MbError, "_OnModbusReceive2Exceptions: Got incorrect exception function code 0x%02X!", mbap.FuncCode);
-			OnModbusClientPanics(FuncCodeIncorrect);
-			break;
 	}
 }
 
@@ -923,9 +936,7 @@ void _OnModbusReceive2Success(byte buffer[], struct ModbusApHeader mbap, int off
 			_OnModbusReceiveConfirmRegisters(mbuffer);
 			break;
 		default:
-			writeDbg(MbError, "_OnModbusReceive2Success: Received unexpected function code 0x%02X!", mbap.FuncCode);
-			OnModbusClientPanics(FuncCodeIncorrect);
-			break;
+			writeDbg(MbError, "OnModbusReceive2Success: We received funcCode 0x%X!?", mbap.FuncCode);
 	}
 }
 
@@ -1058,9 +1069,5 @@ void _ModbusSendTimerError(byte buffer[], enum ModbusRequestError reqError)
 		case ReadWriteRegisters:
 			OnModbusReadWriteRegistersFailed(reqError, None, mbap);
 			break;
-		default:
-			writeDbg(MbError, "_ModbusSendTimerError: Unexpected function code 0x%02X!", mbap.FuncCode);
-			OnModbusClientPanics(FuncCodeIncorrect);
-			break;
 	}
 }
\ No newline at end of file