Jonny007-MKD
7fc38537b3
Added OnModbusClientPanics() better output Common.cin Changed output sink to CAPL
786 lines
No EOL
23 KiB
Text
786 lines
No EOL
23 KiB
Text
/*@!Encoding:1252*/
|
|
// Additionally include ModbusTcpCommon.cin or ModbusUdpCommon.cin
|
|
includes
|
|
{
|
|
#include "ModbusCommonStructs.cin"
|
|
}
|
|
|
|
variables
|
|
{
|
|
const word gMaxPacketLength = __size_of(struct ModbusReqWriteRegisters);
|
|
|
|
msTimer gtRobin; // Timer that sends the packets and watches for timeouts
|
|
word gTxID = 0x0000; // Transaction Identifier for Modbus. Used as index for gQueue
|
|
|
|
// Global storage for pending and sent requests, associated by TxID
|
|
struct QueueElement
|
|
{
|
|
word TimeoutTicks;
|
|
byte Timeouts;
|
|
word Length;
|
|
byte Buffer[gMaxPacketLength];
|
|
};
|
|
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"
|
|
};
|
|
}
|
|
|
|
void ModbusInit()
|
|
{
|
|
char ip[16];
|
|
sysGetVariableString("%BUS_TYPE%%CHANNEL%::%NODE_NAME%::Config", "IP", ip, elCount(ip));
|
|
|
|
writeDbg(MbInfo, "Connecting to %s:%d", ip, @sysvar::Config::Modbus::Port);
|
|
ModbusConnectTo(ip, @sysvar::Config::Modbus::Port);
|
|
}
|
|
|
|
void ModbusMakeHeader(struct ModbusApHeader mbap, word length)
|
|
{
|
|
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
|
|
}
|
|
|
|
|
|
|
|
// REGION: ModbusReadBits -------------------------------------------------------------
|
|
/// <ModbusReadBits>
|
|
void ModbusReadBits(word address, word count)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
const byte funcCode = 0x01;
|
|
byte buffer[length];
|
|
struct ModbusReqRead mbr;
|
|
|
|
ModbusMakeHeader(mbr.Header, length);
|
|
// Payload
|
|
mbr.Header.FuncCode = funcCode; // [1] Function Code; 1: Read Coils (DI), 2: Read Discret Inputs (DIO), seems to be the same for WAGO 750-881
|
|
mbr.Address = address; // [2] Start address
|
|
mbr.Count = count; // [2] Number of items; 1:max 2000=0x7D0
|
|
|
|
writeDbg(MbDebug, "Sending 'Read Bits' (0x01) command. Addr: 0x%04X, Count: %d", address, count);
|
|
|
|
memcpy_h2n(buffer, mbr);
|
|
ModbusSend(buffer, length, mbr.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
void OnModbusReceiveBits(byte buffer[])
|
|
{
|
|
struct ModbusResReceiveBits mbres;
|
|
struct ModbusReqRead mbreq;
|
|
byte bitStatus[1968];
|
|
word numBits;
|
|
byte i, j;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);
|
|
|
|
writeDbg(MbDebug, "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>
|
|
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>
|
|
void ModbusReadRegisters(word address, word count) // 16 bit
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
const byte funcCode = 0x03;
|
|
byte buffer[length];
|
|
struct ModbusReqRead mbr;
|
|
|
|
ModbusMakeHeader(mbr.Header, length);
|
|
// Payload
|
|
mbr.Header.FuncCode = funcCode; // [1] Function Code; 3: Read Holding Registers (AI), 4: Read Input Registers (AIO), seems to be the same for WAGO 750-881
|
|
mbr.Address = address; // [2] Start address
|
|
mbr.Count = count; // [2] Number of items; 1:max 125=0x7D
|
|
|
|
writeDbg(MbDebug, "Sending 'Read Registers' (0x03) command. Addr: 0x%04X, Count: %d", address, count);
|
|
|
|
memcpy_h2n(buffer, mbr);
|
|
ModbusSend(buffer, length, mbr.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
void OnModbusReceiveRegisters(byte buffer[])
|
|
{
|
|
struct ModbusResReceiveRegisters mbres;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);
|
|
|
|
writeDbg(MbDebug, "Received %d registers from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
OnModbusReadRegistersSuccess(mbres, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
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>
|
|
void ModbusWriteBit(word address, byte value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x05;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteSingle mbw;
|
|
|
|
if (value >= 1)
|
|
value = 0xFF;
|
|
|
|
ModbusMakeHeader(mbw.Header, length);
|
|
// Payload
|
|
mbw.Header.FuncCode = funcCode; // [1] Function Code; 5: Write Single Coil (DO)
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Value = value << 8; // [2] Output value (0x0000: Off, 0xFF00: On)
|
|
|
|
writeDbg(Debug, "Sending 'Write Bit' (0x05) command. Addr: 0x%04X, Value: 0x%02X", address, value);
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, length, mbw.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
void OnModbusConfirmBit(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Set bit at 0x%04X to %d", mbc.Address, mbc.Value);
|
|
|
|
OnModbusWriteBitSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
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>
|
|
void ModbusWriteRegister(word address, int value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x06;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteSingle mbw;
|
|
|
|
ModbusMakeHeader(mbw.Header, length);
|
|
// Payload
|
|
mbw.Header.FuncCode = funcCode; // [1] Function Code; 5: Write Single Register (AO)
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Value = value; // [2] Output value
|
|
|
|
writeDbg(MbDebug, "Sending 'Write Register' (0x06) command. Addr: 0x%04X, Value: 0x%02X", address, value);
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, length, mbw.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
void OnModbusConfirmRegister(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Set register at 0x%04X to %d", mbc.Address, mbc.Value);
|
|
|
|
OnModbusWriteRegisterSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
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>
|
|
void ModbusWriteBits(word address, word count, byte values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteBits);
|
|
const byte funcCode = 0x0F;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteBits mbw;
|
|
byte dataLength;
|
|
byte overallLength;
|
|
word i;
|
|
|
|
dataLength = _ceil(count / 8.0);
|
|
overallLength = maxLength - 1968/8 + dataLength;
|
|
ModbusMakeHeader(mbw.Header, overallLength);
|
|
// Payload
|
|
mbw.Header.FuncCode = funcCode; // [1] Function Code; 15: Write Multiple Bits (DOs)
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Count = count; // [2] Number of items; 1:max 1968=0x7B0
|
|
mbw.ByteCount = dataLength; // [1] Number of bytes; = ceil(count/8)
|
|
memcpy(mbw.Data, values, dataLength); // this is 1 memcpy too much -.-
|
|
|
|
writeDbg(MbDebug, "Sending 'Write Bits' (0x0F) command. Addr: 0x%04X, Count: %d", address, count);
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, overallLength, mbw.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void ModbusWriteBitsB(word address, word count, byte values[])
|
|
{
|
|
byte buffer[2]; // length
|
|
word length;
|
|
dword ellCount;
|
|
dword i;
|
|
dword j;
|
|
char str1[20*2], str2[20*3];
|
|
|
|
length = (word)_ceil(count / 8.0);
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: count: %d; length: %d", count, length);
|
|
|
|
if (count % 8 != 0)
|
|
length--;
|
|
|
|
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: %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, (length-1)*8 + j, values[(length-1)*8 + j], (0x01 << j));
|
|
buffer[length] |= (values[(length)*8 + j] & 0x01) << j;
|
|
}
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: %d: %X", length-1, buffer[length-1]);
|
|
|
|
hbin_to_strhex(values, str1);
|
|
bin_to_strhex(buffer, str2);
|
|
writeDbg(AlgoDebug, "ModbusWriteBitsB: Encoded %s to %s", str1, str2);
|
|
ModbusWriteBits(address, count, buffer);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void OnModbusConfirmBits(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Updated &d bits at 0x%04X", mbc.Count, mbc.Address);
|
|
|
|
OnModbusWriteBitsSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
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>
|
|
void ModbusWriteRegisters(word address, word count, word values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteRegisters);
|
|
const byte funcCode = 0x10;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteRegisters mbw;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i;
|
|
|
|
dataLength = 2 * count;
|
|
overallLength = maxLength - 2*123 + dataLength;
|
|
|
|
ModbusMakeHeader(mbw.Header, overallLength);
|
|
// Payload
|
|
mbw.Header.FuncCode = funcCode; // [1] Function Code; 16: Write Multiple Registers (AOs)
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.Count = count; // [2] Number of items; 1:max 123=0x7B
|
|
mbw.ByteCount = dataLength; // [1] Number of bytes; = 2 * count
|
|
|
|
for (i = 0; i < dataLength; i++)
|
|
mbw.Data[i] = values[i];
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, overallLength, mbw.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
void OnModbusConfirmRegisters(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Updated &d registers at 0x%04X", mbc.Count, mbc.Address);
|
|
|
|
OnModbusWriteRegistersSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
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>
|
|
void ModbusWriteMasks(word address, word and, word or)
|
|
{
|
|
const word length = __size_of(struct ModbusReqWriteMasks);
|
|
const byte funcCode = 0x16;
|
|
byte buffer[length];
|
|
struct ModbusReqWriteMasks mbw;
|
|
|
|
ModbusMakeHeader(mbw.Header, length);
|
|
// Payload
|
|
mbw.Header.FuncCode = funcCode; // [1] Function Code; 22: Mask Write Registers (AO)
|
|
mbw.Address = address; // [2] Output address
|
|
mbw.And = and; // [2] AND mask
|
|
mbw.Or = or; // [2] OR mask
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, length, mbw.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
void OnModbusConfirmMasks(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMasks mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbDebug, "Applied masks at 0x%04X", mbc.Address);
|
|
|
|
OnModbusWriteMasksSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
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>
|
|
void ModbusReadWriteRegisters(word readAddress, word readCount, word writeAddress, word writeCount, int values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqReadWriteRegisters);
|
|
const byte funcCode = 0x17;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqReadWriteRegisters mbw;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i;
|
|
|
|
dataLength = 2 * writeCount;
|
|
overallLength = maxLength - 2*121 + dataLength;
|
|
|
|
ModbusMakeHeader(mbw.Header, overallLength);
|
|
// Payload
|
|
mbw.Header.FuncCode = funcCode; // [1] Function Code; 16: Write Multiple Registers (AOs)
|
|
mbw.ReadAddress = readAddress; // [2] Input address
|
|
mbw.ReadCount = readCount; // [2] Number of items; 1:max 125=0x7D
|
|
mbw.WriteAddress = writeAddress;// [2] Output address
|
|
mbw.WriteCount = writeCount; // [2] Number of items; 1:max 121=0x79
|
|
mbw.ByteCount = dataLength; // [1] Number of bytes; = 2 * count
|
|
|
|
for (i = 0; i < dataLength; i++)
|
|
mbw.Data[i] = values[i];
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer, overallLength, mbw.Header.TxID);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
void OnModbusReceiveConfirmRegisters(byte buffer[])
|
|
{
|
|
struct ModbusResReceiveRegisters mbres;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
memcpy_n2h(mbres, buffer);
|
|
memcpy_n2h(mbreq, gQueueAck[mbres.Header.TxID].Buffer);
|
|
|
|
writeDbg(MbDebug, "Wrote some registers and received %d registers from 0x%04X", mbreq.Count, mbreq.Address);
|
|
|
|
OnModbusReadRegistersSuccess(mbres, mbreq);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
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>
|
|
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();
|
|
}
|
|
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();
|
|
}
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
void OnModbusReceive2(byte buffer[], dword size)
|
|
{
|
|
struct ModbusApHeader mbap;
|
|
int offset;
|
|
char str[3*20];
|
|
|
|
if (size < 8) // No complete Modbus Application Header
|
|
return;
|
|
|
|
offset = 0;
|
|
do
|
|
{
|
|
writeDbg(ConnDebug, "OnModbusReceive2: Offset pre = %d", offset);
|
|
memcpy_n2h(mbap, buffer, offset);
|
|
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>
|
|
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
|
|
byte mbuffer[__size_of(struct ModbusResReceiveRegisters)]; // second buffer where we copy the message. This way the user won't overwrite other packages.
|
|
|
|
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]);
|
|
gQueueSent.Remove(mbap.TxID);
|
|
|
|
if (mbap.FuncCode > 0x80) // Oh no, we got a exception!
|
|
{
|
|
OnModbusReceive2Exceptions(buffer[offset+08], mbap);
|
|
|
|
gQueueAck.Remove(mbap.TxID); // Remove from acknowledge queue
|
|
|
|
return;
|
|
}
|
|
|
|
|
|
// Copy the message
|
|
memcpy_off(mbuffer, 0, buffer, offset, length);
|
|
|
|
// Let's give the PDU to the corresponding function
|
|
switch (mbap.FuncCode)
|
|
{
|
|
case 0x01:
|
|
case 0x02:
|
|
OnModbusReceiveBits(mbuffer);
|
|
break;
|
|
case 0x03:
|
|
case 0x04:
|
|
OnModbusReceiveRegisters(mbuffer);
|
|
break;
|
|
case 0x05:
|
|
OnModbusConfirmBit(mbuffer);
|
|
break;
|
|
case 0x06:
|
|
OnModbusConfirmRegister(mbuffer);
|
|
break;
|
|
case 0x0F:
|
|
OnModbusConfirmBits(mbuffer);
|
|
break;
|
|
case 0x10:
|
|
OnModbusConfirmRegisters(mbuffer);
|
|
break;
|
|
case 0x16:
|
|
OnModbusConfirmMasks(mbuffer);
|
|
break;
|
|
case 0x17:
|
|
OnModbusReceiveConfirmRegisters(mbuffer);
|
|
break;
|
|
default:
|
|
writeDbg(MbError, "OnModbusReceive2OnePacket: We received funcCode 0x%X!?", mbap.FuncCode);
|
|
}
|
|
|
|
gQueueAck.Remove(mbap.TxID); // Remove from acknowledge queue
|
|
}
|
|
|
|
/// <-OnModbusReceive>
|
|
void OnModbusReceive2Exceptions(byte exCode, struct ModbusApHeader mbap)
|
|
{
|
|
enum ModbusException ex;
|
|
ex = (enum ModbusException)exCode;
|
|
|
|
switch (mbap.FuncCode)
|
|
{
|
|
case 0x81:
|
|
case 0x82:
|
|
OnModbusReceiveBitsException(mbap, ex);
|
|
break;
|
|
case 0x83:
|
|
case 0x84:
|
|
OnModbusReceiveRegistersException(mbap, ex);
|
|
break;
|
|
case 0x85:
|
|
OnModbusConfirmBitException(mbap, ex);
|
|
break;
|
|
case 0x86:
|
|
OnModbusConfirmRegisterException(mbap, ex);
|
|
break;
|
|
case 0x8F:
|
|
OnModbusConfirmBitsException(mbap, ex);
|
|
break;
|
|
case 0x90:
|
|
OnModbusConfirmRegistersException(mbap, ex);
|
|
break;
|
|
case 0x96:
|
|
OnModbusConfirmMasksException(mbap, ex);
|
|
break;
|
|
case 0x97:
|
|
OnModbusReceiveConfirmRegistersException(mbap, ex);
|
|
break;
|
|
}
|
|
}
|
|
|
|
|
|
// ------------------------------------------------------------------------------------
|
|
// REGION: ModbusSend -----------------------------------------------------------------
|
|
/// <-ModbusSend>
|
|
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);
|
|
|
|
if (gQueuePending.Size() == 1 && gQueueSent.Size() == 0 && gSocketState == OK) // start timer if connection established
|
|
ModbusStartQueue();
|
|
}
|
|
|
|
void ModbusStartQueue()
|
|
{
|
|
writeDbg(ConnDebug, "Starting Timer gtRobin");
|
|
setTimerCyclic(gtRobin, 1);
|
|
}
|
|
|
|
/// <-ModbusSend>
|
|
on timer gtRobin
|
|
{
|
|
struct ModbusApHeader mbap;
|
|
enum ModbusRequestError reqError;
|
|
|
|
writeDbg(ConnDebug, "gtRobin: 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 < @sysvar::Config::Modbus::RequestTimeout) // not timed out yet
|
|
continue;
|
|
// timed out!
|
|
if (++gQueueSent[TxID].Timeouts < @sysvar::Config::Modbus::MaxTransmissionCount) // if we may resend it
|
|
{
|
|
writeDbg(ConnInfo, "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, "Packet 0x%04X timed out! Giving up", TxID);
|
|
reqError = FinalTimeout;
|
|
}
|
|
|
|
memcpy_n2h(mbap, gQueueSent[TxID].Buffer);
|
|
switch(mbap.FuncCode) // throw an "error" in each case
|
|
{
|
|
case ReadBits1:
|
|
case ReadBits2:
|
|
OnModbusReadBitsFailed(reqError, None, mbap);
|
|
break;
|
|
case ReadRegisters1:
|
|
case ReadRegisters2:
|
|
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;
|
|
}
|
|
|
|
if (reqError == FinalTimeout) // remove the packet from queue
|
|
gQueueSent.Remove(TxID); // wait until here to let the methods access the request
|
|
}
|
|
|
|
// Second: send new packets
|
|
for (long TxID : gQueuePending)
|
|
{
|
|
if (gQueueSent.Size() > 4) // Wago 750-881 cannot handle more than 5 messages at a time :(
|
|
continue;
|
|
|
|
// if packet was sent or the socket is not currently being opened
|
|
if (ModbusSnd(gQueuePending[TxID].Buffer, gQueuePending[TxID].Length) == 0 || gSocketState != NULL)
|
|
{
|
|
memcpy(gQueueSent[TxID], gQueuePending[TxID]); // move packet to sent queue
|
|
gQueuePending.Remove(TxID);
|
|
ModbusRecv();
|
|
}
|
|
}
|
|
|
|
if (gSocketState == ERROR || gQueueSent.Size() == 0 && gQueuePending.Size() == 0) // Stop timer to reduce latency of first packet
|
|
{
|
|
writeDbg(ConnDebug, "Stopping Timer gtRobin");
|
|
this.Cancel();
|
|
}
|
|
} |