1139 lines
44 KiB
Plaintext
1139 lines
44 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 watches for timeouts. When responses are received, the corresponding callback method will be called
|
|
/// There are three queues available: Pending, Sent & Ack. Only the latter (gQueueAck) 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
|
|
|
|
/// This Modbus Client file provides following methods
|
|
/// - ModbusInit() Prepare anything that Modbus works (open connection etc.)
|
|
/// - ModbusEnd() Clean up everything (close connection etc.)
|
|
|
|
/// - ModbusReadOutBits() Modbus Function Code: 0x01 (Read Discrete inputs)
|
|
/// - ModbusReadInBits() Modbus Function Code: 0x02 (Read Coils)
|
|
/// --> results in OnModbusReadBitsSuccess() or OnModbusReadBitsFailed() (have to be implemented by the higher layer)
|
|
|
|
/// - ModbusReadOutRegisters() Modbus Function Code: 0x03 (Read Input registers)
|
|
/// - ModbusReadInRegisters() Modbus Function Code: 0x04 (Read Holding registers)
|
|
/// --> results in OnModbusReadRegistersSuccess() or OnModbusReadRegistersFailed() (have to be implemented by the higher layer)
|
|
|
|
/// - ModbusWriteBit() Modbus Function Code: 0x05 (Write Coil)
|
|
/// --> results in OnModbusWriteBitSuccess() or OnModbusWriteBitFailed() (have to be implemented by the higher layer)
|
|
|
|
/// - ModbusWriteRegister() Modbus Function Code: 0x06 (Write Holding register)
|
|
/// --> results in OnModbusWriteRegisterSuccess() or OnModbusWriteRegisterFailed() (have to be implemented by the higher layer)
|
|
|
|
/// - ModbusWriteBits() Modbus Function Code: 0x0F (Write multiple 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 the higher layer)
|
|
|
|
/// - ModbusWriteRegisters() Modbus Function Code: 0x10 (Write multiple Holding registers)
|
|
/// - ModbusReadWriteRegisters() Modbus Function Code: 0x17 (Read/Write multiple Holding registers)
|
|
/// --> results in OnModbusWriteRegistersSuccess() or OnModbusWriteRegistersFailed() (have to be implemented by the higher layer)
|
|
|
|
/// - ModbusWriteMasks() Modbus Function Code: 0x10 (Mask Write Holding registers)
|
|
/// --> results in OnModbusWriteMasksSuccess() or OnModbusWriteMasksFailed() (have to be implemented by the higher layer)
|
|
|
|
/// Additionally the highest layer have to implement OnModbusClientPanics() which all layers will call and pass errors. The highest layer may decide what to do with this error.
|
|
|
|
|
|
/// Methods that have to be implemented by the higher layer are:
|
|
///// This method gets called when an error occured while trying to read some bits (ModbusReadBits())
|
|
///void OnModbusReadBitsFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to read some registers (ModbusReadRegisters())
|
|
///void OnModbusReadRegistersFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to set a bit (ModbusWriteBit())
|
|
///void OnModbusWriteBitFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to set a register (ModbusWriteRegister())
|
|
///void OnModbusWriteRegisterFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to apply a mask on a register (ModbusWriteMask())
|
|
///void OnModbusWriteMasksFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to read and write registers (ModbusReadWriteRegisters())
|
|
///void OnModbusReadWriteRegistersFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to set multiple bits (ModbusWriteBits())
|
|
///void OnModbusWriteBitsFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when an error occured while trying to set multiple registers (ModbusWriteRegisters())
|
|
///void OnModbusWriteRegistersFailed(enum ModbusRequestError error, enum ModbusException ex, struct ModbusApHeader mbap){}
|
|
///// This method gets called when some bits were read successfully. See 'bitStatus' for their values and 'mbreq.Address' for their start address
|
|
///void OnModbusReadBitsSuccess(struct ModbusResReceiveBits mbres, byte bitStatus[], struct ModbusReqRead mbreq){}
|
|
///// This method gets called when some bits were read successfully. See 'mbres.Data' for their values and 'mbreq.Address' for their start address
|
|
///void OnModbusReadRegistersSuccess(struct ModbusResReceiveRegisters mbres, struct ModbusReqRead mbreq){}
|
|
///// This method gets called when a bit was set successfully.
|
|
///void OnModbusWriteBitSuccess(struct ModbusResConfirmSingle mbres){}
|
|
///// This method gets called when a register was set successsfully.
|
|
///void OnModbusWriteRegisterSuccess(struct ModbusResConfirmSingle mbres){}
|
|
///// This method gets called when multiple bits were set successfully.
|
|
///void OnModbusWriteBitsSuccess(struct ModbusResConfirmMultiple mbres){}
|
|
///// This method gets called when multiple registers were set successfully.
|
|
///void OnModbusWriteRegistersSuccess(struct ModbusResConfirmMultiple mbres){}
|
|
///// This method gets called when a mask was applied successfully.
|
|
///void OnModbusWriteMasksSuccess(struct ModbusResConfirmMasks mbres){}
|
|
///
|
|
///// This method gets called when the Modbus Client panics (saying a fatal error occured).
|
|
///// It will pass as argument what happened. Please see the log (increase debug level in preStart) for more details.
|
|
///void OnModbusClientPanics(enum FatalErrors reason){}
|
|
|
|
|
|
includes
|
|
{
|
|
#include "ModbusStructs.cin" // We only depend on the structures
|
|
// The network layer will be included by the higher layer
|
|
}
|
|
|
|
variables
|
|
{
|
|
msTimer gtModbusRobin; // Timer that sends the packets and watches for timeouts
|
|
word gTxID = 0x0000; // Transaction Identifier for the next Modbus packet. Used as index for gQueue
|
|
word gRequestTimeout; // Timeout of a packet [ms]
|
|
byte gMaxTransmissionCount; // Maximum number of transmissions (after timeouts)
|
|
|
|
// Storage element for pending, sent and acknowledge 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];
|
|
};
|
|
// Storages with these requests
|
|
struct QueueElement gQueuePending[long, 2];
|
|
struct QueueElement gQueueSent[long, 2];
|
|
struct QueueElement gQueueAck[long, 2];
|
|
}
|
|
|
|
// 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();
|
|
|
|
// Now we have to move the already sent telegrams back to the pending queue to send them again
|
|
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 connection.
|
|
_OnModbusReceive2(buffer, size);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
gIpLastErr = _ModbusGetLastConnectionError(gIpLastErrStr);
|
|
writeDbg(ConnError, "OnModbusReceive error (%d): %s", gIpLastErr, gIpLastErrStr);
|
|
_ModbusDisconnect();
|
|
}
|
|
_ModbusRecv(); // Continue receiving
|
|
}
|
|
|
|
/// <-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 < __size_of(struct ModbusApHeader)) // No complete Modbus Application Header fits into this data
|
|
return;
|
|
|
|
offset = 0;
|
|
do
|
|
{
|
|
memcpy_n2h(mbap, buffer, offset); // Copy the data into the Modbus Header
|
|
writeDbg(ConnDebug, "OnModbusReceive2: Offset pre = %d. TxID = %d", offset, mbap.TxID);
|
|
_OnModbusReceive2OnePacket(buffer, offset, mbap); // Parse the resulting telegram
|
|
|
|
offset += __offset_of(struct ModbusApHeader, UnitID) + mbap.Length; // Move to the next telegram
|
|
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) // If the offset is not pointing at the end of the data
|
|
{ // This is a indicator that the length field in the Modbus Header was incorrect (attempt to spam the connection?)
|
|
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)
|
|
{
|
|
// Should we test the transaction identifier?
|
|
// Should we test the unit/device identifier?
|
|
word i; // counter
|
|
word length; // length of current packet
|
|
|
|
length = __offset_of(struct ModbusApHeader, UnitID) + mbap.Length; // the length of the complete telegram
|
|
|
|
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);
|
|
// either the length field is incorrect or the package is fragmented in the buffer
|
|
// 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!? Skip it.
|
|
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;
|
|
|
|
if (ex == Acknowledge) // An ACK resets the timeout timer
|
|
{
|
|
gQueueSent[mbap.TxID].TimeoutTicks = 0;
|
|
return;
|
|
}
|
|
// else report the error to the corresponding function
|
|
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 into 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;
|
|
}
|
|
} |