953 lines
No EOL
27 KiB
Text
953 lines
No EOL
27 KiB
Text
/*@!Encoding:1252*/
|
|
// Additionally include ModbusTcpCommon.cin or ModbusUdpCommon.cin
|
|
includes
|
|
{
|
|
#include "ModbusCommonStructs.cin"
|
|
}
|
|
|
|
variables
|
|
{
|
|
const word gMaxPacketLength = __size_of(struct ModbusReqWriteRegisters);
|
|
|
|
// Global storage for pending requests, associated by funcCode
|
|
struct
|
|
{
|
|
byte Count;
|
|
word Length;
|
|
byte Buffer[gMaxPacketLength];
|
|
} gRequests[int64, 8];
|
|
|
|
|
|
|
|
msTimer gTimerModbusReadBits;
|
|
msTimer gTimerModbusReadRegisters;
|
|
msTimer gTimerModbusWriteBit;
|
|
msTimer gTimerModbusWriteRegister;
|
|
msTimer gTimerModbusWriteBits;
|
|
msTimer gTimerModbusWriteRegisters;
|
|
msTimer gTimerModbusWriteMasks;
|
|
msTimer gTimerModbusReadWriteRegisters;
|
|
}
|
|
|
|
void ModbusInit(char Remote_IP[], word Remote_Port)
|
|
{
|
|
sysDefineVariableString("%NETWORK_NAME%::%NODE_NAME%::Info", "IP", Remote_IP);
|
|
sysDefineVariableInt("%NETWORK_NAME%::%NODE_NAME%::Info", "Port", Remote_Port);
|
|
|
|
ModbusConnectTo(Remote_IP, Remote_Port);
|
|
}
|
|
|
|
void ModbusMakeHeader(struct ModbusApHeader mbap, word length)
|
|
{
|
|
mbap.TxID = 0x1234; // [2] Transaction ID. Not needed here?
|
|
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;
|
|
|
|
if (gRequests[funcCode].Count > 0) // TODO: a transmission is in progress. What shall we do?
|
|
return;
|
|
|
|
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
|
|
|
|
memcpy_h2n(buffer, mbr);
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, length);
|
|
gRequests[funcCode].Length = length;
|
|
gTimerModbusReadBits.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
void OnModbusReceiveBits(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x01;
|
|
struct ModbusResReceiveBits mbr;
|
|
byte bitStatus[1968];
|
|
word numBits;
|
|
byte i, j;
|
|
|
|
gTimerModbusReadBits.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbr, buffer);
|
|
|
|
numBits = (gRequests[funcCode].Buffer[10] << 8) + gRequests[funcCode].Buffer[11];
|
|
|
|
for (i = 0; i < mbr.ByteCount; i++)
|
|
{
|
|
for (j = 0; j < 8; j++)
|
|
{
|
|
bitStatus[8*i+j] = mbr.Data[i] & (0x01 << j);
|
|
}
|
|
}
|
|
|
|
OnModbusReadBitsSuccess(mbr, bitStatus, numBits);
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
void OnModbusReceiveBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x01;
|
|
|
|
gTimerModbusReadBits.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusReadBits>
|
|
on timer gTimerModbusReadBits
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
const byte funcCode = 0x01;
|
|
byte buffer[length];
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusReadBitsFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
memcpy(buffer, gRequests[funcCode].Buffer, length);
|
|
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
if (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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
|
|
|
|
memcpy_h2n(buffer, mbr);
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, length);
|
|
gRequests[funcCode].Length = length;
|
|
gTimerModbusReadRegisters.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
void OnModbusReceiveRegisters(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x03;
|
|
struct ModbusResReceiveRegisters mbr;
|
|
word numRegs;
|
|
|
|
gTimerModbusReadRegisters.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbr, buffer);
|
|
numRegs = (gRequests[funcCode].Buffer[10] << 8) + gRequests[funcCode].Buffer[11];
|
|
|
|
OnModbusReadRegistersSuccess(mbr, numRegs);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
void OnModbusReceiveRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x03;
|
|
|
|
gTimerModbusReadRegisters.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusReadRegisters>
|
|
on timer gTimerModbusReadRegisters
|
|
{
|
|
const byte length = __size_of(struct ModbusReqRead);
|
|
const byte funcCode = 0x03;
|
|
byte buffer[length];
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusReadRegistersFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
memcpy(buffer, gRequests[funcCode].Buffer, length);
|
|
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 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 (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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)
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, length);
|
|
gRequests[funcCode].Length = length;
|
|
gTimerModbusWriteBit.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
void OnModbusConfirmBit(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x05;
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
gTimerModbusWriteBit.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
OnModbusWriteBitSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
void OnModbusConfirmBitException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x05;
|
|
|
|
gTimerModbusWriteBit.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusWriteBit>
|
|
on timer gTimerModbusWriteBit
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x05;
|
|
byte buffer[length];
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusWriteBitFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
memcpy(buffer, gRequests[funcCode].Buffer, length);
|
|
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
if (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, length);
|
|
gRequests[funcCode].Length = length;
|
|
gTimerModbusWriteRegister.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
void OnModbusConfirmRegister(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x06;
|
|
struct ModbusResConfirmSingle mbc;
|
|
|
|
gTimerModbusWriteRegister.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
OnModbusWriteRegisterSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
void OnModbusConfirmRegisterException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x06;
|
|
|
|
gTimerModbusWriteRegister.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusWriteRegister>
|
|
on timer gTimerModbusWriteRegister
|
|
{
|
|
const byte length = __size_of(struct ModbusReqWriteSingle);
|
|
const byte funcCode = 0x06;
|
|
byte buffer[length];
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusWriteRegisterFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
memcpy(buffer, gRequests[funcCode].Buffer, length);
|
|
|
|
ModbusSend(buffer);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
|
|
|
|
// 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;
|
|
|
|
if (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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 -.-
|
|
|
|
memcpy_h2n(buffer, mbw);
|
|
|
|
ModbusSend(buffer, overallLength);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, overallLength);
|
|
gRequests[funcCode].Length = overallLength;
|
|
gTimerModbusWriteBits.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <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];
|
|
|
|
ellCount = elCount(values);
|
|
length = (word)_ceil(ellCount / 8.0);
|
|
//writeLineEx(0, 3, "ellCount: %d; length: %d", ellCount, length);
|
|
|
|
if (ellCount % 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;
|
|
//writeLineEx(0, 3, "j: %d; indx: %d; value: %d; mask: %X", j, i*8 + j, values[i*8 + j], (0x01 << j));
|
|
}
|
|
//writeLineEx(0, 3, "%d: %X", i, buffer[i]);
|
|
}
|
|
for (j = 0; j < ellCount % 8; j++) // wont be executed if there is no remainder
|
|
{
|
|
//writeLineEx(0, 3, "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;
|
|
}
|
|
//writeLineEx(0, 3, "%d: %X", length-1, buffer[length-1]);
|
|
|
|
hbin_to_strhex(values, str1);
|
|
bin_to_strhex(buffer, str2);
|
|
writeLineEx(0, 1, "<%NODE_NAME%> ModbusWriteBitsB: Encoded %s to %s", str1, str2);
|
|
ModbusWriteBits(address, count, buffer);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void OnModbusConfirmBits(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x0F;
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
gTimerModbusWriteBits.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
OnModbusWriteBitsSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
void OnModbusConfirmBitsException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x0F;
|
|
|
|
gTimerModbusWriteBits.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusWriteBits>
|
|
on timer gTimerModbusWriteBits
|
|
{
|
|
const byte funcCode = 0x0F;
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusWriteBitsFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
ModbusSend(gRequests[funcCode].Buffer, gRequests[funcCode].Length);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
|
|
|
|
// REGION: ModbusWriteRegisters -------------------------------------------------------
|
|
/// <ModbusWriteRegisters>
|
|
void ModbusWriteRegisters(word address, word count, int values[])
|
|
{
|
|
const word maxLength = __size_of(struct ModbusReqWriteRegisters);
|
|
const byte funcCode = 0x10;
|
|
byte buffer[maxLength];
|
|
struct ModbusReqWriteRegisters mbw;
|
|
byte dataLength;
|
|
word overallLength;
|
|
word i;
|
|
|
|
if (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, overallLength);
|
|
gRequests[funcCode].Length = overallLength;
|
|
gTimerModbusWriteRegisters.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
void OnModbusConfirmRegisters(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x10;
|
|
struct ModbusResConfirmMultiple mbc;
|
|
|
|
gTimerModbusWriteRegisters.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
OnModbusWriteRegistersSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
void OnModbusConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x10;
|
|
|
|
gTimerModbusWriteRegisters.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusWriteRegisters>
|
|
on timer gTimerModbusWriteRegisters
|
|
{
|
|
const byte funcCode = 0x10;
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusWriteRegistersFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
ModbusSend(gRequests[funcCode].Buffer, gRequests[funcCode].Length);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
// 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;
|
|
|
|
if (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, length);
|
|
gRequests[funcCode].Length = length;
|
|
gTimerModbusWriteMasks.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
void OnModbusConfirmMasks(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x15;
|
|
struct ModbusResConfirmMasks mbc;
|
|
|
|
gTimerModbusWriteMasks.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbc, buffer);
|
|
|
|
OnModbusWriteMasksSuccess(mbc);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
void OnModbusConfirmMasksException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x15;
|
|
|
|
gTimerModbusWriteMasks.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusWriteMasks>
|
|
on timer gTimerModbusWriteMasks
|
|
{
|
|
const byte funcCode = 0x16;
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusWriteMasksFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
ModbusSend(gRequests[funcCode].Buffer, gRequests[funcCode].Length);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
// 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;
|
|
|
|
if (gRequests[funcCode].Count > 0)
|
|
return;
|
|
|
|
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);
|
|
gRequests[funcCode].Count = 1;
|
|
memcpy(gRequests[funcCode].Buffer, buffer, overallLength);
|
|
gRequests[funcCode].Length = overallLength;
|
|
gTimerModbusWriteRegisters.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
void OnModbusReceiveConfirmRegisters(byte buffer[])
|
|
{
|
|
const byte funcCode = 0x17;
|
|
byte numRegs;
|
|
struct ModbusResReceiveRegisters mbr;
|
|
|
|
gTimerModbusReadWriteRegisters.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
memcpy_n2h(mbr, buffer);
|
|
numRegs = (gRequests[funcCode].Buffer[10] << 8) + gRequests[funcCode].Buffer[11];
|
|
|
|
OnModbusReadRegistersSuccess(mbr, numRegs);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
void OnModbusReceiveConfirmRegistersException(struct ModbusApHeader mbap, enum ModbusException ex)
|
|
{
|
|
const byte funcCode = 0x17;
|
|
|
|
gTimerModbusReadWriteRegisters.Cancel();
|
|
gRequests[funcCode].Count = 0;
|
|
|
|
OnModbusReadBitsFailed(Exception, ex, mbap);
|
|
}
|
|
|
|
/// <ModbusReadWriteRegisters>
|
|
on timer gTimerModbusReadWriteRegisters
|
|
{
|
|
const byte funcCode = 0x17;
|
|
struct ModbusApHeader mbap;
|
|
|
|
if (gRequests[funcCode].Count == @sysvar::Config::Modbus::MaxRetransmissionCount)
|
|
{
|
|
memcpy_n2h(mbap, gRequests[funcCode].Buffer);
|
|
OnModbusReadWriteRegistersFailed(Timeout, None, mbap);
|
|
gRequests[funcCode].Count = 0;
|
|
return;
|
|
}
|
|
|
|
ModbusSend(gRequests[funcCode].Buffer, gRequests[funcCode].Length);
|
|
gRequests[funcCode].Count++;
|
|
this.Set(@sysvar::Config::Modbus::RequestTimeout);
|
|
ModbusRecv();
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
// ------------------------------------------------------------------------------------
|
|
// REGION: OnModbusReceive ------------------------------------------------------------
|
|
/// <OnModbusReceive>
|
|
void OnModbusReceive(dword socket, long result, dword address, dword port, byte buffer[], dword size)
|
|
{
|
|
if (result == 0)
|
|
{
|
|
if (size == 0)
|
|
{
|
|
// Size of zero indicates that the socket was closed by the communication peer.
|
|
writeLineEx(0, 2, "<%NODE_NAME%> OnTcpReceive: Socket closed by peer");
|
|
gSocket.Close();
|
|
gSocketState = CLOSED;
|
|
}
|
|
else
|
|
{
|
|
// Sucessfully received some bytes over the TCP/IP connection.
|
|
OnModbusReceive2(buffer, size);
|
|
}
|
|
}
|
|
else
|
|
{
|
|
gIpLastErr = gSocket.GetLastSocketError();
|
|
gSocket.GetLastSocketErrorAsString(gIpLastErrStr, elcount(gIpLastErrStr));
|
|
writeLineEx(0, 2, "<%NODE_NAME%> OnTcpReceive error (%d): %s", gIpLastErr, gIpLastErrStr);
|
|
gSocket.Close();
|
|
gSocketState = CLOSED;
|
|
}
|
|
}
|
|
|
|
/// <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;
|
|
//write("OnModbusReceive2: size = %d", size);
|
|
do
|
|
{
|
|
//write("OnModbusReceive2: offset pre = %d", offset);
|
|
memcpy_n2h(mbap, buffer, offset);
|
|
OnModbusReceive2OnePacket(buffer, offset, mbap);
|
|
|
|
offset += __offset_of(struct ModbusApHeader, UnitID) + mbap.Length;
|
|
//write("OnModbusReceive2: offset post = %d. %d <= %d?", offset, offset, size-8);
|
|
}
|
|
while(offset <= size-8); // We need at least 8 bytes for a new packet
|
|
//write("OnModbusReceive2: yes. finished", offset);
|
|
|
|
if (offset != size) // Can be removed.
|
|
{
|
|
bin_to_strhex(buffer, str);
|
|
write("Error while going through receive buffer. Our final offset is %d, but the size of the buffer is %d! Buffer: %s", offset, size, str);
|
|
runError(1002, 1);
|
|
}
|
|
}
|
|
|
|
/// <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 (elCount(buffer) < offset + length) // TCP packet not as large enough
|
|
{
|
|
writeLineEx(0, 3, "<%NODE_NAME%> OnModbusReceive Error: Modbus 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.
|
|
runError(1001,1);
|
|
return;
|
|
}
|
|
if (mbap.Protocol != 0) // Protocol is not Modbus (0x0000). Wayne.
|
|
{
|
|
writeLineEx(0, 2, "<%NODE_NAME%> OnModbusReceive Error: Tcp packet is no Modbus packet: Protocol = %d", mbap.Protocol);
|
|
return;
|
|
}
|
|
// MBAP Header is OK :) Go on
|
|
|
|
if (mbap.FuncCode > 0x80) // Oh no, we got a exception!
|
|
{
|
|
OnModbusReceive2Exceptions(buffer[offset+08], mbap);
|
|
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:
|
|
writeLineEx(0, 3, "We received funcCode 0x%X?", mbap.FuncCode);
|
|
}
|
|
}
|
|
|
|
/// <OnModbusReceive>
|
|
void OnModbusReceive2Exceptions(byte exCode, struct ModbusApHeader mbap)
|
|
{
|
|
enum ModbusException ex;
|
|
ex = (enum ModbusException)exCode;
|
|
switch(exCode) // Let's have a look at the reason
|
|
{
|
|
case 0x01: // Illegal function code: Implementation failure!
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Illegal function code (0x01). The function code %d is unknown by the server.", mbap.FuncCode & 0x0F);
|
|
break;
|
|
case 0x02: // Illegal data address: Configuration failure!
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Illegal data address (0x02). Please check your configuration!");
|
|
break;
|
|
case 0x03: // Illegal data value: Depends.
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Illegal data value (0x03).");
|
|
break;
|
|
case 0x04: // Server failure: We can't do anything about that, can we?
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Server failure (0x04). The server failed during execution.");
|
|
break;
|
|
case 0x05: // Acknowledge: That's ok.
|
|
writeLineEx(0, 1, "<%NODE_NAME%> Exception: Acknowledge (0x05). The server simply needs more time to generate the response.");
|
|
break;
|
|
case 0x06: // Server busy: We should resend the request.
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Server busy (0x06). The request could not be accepted.");
|
|
break;
|
|
case 0x0A: // Gateway problem: We don't have gateways :)
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Gateway problem (0x0A). Gateway paths not available.");
|
|
break;
|
|
case 0x0B: // Gateway problem: We don't have gateways :)
|
|
writeLineEx(0, 3, "<%NODE_NAME%> Exception: Gateway problem (0x0B). The targeted device failed to respond.");
|
|
break;
|
|
}
|
|
|
|
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;
|
|
}
|
|
} |