Jonny007-MKD
93dd56469d
Introduced loops to automatically split requests that are too large for Modbus ModbusClient.can Modified Modbus events so that the split requests (see above) will be written at the correct position in sys vars ModbusFunctions.cin DeviceInformation.cin Introduced new file that will handle most device specific things ModbusStructs.cin Introduced new constants with maximum Modbus values MakeConfig.can Increment IP address with swapDWord Moved detection stuff to DeviceInformation.cin
857 lines
26 KiB
Plaintext
857 lines
26 KiB
Plaintext
/*@!Encoding:1252*/
|
|
// Additionally include ModbusTcpCommon.cin or ModbusUdpCommon.cin
|
|
includes
|
|
{
|
|
#include "ModbusStructs.cin"
|
|
}
|
|
|
|
variables
|
|
{
|
|
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[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"
|
|
};
|
|
}
|
|
|
|
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, byte 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>
|
|
void ModbusReadInBits(word address, long count)
|
|
{
|
|
ModbusReadBits(0x02, address, count);
|
|
}
|
|
/// <ModbusReadBits>
|
|
void ModbusReadBits(word address, long count)
|
|
{
|
|
ModbusReadBits(0x02, address, count);
|
|
}
|
|
/// <ModbusReadBits>
|
|
void ModbusReadOutBits(word address, long count)
|
|
{
|
|
ModbusReadBits(0x01, address, count);
|
|
}
|
|
/// <ModbusReadBits>
|
|
void ModbusReadBits(byte funcCode, word address, long count)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
byte buffer[length];
|
|
word curCount;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
// FC1: Read Coils (DO), FC2: Read Discret Inputs (DI)
|
|
while (count > 0)
|
|
{
|
|
curCount = count > gMaxBitsPerRead ? gMaxBitsPerRead : count;
|
|
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>
|
|
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>
|
|
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 ModbusReadInRegisters(word address, long count)
|
|
{
|
|
ModbusReadRegisters(0x04, address, count);
|
|
}
|
|
/// <ModbusReadRegisters>
|
|
void ModbusReadRegisters(word address, long count)
|
|
{
|
|
ModbusReadRegisters(0x04, address, count);
|
|
}
|
|
/// <ModbusReadRegisters>
|
|
void ModbusReadOutRegisters(word address, long count)
|
|
{
|
|
ModbusReadRegisters(0x03, address, count);
|
|
}
|
|
/// <ModbusReadRegisters>
|
|
void ModbusReadRegisters(byte funcCode, word address, long count)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
byte buffer[length];
|
|
word curCount;
|
|
struct ModbusReqRead mbreq;
|
|
|
|
// FC3: Read Holding Registers (AO), FC4: Read Input Registers (AI)
|
|
while (count > 0)
|
|
{
|
|
curCount = count > gMaxRegsPerRead ? gMaxRegsPerRead : count;
|
|
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>
|
|
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>
|
|
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; // B&R does not support 0x06
|
|
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>
|
|
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>
|
|
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, word value)
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x06;
|
|
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>
|
|
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>
|
|
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, long count, byte values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteBits);
|
|
const byte funcCode = 0x0F;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteBits mbreq;
|
|
word curCount;
|
|
byte dataLength;
|
|
byte overallLength;
|
|
word i;
|
|
|
|
// FC15: Write Multiple Bits (DOs)
|
|
while (count > 0)
|
|
{
|
|
curCount = count > gMaxBitsPerWrite ? gMaxBitsPerWrite : count;
|
|
dataLength = _ceil(curCount / 8.0);
|
|
overallLength = maxLength - gMaxBitsPerWrite/8 + dataLength;
|
|
ModbusMakeHeader(mbreq.Header, overallLength, funcCode);
|
|
|
|
mbreq.Address = address; // [2] Output address
|
|
mbreq.Count = curCount; // [2] Number of items; 1:max 1968=0x7B0
|
|
mbreq.ByteCount = dataLength; // [1] Number of bytes; = ceil(count/8)
|
|
memcpy(mbreq.Data, values, dataLength); // [246] Data; this is 1 unneccessary memcpy :( Well, readability...
|
|
|
|
writeDbg(MbDebug, "Sending 'Write Bits' (0x0F) command. Addr: 0x%04X, Count: %d", address, curCount);
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
ModbusSend(buffer, overallLength, mbreq.Header.TxID);
|
|
|
|
count -= gMaxBitsPerWrite;
|
|
address += gMaxBitsPerWrite;
|
|
}
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void ModbusWriteBitsB(word address, long 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(MbInfo, "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, long count, word values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteRegisters);
|
|
const byte funcCode = 0x10;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteRegisters mbreq;
|
|
word curCount;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i;
|
|
|
|
// FC16: Write Multiple Registers (AOs)
|
|
while (count > 0)
|
|
{
|
|
curCount = count > gMaxRegsPerWrite ? gMaxRegsPerWrite : count;
|
|
dataLength = 2 * curCount;
|
|
overallLength = maxLength - 2*gMaxRegsPerWrite + dataLength;
|
|
|
|
ModbusMakeHeader(mbreq.Header, overallLength, funcCode);
|
|
|
|
mbreq.Address = address; // [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];
|
|
for ( ; i < gMaxRegsPerWrite; i++) // do we need this?
|
|
mbreq.Data[i] = 0;
|
|
|
|
memcpy_h2n(buffer, mbreq);
|
|
ModbusSend(buffer, overallLength, mbreq.Header.TxID);
|
|
}
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
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>
|
|
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 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>
|
|
void OnModbusConfirmMasks(byte buffer[])
|
|
{
|
|
struct ModbusResConfirmMasks mbc;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
writeDbg(MbInfo, "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, long readCount, word writeAddress, long writeCount, word values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqReadWriteRegisters);
|
|
const byte funcCode = 0x17;
|
|
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>
|
|
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>
|
|
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;
|
|
long 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[gModbusMaxTelegramSize]; // 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() >= gDevReceiveWindow) // Device cannot handle many messages at a time
|
|
break;
|
|
|
|
// 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();
|
|
}
|
|
} |