Jonny007-MKD
7704769468
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
1103 lines
40 KiB
Plaintext
1103 lines
40 KiB
Plaintext
/*@!Encoding:1252*/
|
|
|
|
// This is the main file providing functions for talking to Modbus servers.
|
|
// Additionally include a network layer: ModbusUdp.cin, ModbusTcp.cin or ModbusEil.cin
|
|
|
|
/// This layer provides functions to send Modbus requests, enqueues these messages and watch for timeouts. When responses are received, the corresponding callback method will be called
|
|
/// There are three queues available: Pending, Sent & Ack. Only the latter shall be used by the higher layer. Pending telegrams are waiting to be sent, sent telegrams are waiting to be responded and acknowledged telegrams were already responded and can now be processed further.
|
|
/// The Modbus telegrams are distinguished in the queues by their TxID
|
|
|
|
/// It provides following methods
|
|
/// - ModbusInit() Prepare anything that Modbus works (open connection etc.)
|
|
/// - ModbusEnd() Client up everything (close connection etc.)
|
|
|
|
/// - ModbusReadOutBits() Modbus Function Code: 0x01 (Discrete inputs)
|
|
/// - ModbusReadInBits() Modbus Function Code: 0x02 (Coils)
|
|
/// --> results in OnModbusReadBitsSuccess() or OnModbusReadBitsFailed() (have to be implemented by you)
|
|
|
|
/// - ModbusReadOutRegisters() Modbus Function Code: 0x03 (Input registers)
|
|
/// - ModbusReadInRegisters() Modbus Function Code: 0x04 (Holding registers)
|
|
/// --> results in OnModbusReadRegistersSuccess() or OnModbusReadRegistersFailed() (have to be implemented by you)
|
|
|
|
/// - ModbusWriteBit() Modbus Function Code: 0x05 (Coils)
|
|
/// --> results in OnModbusWriteBitSuccess() or OnModbusWriteBitFailed() (have to be implemented by you)
|
|
|
|
/// - ModbusWriteRegister() Modbus Function Code: 0x06 (Holding registers)
|
|
/// --> results in OnModbusWriteRegisterSuccess() or OnModbusWriteRegisterFailed() (have to be implemented by you)
|
|
|
|
/// - ModbusWriteBits() Modbus Function Code: 0x0F (Coils)
|
|
/// - ModbusWriteBitsB() Wrapper for ModbusWriteBits: Encodes a bit array with size N to an array with size N/8
|
|
/// --> results in OnModbusWriteBitsSuccess() or OnModbusWriteBitsFailed() (have to be implemented by you)
|
|
|
|
/// - ModbusWriteRegisters() Modbus Function Code: 0x10 (Holding registers)
|
|
/// - ModbusReadWriteRegisters() Modbus Function Code: 0x17 (Holding registers)
|
|
/// --> results in OnModbusWriteRegistersSuccess() or OnModbusWriteRegistersFailed() (have to be implemented by you)
|
|
|
|
/// - ModbusWriteMasks() Modbus Function Code: 0x10 (Holding registers)
|
|
/// --> results in OnModbusWriteMasksSuccess() or OnModbusWriteMasksFailed() (have to be implemented by you)
|
|
|
|
includes
|
|
{
|
|
#include "ModbusStructs.cin"
|
|
}
|
|
|
|
variables
|
|
{
|
|
msTimer gtModbusRobin; // Timer that sends the packets and watches for timeouts
|
|
word gTxID = 0x0000; // Transaction Identifier for Modbus. Used as index for gQueue
|
|
word gRequestTimeout; // Timeout of a packet [ms]
|
|
byte gMaxTransmissionCount; // Maximum number of transmissions (after timeouts)
|
|
|
|
// Global storage for pending and sent requests, associated by TxID
|
|
struct QueueElement
|
|
{
|
|
word TimeoutTicks; // Time counter [ms]. Used to watch for timeouts (see gRequestTimeout)
|
|
byte Timeouts;
|
|
byte SendTries;
|
|
word Length;
|
|
byte Buffer[gModbusMaxTelegramSize];
|
|
};
|
|
struct QueueElement gQueuePending[long, 2];
|
|
struct QueueElement gQueueSent[long, 2];
|
|
struct QueueElement gQueueAck[long, 2];
|
|
|
|
char ModbusExceptions[11][72] = {
|
|
"Illegal func code (0x01). The function code is unknown by the server",
|
|
"Illegal data address (0x02). Please check your configuration",
|
|
"Illegal data value (0x03)",
|
|
"Server failure (0x04). The server failed during execution",
|
|
"Acknowledge (0x05). The server needs more time to generate the response",
|
|
"Server busy (0x06). The request could not be accepted",
|
|
"",
|
|
"",
|
|
"",
|
|
"Gateway problem (0x0A). Gateway paths not available",
|
|
"Gateway problem (0x0B). The targeted device failed to respond"
|
|
};
|
|
}
|
|
|
|
// This method prepares anything that sending Modbus requests works. Currently this is only the connection.
|
|
// It has to be called by the user/modbus client at the beginning with IP and Port of the Modbus server,
|
|
// the timeout value and the number of retries that shall be performed after a timeout
|
|
void ModbusInit(char Remote_IP[], word remotePort, word requestTimeout, byte maxTransmissions)
|
|
{
|
|
_ModbusConnectTo(Remote_IP, remotePort);
|
|
gRequestTimeout = requestTimeout;
|
|
gMaxTransmissionCount = maxTransmissions;
|
|
}
|
|
|
|
// This method cleans up everything: Closes the connection, clears the queues and stops the timer
|
|
// It has to be called by the user/modbus client at the end of the measurement
|
|
void ModbusEnd()
|
|
{
|
|
_ModbusDisconnect();
|
|
gQueuePending.Clear();
|
|
gQueueSent.Clear();
|
|
gtModbusRobin.Cancel();
|
|
}
|
|
|
|
// This method fills the ModbusApHeader structure 'mbap'.
|
|
// It gets called by the Modbus request methods
|
|
void _ModbusMakeHeader(struct ModbusApHeader mbap, word length, enum ModbusFuncCode funcCode)
|
|
{
|
|
mbap.TxID = gTxID++; // [2] Transaction ID
|
|
mbap.Protocol = 0x0000; // [2] Protocol ID = 0 = Modbus
|
|
mbap.Length = length - __offset_of(struct ModbusApHeader, UnitID); // [2] Length; Number of bytes following
|
|
mbap.UnitID = 0xFF; // [1] Unit identifier; not relevant
|
|
mbap.FuncCode = funcCode; // [1] Function Code
|
|
}
|
|
|
|
|
|
|
|
// REGION: ModbusReadBits -------------------------------------------------------------
|
|
/// <ModbusReadBits>
|
|
// This method will submit a request to the Modbus server to read discrete inputs
|
|
void ModbusReadInBits(word address, long count)
|
|
{
|
|
_ModbusReadBits(ReadBitsIn, address, count);
|
|
}
|
|
/// <ModbusReadBits>
|
|
// This method will submit a request to the Modbus server to read discrete inputs
|
|
void ModbusReadBits(word address, long count)
|
|
{
|
|
_ModbusReadBits(ReadBitsIn, address, count);
|
|
}
|
|
/// <ModbusReadBits>
|
|
// This method will submit a request to the Modbus server to read coils
|
|
void ModbusReadOutBits(word address, long count)
|
|
{
|
|
_ModbusReadBits(ReadBitsOut, address, count);
|
|
}
|
|
/// <ModbusReadBits>
|
|
// This method will submit a request to the Modbus server to read discrete inputs or coils
|
|
void _ModbusReadBits(enum ModbusFuncCode funcCode, word address, long count)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
byte buffer[length];
|
|
word curCount;
|
|
struct ModbusReqRead mbreq;
|
|
word devStartAddr, devEndAddr;
|
|
|
|
// Check the function code. If this method was private we would not need this
|
|
switch (funcCode)
|
|
{
|
|
case ReadBitsOut:
|
|
case ReadBitsIn:
|
|
break;
|
|
default:
|
|
writeDbg(MbError, "_ModbusReadBits: Got incorrect function code 0x%02X!", funcCode);
|
|
OnModbusClientPanics(FuncCodeIncorrect);
|
|
return;
|
|
}
|
|
|
|
devStartAddr = funcCode == ReadBitsOut ? thisDev.Addr.Read.OutputBits : thisDev.Addr.Read.InputBits; // 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, "_ModbusReadBits: The given start address 0x%04X is smaller than the obligatory start address 0x%04X", address, devStartAddr);
|
|
OnModbusClientPanics(AddressFailure);
|
|
return;
|
|
}
|
|
|
|
// FC1: Read Coils (DO), FC2: Read Discret Inputs (DI)
|
|
while (count > 0 && address < devEndAddr)
|
|
{
|
|
curCount = count > gMaxBitsPerRead ? gMaxBitsPerRead : count; // divide packets that are too large
|
|
if (address + curCount > devEndAddr) // upper bound in process image
|
|
{
|
|
writeDbg(MbWarning, "_ModbusReadBits: Impossible to read %d bits at 0x%04X. Changed count to %d.", curCount, address, devEndAddr - address);
|
|
curCount = devEndAddr - address;
|
|
}
|
|
|
|
_ModbusMakeHeader(mbreq.Header, length, funcCode);
|
|
|
|
mbreq.Address = address; // [2] Start address
|
|
mbreq.Count = curCount; // [2] Number of items; 1:max 2000=0x7D0
|
|
|
|
writeDbg(MbDebug, "Sending 'Read Bits' (0x01) command. TxID: 0x%04X, Addr: 0x%04X, Count: %d", mbreq.Header.TxID, address, curCount);
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, length, mbreq.Header.TxID);
|
|
|
|
count -= gMaxBitsPerRead;
|
|
address += gMaxBitsPerRead;
|
|
}
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when receiving data
|
|
void _OnModbusReceiveBits(byte buffer[])
|
|
{
|
|
struct ModbusResReceiveBits mbres;
|
|
struct ModbusReqRead mbreq;
|
|
byte bitStatus[gMaxBitsPerRead];
|
|
word numBits;
|
|
byte i, j;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);
|
|
|
|
writeDbg(MbInfo, "Received %d bits from 0x%04X", mbreq.Count, mbreq.Address);
|
|
for (i = 0; i < mbres.ByteCount; i++)
|
|
{
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
bitStatus[8*i+j] = (mbres.Data[i] >> j) & 0x01;
|
|
}
|
|
}
|
|
|
|
OnModbusReadBitsSuccess(mbres, bitStatus, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusReceiveBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while reading bits: %s", ModbusExceptions[ex-1]);
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusReadRegisters -------------------------------------------------------
|
|
/// <ModbusReadRegisters>
|
|
// This method will submit a request to the Modbus server to read input registers
|
|
void ModbusReadInRegisters(word address, long count)
|
|
{
|
|
_ModbusReadRegisters(ReadRegistersIn, address, count);
|
|
}
|
|
/// <ModbusReadRegisters>
|
|
// This method will submit a request to the Modbus server to read holding registers
|
|
void ModbusReadRegisters(word address, long count)
|
|
{
|
|
_ModbusReadRegisters(ReadRegistersIn, address, count);
|
|
}
|
|
/// <ModbusReadRegisters>
|
|
// This method will submit a request to the Modbus server to read holding registers
|
|
void ModbusReadOutRegisters(word address, long count)
|
|
{
|
|
_ModbusReadRegisters(ReadRegistersOut, address, count);
|
|
}
|
|
/// <ModbusReadRegisters>
|
|
// This method will submit a request to the Modbus server to read input or holding registers
|
|
void _ModbusReadRegisters(enum ModbusFuncCode funcCode, word address, long count)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
byte buffer[length];
|
|
word curCount;
|
|
struct ModbusReqRead mbreq;
|
|
word devStartAddr, devEndAddr; // Check the function code. If this method was private we would not need this
|
|
|
|
switch (funcCode)
|
|
{
|
|
case ReadRegistersOut:
|
|
case ReadRegistersIn:
|
|
break;
|
|
default:
|
|
writeDbg(MbError, "_ModbusReadRegisters: Got incorrect function code 0x%02X!", funcCode);
|
|
OnModbusClientPanics(FuncCodeIncorrect);
|
|
return;
|
|
}
|
|
|
|
devStartAddr = funcCode == ReadRegistersOut ? thisDev.Addr.Read.OutputRegisters : thisDev.Addr.Read.InputRegisters; // The start address of the bits
|
|
devEndAddr = devStartAddr + thisDev.MaxRegisterCount; // The address behind the last register
|
|
if (address >= devEndAddr)
|
|
devEndAddr = 0xFFFF; // Some other address. We might be reading extra registers (no input)
|
|
|
|
if (address < devStartAddr) // Oh, reading at the wrong address?
|
|
{
|
|
writeDbg(MbError, "_ModbusReadRegisters: The given start address 0x%04X is smaller than the obligatory start address 0x%04X", address, devStartAddr);
|
|
OnModbusClientPanics(AddressFailure);
|
|
return;
|
|
}
|
|
// FC3: Read Holding Registers (AO), FC4: Read Input Registers (AI)
|
|
while (count > 0 && address < devEndAddr)
|
|
{
|
|
curCount = count > gMaxRegsPerRead ? gMaxRegsPerRead : count; // divide packets that are too large
|
|
if (address + curCount > devEndAddr) // upper bound in process image
|
|
{
|
|
writeDbg(MbWarning, "_ModbusReadBits: Impossible to read %d bits at 0x%04X. Changed count to %d.", curCount, address, devEndAddr - address);
|
|
curCount = devEndAddr - address;
|
|
}
|
|
//write("address = 0x%04X, count = %d, curCount = %d, devStartAddr = 0x%04X, devEndAddr = 0x%04X", address, count, curCount, devStartAddr, devEndAddr);
|
|
|
|
_ModbusMakeHeader(mbreq.Header, length, funcCode);
|
|
|
|
mbreq.Address = address; // [2] Start address
|
|
mbreq.Count = curCount; // [2] Number of items; 1:max 125=0x7D
|
|
|
|
writeDbg(MbDebug, "Sending 'Read Registers' (0x03) command. TxID: 0x%04X, Addr: 0x%04X, Count: %d", mbreq.Header.TxID, address, curCount);
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, length, mbreq.Header.TxID);
|
|
|
|
count -= gMaxRegsPerRead;
|
|
address += gMaxRegsPerRead;
|
|
}
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when receiving data
|
|
void _OnModbusReceiveRegisters(byte buffer[])
|
|
{
|
|
struct ModbusResReceiveRegisters mbres;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);
|
|
|
|
writeDbg(MbInfo, "Received %d registers from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
OnModbusReadRegistersSuccess(mbres, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusReceiveRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while reading registers: %s", ModbusExceptions[ex-1]);
|
|
OnModbusReadRegistersFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteBit -------------------------------------------------------------
|
|
/// <ModbusWriteBit>
|
|
// This method will submit a request to the Modbus server to set a coil
|
|
void ModbusWriteBit(word address, byte value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
enum ModbusFuncCode funcCode = WriteBit;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteSingle mbreq;
|
|
|
|
if (value >= 1)
|
|
value = 0xFF;
|
|
|
|
// FC5: Write Single Coil (DO)
|
|
_ModbusMakeHeader(mbreq.Header, length, funcCode);
|
|
|
|
mbreq.Address = address; // [2] Output address
|
|
mbreq.Value = value << 8; // [2] Output value (0x0000: Off, 0xFF00: On)
|
|
|
|
writeDbg(Debug, "Sending 'Write Bit' (0x05) command. TxID: 0x%04X, Addr: 0x%04X, Value: %d", mbreq.Header.TxID, address, value);
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, length, mbreq.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when confirming the request
|
|
void _OnModbusConfirmBit(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbInfo, "Set bit at 0x%04X to %d", mbc.Address, mbc.Value);
|
|
|
|
OnModbusWriteBitSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusConfirmBitException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing bit: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteBitFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteRegister ------------------------------------------------------
|
|
/// <ModbusWriteRegister>
|
|
// This method will submit a request to the Modbus server to a holding register
|
|
void ModbusWriteRegister(word address, word value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
enum ModbusFuncCode funcCode = WriteRegister;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteSingle mbreq;
|
|
|
|
// 5: Write Single Register (AO)
|
|
_ModbusMakeHeader(mbreq.Header, length, funcCode);
|
|
|
|
mbreq.Address = address; // [2] Output address
|
|
mbreq.Value = value; // [2] Output value
|
|
|
|
writeDbg(MbDebug, "Sending 'Write Register' (0x06) command. TxID: 0x%04X, Addr: 0x%04X, Value: %d", mbreq.Header.TxID, address, value);
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, length, mbreq.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when confirming the request
|
|
void _OnModbusConfirmRegister(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbInfo, "Set register at 0x%04X to %d", mbc.Address, mbc.Value);
|
|
|
|
OnModbusWriteRegisterSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusConfirmRegisterException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing register: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteRegisterFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteBits ----------------------------------------------------------
|
|
/// <ModbusWriteBits>
|
|
// This method will submit a request to the Modbus server to set several coils
|
|
// It will not encode the 'values'
|
|
void ModbusWriteBits(word address, long count, byte values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteBits);
|
|
enum ModbusFuncCode funcCode = WriteBits;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteBits mbreq;
|
|
word overallLength, curCount, addressO;
|
|
byte dataLength;
|
|
word i;
|
|
long offset;
|
|
word devStartAddr, devEndAddr;
|
|
|
|
devStartAddr = 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 && (address + offset) < devEndAddr)
|
|
{
|
|
addressO = address + offset;
|
|
if (count > gMaxBitsPerWrite) // divide packets that are too large
|
|
{
|
|
curCount = gMaxBitsPerWrite;
|
|
dataLength = gMaxBitsPerWrite/8;
|
|
overallLength = maxLength;
|
|
}
|
|
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 = 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];
|
|
|
|
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);
|
|
|
|
count -= gMaxBitsPerWrite;
|
|
offset += gMaxBitsPerWrite;
|
|
}
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
// This method will submit a request to the Modbus server to set several coils
|
|
// It additionally encodes the bit 'values' to bytes. This means that 8 bytes of 'values' are stacked into one byte.
|
|
// Use this when you simply have 1s and 0s
|
|
void ModbusWriteBitsB(word address, long count, byte values[])
|
|
{
|
|
byte buffer[0x4000/8]; // Maximum value from B&R devices. *sigh*
|
|
long length;
|
|
dword ellCount;
|
|
dword i;
|
|
byte j;
|
|
char str1[20*2], str2[20*3];
|
|
|
|
length = (long)_floor(count / 8.0);
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: count: %d; length: %d", count, length+1);
|
|
|
|
for (i = 0; i < length; i++)
|
|
{
|
|
buffer[i] = 0;
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
buffer[i] |= (values[i*8 + j] & 0x01) << j;
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: j: %d; indx: %d; value: %d; mask: %X", j, i*8 + j, values[i*8 + j], (0x01 << j));
|
|
}
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: Byte %d = %X", i, buffer[i]);
|
|
}
|
|
for (j = 0; j < count % 8; j++) // wont be executed if there is no remainder
|
|
{
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: j: %d; indx: %d; value: %d; mask: %X", j, i*8 + j, values[i*8 + j], (0x01 << j));
|
|
buffer[i] |= (values[(i)*8 + j] & 0x01) << j;
|
|
}
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: Byte %d = %X", i, buffer[i]);
|
|
|
|
hbin_to_strhex(values, str1);
|
|
bin_to_strhex(buffer, str2);
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: Encoded %s to %s (%d -> %d)", str1, str2, count, length);
|
|
ModbusWriteBits(address, count, buffer);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when confirming the request
|
|
void _OnModbusConfirmBits(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbInfo, "Updated %d bits at 0x%04X", mbc.Count, mbc.Address);
|
|
|
|
OnModbusWriteBitsSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusConfirmBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing bits: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteRegisters -------------------------------------------------------
|
|
/// <ModbusWriteRegisters>
|
|
// This method will submit a request to the Modbus server to set several holding registers
|
|
void ModbusWriteRegisters(word address, long count, word values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteRegisters);
|
|
enum ModbusFuncCode funcCode = WriteRegisters;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteRegisters mbreq;
|
|
word overallLength, curCount, addressO;
|
|
byte dataLength;
|
|
word i;
|
|
long offset;
|
|
word devStartAddr, devEndAddr;
|
|
|
|
devStartAddr = thisDev.Addr.Write.OutputRegisters; // The start address of the bits
|
|
devEndAddr = devStartAddr + thisDev.MaxRegisterCount; // The address behind the last bit
|
|
if (address >= devEndAddr)
|
|
devEndAddr = 0xFFFF;
|
|
if (address < devStartAddr) // Oh, reading at the wrong address?
|
|
{
|
|
writeDbg(MbError, "ModbusWriteRegisters: The given start address 0x%04X is smaller than the obligatory start address 0x%04X", address, devStartAddr);
|
|
OnModbusClientPanics(AddressFailure);
|
|
return;
|
|
}
|
|
|
|
// FC16: Write Multiple Registers (AOs)
|
|
offset = 0;
|
|
while (count > 0 && (address + offset) > devEndAddr)
|
|
{
|
|
addressO = address + offset;
|
|
curCount = count > gMaxRegsPerWrite ? gMaxRegsPerWrite : count;
|
|
|
|
if (addressO + curCount > devEndAddr) // upper bound in process image
|
|
{
|
|
writeDbg(MbWarning, "ModbusWriteRegisters: Impossible to write %d bits at 0x%04X. Changed count to %d.", curCount, addressO, devEndAddr - addressO);
|
|
curCount = devEndAddr - addressO;
|
|
}
|
|
|
|
dataLength = 2 * curCount;
|
|
overallLength = maxLength - 2*gMaxRegsPerWrite + dataLength;
|
|
|
|
_ModbusMakeHeader(mbreq.Header, overallLength, funcCode);
|
|
|
|
mbreq.Address = addressO; // [2] Output address
|
|
mbreq.Count = curCount; // [2] Number of items; 1:max 123=0x7B
|
|
mbreq.ByteCount = dataLength; // [1] Number of bytes; = 2 * count
|
|
|
|
for (i = 0; i < curCount; i++)
|
|
mbreq.Data[i] = values[i+offset];
|
|
//for ( ; i < gMaxRegsPerWrite; i++) // do we need this?
|
|
// mbreq.Data[i] = 0; // I'd say that the Modbus server will stop when it has read curCount values
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, overallLength, mbreq.Header.TxID);
|
|
|
|
count -= gMaxRegsPerWrite;
|
|
offset += gMaxRegsPerWrite;
|
|
}
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when confirming the request
|
|
void _OnModbusConfirmRegisters(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbInfo, "Updated %d registers at 0x%04X", mbc.Count, mbc.Address);
|
|
|
|
OnModbusWriteRegistersSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while writing registers: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteRegistersFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteMasks ------------------------------------------------------------
|
|
/// <ModbusWriteMasks>
|
|
// This method will submit a request to the Modbus server to mask a holding registers
|
|
/// The register will be using the following rule:
|
|
/// Result = (Content & AndMask) | (Content & !AndMask)
|
|
/// This means, you must not AND the bits you are trying to OR.
|
|
/// Example: You want to OR the 4th bit --> AND = 0xFFFB, OR = 0x0004
|
|
void ModbusWriteMasks(word address, word and, word or)
|
|
{
|
|
const word length = __size_of(struct ModbusReqWriteMasks);
|
|
enum ModbusFuncCode funcCode = MaskRegister;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteMasks mbreq;
|
|
|
|
// FC22: Mask Write Registers (AO)
|
|
_ModbusMakeHeader(mbreq.Header, length, funcCode);
|
|
|
|
mbreq.Address = address; // [2] Output address
|
|
mbreq.And = and; // [2] AND mask
|
|
mbreq.Or = or; // [2] OR mask
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, length, mbreq.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when confirming the request
|
|
void _OnModbusConfirmMasks(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMasks mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbInfo, "Applied masks at 0x%04X", mbc.Address);
|
|
|
|
OnModbusWriteMasksSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusConfirmMasksException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while applying masks: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteMasksFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusReadWriteRegisters -------------------------------------------------------
|
|
/// <ModbusReadWriteRegisters>
|
|
// This method will submit a request to the Modbus server to first set several holding registers and afterwards read (different) registers
|
|
void ModbusReadWriteRegisters(word readAddress, long readCount, word writeAddress, long writeCount, word values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqReadWriteRegisters);
|
|
enum ModbusFuncCode funcCode = ReadWriteRegisters;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqReadWriteRegisters mbreq;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i, offset;
|
|
|
|
offset = 0;
|
|
if (readCount > gMaxRegsPerRead - 2) // if we have to split the read request. count = n*max + y
|
|
{
|
|
ModbusReadRegisters(readAddress, readCount - readCount % gMaxRegsPerRead); // let this function read the main part: n*max
|
|
readAddress += readCount - readCount % gMaxRegsPerRead; // increment address by n*max
|
|
readCount %= gMaxRegsPerRead; // only read y elements in this function
|
|
}
|
|
if (writeCount > gMaxRegsPerWrite - 2) // if we have to split the write request. count = n*max + y
|
|
{
|
|
ModbusWriteRegisters(writeAddress, writeCount - writeCount % gMaxRegsPerWrite, values); // let this function read the main part: n*max
|
|
offset = writeCount - writeCount % gMaxRegsPerWrite; // start reading values at n*max
|
|
writeAddress += offset; // increment address by n*max
|
|
writeCount %= gMaxRegsPerWrite; // only read y elements in this function
|
|
}
|
|
|
|
dataLength = 2 * writeCount;
|
|
overallLength = maxLength - 2*(gMaxRegsPerWrite-2) + dataLength;
|
|
|
|
// FC16: Write Multiple Registers (AOs)
|
|
_ModbusMakeHeader(mbreq.Header, overallLength, funcCode);
|
|
|
|
mbreq.ReadAddress = readAddress; // [2] Input address
|
|
mbreq.ReadCount = readCount; // [2] Number of items; 1:max 125=0x7D
|
|
mbreq.WriteAddress = writeAddress; // [2] Output address
|
|
mbreq.WriteCount = writeCount; // [2] Number of items; 1:max 121=0x79
|
|
mbreq.ByteCount = dataLength; // [1] Number of bytes; = 2 * count
|
|
|
|
for (i = 0; i < writeCount; i++)
|
|
mbreq.Data[i] = values[i + offset];
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
_ModbusSend(buffer, overallLength, mbreq.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
// This method will be called by _OnModbusReceive2OnePacket() when confirming the request
|
|
void _OnModbusReceiveConfirmRegisters(byte buffer[])
|
|
{
|
|
struct ModbusResReceiveRegisters mbres;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);
|
|
|
|
writeDbg(MbInfo, "Wrote some registers and received %d registers from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
OnModbusReadRegistersSuccess(mbres, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
// This method will be called by _OnModbusReceive2Exceptions() when an exception occured
|
|
void _OnModbusReceiveConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
writeDbg(MbError, "Received an Exception while reading and writing registers: %s", ModbusExceptions[ex-1]);
|
|
OnModbusWriteRegistersFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
// ------------------------------------------------------------------------------------
|
|
// REGION: OnModbusReceive ------------------------------------------------------------
|
|
/// <-OnModbusReceive>
|
|
// This method parses the received data. It checks for network errors
|
|
// It gets called by the network layer (TCP, UDP, EIL) receive function (OnTcpReceive(),...)
|
|
void _OnModbusReceive(dword socket, long result, dword address, dword port, byte buffer[], dword size)
|
|
{
|
|
writeDbg(ConnDebug, "OnModbusReceive: Received %d bytes", size);
|
|
if (result == 0)
|
|
{
|
|
if (size == 0)
|
|
{
|
|
// 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
|
|
{
|
|
// Sucessfully received some bytes over the TCP/IP connection.
|
|
_OnModbusReceive2(buffer, size);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
gIpLastErr = _ModbusGetLastConnectionError(gIpLastErrStr);
|
|
writeDbg(ConnError, "OnModbusReceive error (%d): %s", gIpLastErr, gIpLastErrStr);
|
|
_ModbusDisconnect();
|
|
}
|
|
_ModbusRecv();
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
// This method continues with parsing the received data. It tries to separate several packets in the buffer
|
|
// It gets called by _OnModbusReceive()
|
|
void _OnModbusReceive2(byte buffer[], dword size)
|
|
{
|
|
struct ModbusApHeader mbap;
|
|
long offset;
|
|
char str[3*20];
|
|
|
|
if (size < 8) // No complete Modbus Application Header
|
|
return;
|
|
|
|
offset = 0;
|
|
do
|
|
{
|
|
memcpy_n2h(mbap, buffer, offset);
|
|
writeDbg(ConnDebug, "OnModbusReceive2: Offset pre = %d. TxID = %d", offset, mbap.TxID);
|
|
_OnModbusReceive2OnePacket(buffer, offset, mbap);
|
|
|
|
offset += __offset_of(struct ModbusApHeader, UnitID) + mbap.Length;
|
|
writeDbg(ConnDebug, "OnModbusReceive2: offset post = %d. %d <= %d?", offset, offset, size-8);
|
|
}
|
|
while(offset <= size-8); // We need at least 8 bytes for a new packet
|
|
writeDbg(ConnDebug, "OnModbusReceive2: yes. finished");
|
|
|
|
if (offset != size) // Can be removed.
|
|
{
|
|
bin_to_strhex(buffer, str);
|
|
writeDbg(ConnError, "OnModbusReceive2: Error while going through receive buffer. Our final offset is %d, but the size of the buffer is %d! Buffer: %s", offset, size, str);
|
|
OnModbusClientPanics(ParsingBuffer);
|
|
}
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
// This method continues with parsing the received packet.
|
|
// It checks whether it is a modbus packet and then hands it to _OnModbusReceive2Exceptions() or to the Success() function of the request
|
|
// It gets called by _OnModbusReceive2()
|
|
void _OnModbusReceive2OnePacket(byte buffer[], int offset, struct ModbusApHeader mbap)
|
|
{
|
|
// Test transaction identifier?
|
|
// Test unit/device identifier?
|
|
word i; // counter
|
|
word length; // length of current packet
|
|
|
|
length = __offset_of(struct ModbusApHeader, UnitID) + mbap.Length;
|
|
// We cannot check this properly anymore. We have to trust the TCP/UDP stack and the sender... *sigh*
|
|
if (mbap.Protocol != 0) // Protocol is not Modbus (0x0000). Wayne.
|
|
{
|
|
writeDbg(ConnDebug, "OnModbusReceive2OnePacket: packet is no Modbus packet: Protocol = %d", mbap.Protocol);
|
|
return;
|
|
}
|
|
if (elCount(buffer) < offset + length) // packet larger than the (rest of the) buffer
|
|
{
|
|
writeDbg(ConnError, "OnModbusReceive2OnePacket: packet did not fit into Buffer: buffer length = %d, packet length = %d, offset = %d", elCount(buffer), __offset_of(struct ModbusApHeader, UnitID) + mbap.Length, offset);
|
|
// I REALLY don't want to assemble the two package fragments.
|
|
OnModbusClientPanics(ModbusPackageWasSplit);
|
|
return;
|
|
}
|
|
// MBAP Header is OK :) Go on
|
|
|
|
if (!gQueueSent.ContainsKey(mbap.TxID)) // We don't wait for this message!?
|
|
return;
|
|
|
|
//write("Received TxID: %d", mbap.TxID);
|
|
memcpy(gQueueAck[mbap.TxID], gQueueSent[mbap.TxID]); // Move to acknowledge queue
|
|
gQueueSent.Remove(mbap.TxID);
|
|
|
|
if (mbap.FuncCode > 0x80) // Oh no, we got a exception!
|
|
_OnModbusReceive2Exceptions(buffer[offset+08], mbap);
|
|
else // Ok, everything is alright
|
|
_OnModbusReceive2Success(buffer, mbap, offset, length);
|
|
|
|
gQueueAck.Remove(mbap.TxID); // Remove from acknowledge queue
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
// This method will hand the exception code to the correct Exception() function
|
|
// It gets called by _OnModbusReceive2OnePacket()
|
|
void _OnModbusReceive2Exceptions(byte exCode, struct ModbusApHeader mbap)
|
|
{
|
|
enum ModbusException ex;
|
|
ex = (enum ModbusException)exCode;
|
|
|
|
switch (mbap.FuncCode)
|
|
{
|
|
case 0x80+ReadBitsOut:
|
|
case 0x80+ReadBitsIn:
|
|
_OnModbusReceiveBitsException(mbap, ex);
|
|
break;
|
|
case 0x80+ReadRegistersOut:
|
|
case 0x80+ReadRegistersIn:
|
|
_OnModbusReceiveRegistersException(mbap, ex);
|
|
break;
|
|
case 0x80+WriteBit:
|
|
_OnModbusConfirmBitException(mbap, ex);
|
|
break;
|
|
case 0x80+WriteRegister:
|
|
_OnModbusConfirmRegisterException(mbap, ex);
|
|
break;
|
|
case 0x80+WriteBits:
|
|
_OnModbusConfirmBitsException(mbap, ex);
|
|
break;
|
|
case 0x80+WriteRegisters:
|
|
_OnModbusConfirmRegistersException(mbap, ex);
|
|
break;
|
|
case 0x80+MaskRegister:
|
|
_OnModbusConfirmMasksException(mbap, ex);
|
|
break;
|
|
case 0x80+ReadWriteRegisters:
|
|
_OnModbusReceiveConfirmRegistersException(mbap, ex);
|
|
break;
|
|
}
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
// This method will hand the received data to the correct Success() function
|
|
// It gets called by _OnModbusReceive2OnePacket()
|
|
void _OnModbusReceive2Success(byte buffer[], struct ModbusApHeader mbap, int offset, word length)
|
|
{
|
|
byte mbuffer[gModbusMaxTelegramSize]; // second buffer where we copy the message. This way the user won't overwrite other packages.
|
|
// Copy the message
|
|
memcpy_off(mbuffer, 0, buffer, offset, length);
|
|
|
|
// Let's give the PDU to the corresponding function
|
|
switch (mbap.FuncCode)
|
|
{
|
|
case ReadBitsOut:
|
|
case ReadBitsIn:
|
|
_OnModbusReceiveBits(mbuffer);
|
|
break;
|
|
case ReadRegistersOut:
|
|
case ReadRegistersIn:
|
|
_OnModbusReceiveRegisters(mbuffer);
|
|
break;
|
|
case WriteBit:
|
|
_OnModbusConfirmBit(mbuffer);
|
|
break;
|
|
case WriteRegister:
|
|
_OnModbusConfirmRegister(mbuffer);
|
|
break;
|
|
case WriteBits:
|
|
_OnModbusConfirmBits(mbuffer);
|
|
break;
|
|
case WriteRegisters:
|
|
_OnModbusConfirmRegisters(mbuffer);
|
|
break;
|
|
case MaskRegister:
|
|
_OnModbusConfirmMasks(mbuffer);
|
|
break;
|
|
case ReadWriteRegisters:
|
|
_OnModbusReceiveConfirmRegisters(mbuffer);
|
|
break;
|
|
default:
|
|
writeDbg(MbError, "OnModbusReceive2Success: We received funcCode 0x%X!?", mbap.FuncCode);
|
|
}
|
|
}
|
|
|
|
|
|
// ------------------------------------------------------------------------------------
|
|
// REGION: ModbusSend -----------------------------------------------------------------
|
|
/// <-ModbusSend>
|
|
// This method will enqueue the data in gQueuePending and start gtModbusRobin if appropriate
|
|
// It will get called by the Modbus request functions
|
|
void _ModbusSend(byte buffer[], word length, word TxID)
|
|
{
|
|
struct QueueElement qe;
|
|
|
|
qe.Length = length;
|
|
memcpy(qe.Buffer, buffer, length);
|
|
|
|
memcpy(gQueuePending[TxID], qe);
|
|
writeDbg(ConnDebug, "Appended packet 0x%04X to pending queue", TxID);
|
|
|
|
_ModbusStartQueue();
|
|
}
|
|
|
|
// This method will start the timer. Nothing special :)
|
|
// It is seperate because the network layer may want to start it when it delayed some messages (e.g. while waiting for an ARP response)
|
|
// It gets called by _ModbusSend() and OnEthReceivePacket() in ModbusEil.cin
|
|
void _ModbusStartQueue()
|
|
{
|
|
if (gSocketState >= CLOSED && (gQueuePending.Size() > 0 || gQueueSent.Size() > 0))
|
|
{
|
|
writeDbg(ConnDebug, "Starting Timer gtModbusRobin");
|
|
setTimerCyclic(gtModbusRobin, 1);
|
|
}
|
|
}
|
|
|
|
/// <-ModbusSend>
|
|
// This timer will handle the pending and sent queues.
|
|
// The sent packets will be checked for timeouts (.TimeoutTicks) and may be resent
|
|
// The pending packets may be sent when there is free space in the sending window
|
|
on timer gtModbusRobin
|
|
{
|
|
enum ModbusRequestError reqError;
|
|
|
|
writeDbg(ConnDebug, "gtModbusRobin: Queue Sent: %d, Queue Pending: %d, Queue Ack: %d", gQueueSent.Size(), gQueuePending.Size(), gQueueAck.Size());
|
|
|
|
// First: check timeouts = packets that were sent in previous run and not removed by response
|
|
for (long TxID : gQueueSent)
|
|
{
|
|
if (++gQueueSent[TxID].TimeoutTicks < gRequestTimeout) // not timed out yet
|
|
continue;
|
|
// timed out!
|
|
if (++gQueueSent[TxID].Timeouts < gMaxTransmissionCount) // if we may resend it
|
|
{
|
|
writeDbg(ConnInfo, "gtModbusRobin: Packet 0x%04X timed out! Retrying...", TxID);
|
|
gQueueSent[TxID].TimeoutTicks = 0;
|
|
_ModbusSnd(gQueueSent[TxID].Buffer, gQueueSent[TxID].Length); // resend it
|
|
reqError = Timeout;
|
|
_ModbusRecv();
|
|
}
|
|
else // we will NOT resend it
|
|
{
|
|
writeDbg(ConnWarning, "gtModbusRobin: Packet 0x%04X timed out! Giving up", TxID);
|
|
reqError = FinalTimeout;
|
|
}
|
|
|
|
_ModbusSendTimerError(gQueueSent[TxID].Buffer, reqError); // throw an "error" in each case
|
|
|
|
if (reqError == FinalTimeout) // remove the packet from queue
|
|
gQueueSent.Remove(TxID); // wait until here to let the methods above access the request
|
|
}
|
|
|
|
// Second: send new packets
|
|
for (long TxID : gQueuePending)
|
|
{
|
|
if (gQueueSent.Size() >= thisDev.ReceiveWindow) // Device cannot handle that many messages at a time
|
|
break; // Wait for the next turn
|
|
|
|
// Now send the packet
|
|
// if packet was sent or the socket is not currently being opened
|
|
if (_ModbusSnd(gQueuePending[TxID].Buffer, gQueuePending[TxID].Length) == 0)
|
|
{
|
|
memcpy(gQueueSent[TxID], gQueuePending[TxID]); // move packet to sent queue
|
|
gQueuePending.Remove(TxID);
|
|
_ModbusRecv(); // wait for new packets
|
|
}
|
|
else if (gQueuePending[TxID].SendTries++ > 10) // We have tried to send this packet too often. Something is wrong!?
|
|
{
|
|
writeDbg(ConnError, "gtModbusRobin: The packet 0x%04X could not be sent, _ModbusSnd() rejected it several times", TxID);
|
|
_ModbusSendTimerError(gQueuePending[TxID].Buffer, NotSent);
|
|
gQueuePending.Remove(TxID);
|
|
}
|
|
}
|
|
|
|
// Stop timer to reduce load and latency of first new packet
|
|
if (gSocketState != OK || gQueueSent.Size() == 0 && gQueuePending.Size() == 0)
|
|
{
|
|
writeDbg(ConnDebug, "gtModbusRobin: Stopping Timer. Queue Sent: %d, Queue Pending: %d", gQueueSent.Size(), gQueuePending.Size());
|
|
this.Cancel();
|
|
}
|
|
}
|
|
|
|
/// <-ModbusSend>
|
|
// This method will give an Error
|
|
void _ModbusSendTimerError(byte buffer[], enum ModbusRequestError reqError)
|
|
{
|
|
struct ModbusApHeader mbap;
|
|
memcpy_n2h(mbap, buffer);
|
|
switch(mbap.FuncCode)
|
|
{
|
|
case ReadBitsOut:
|
|
case ReadBitsIn:
|
|
OnModbusReadBitsFailed(reqError, None, mbap);
|
|
break;
|
|
case ReadRegistersOut:
|
|
case ReadRegistersIn:
|
|
OnModbusReadRegistersFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteBit:
|
|
OnModbusWriteBitFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteRegister:
|
|
OnModbusWriteRegisterFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteBits:
|
|
OnModbusWriteBitsFailed(reqError, None, mbap);
|
|
break;
|
|
case WriteRegisters:
|
|
OnModbusWriteRegistersFailed(reqError, None, mbap);
|
|
break;
|
|
case MaskRegister:
|
|
OnModbusWriteMasksFailed(reqError, None, mbap);
|
|
break;
|
|
case ReadWriteRegisters:
|
|
OnModbusReadWriteRegistersFailed(reqError, None, mbap);
|
|
break;
|
|
}
|
|
} |