Add project files.
This commit is contained in:
545
ModbusSlave.cs
Normal file
545
ModbusSlave.cs
Normal file
@@ -0,0 +1,545 @@
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Diagnostics;
|
||||
using System.Linq;
|
||||
using System.Net.Sockets;
|
||||
using System.Runtime.CompilerServices;
|
||||
using System.Text;
|
||||
using System.Threading.Tasks;
|
||||
using System.Threading;
|
||||
using System.Windows.Input;
|
||||
|
||||
namespace FAtoPA
|
||||
{
|
||||
// Source : https://www.simplymodbus.ca/TCP.htm
|
||||
// Source : https://www.modbustools.com/modbus.html
|
||||
|
||||
internal class ModbusSlave
|
||||
{
|
||||
public static bool Started = false;
|
||||
TcpListener listener;
|
||||
Dictionary<String, ModbusClientRecord> clientmap;
|
||||
UInt16[] Registers;
|
||||
Boolean running = false;
|
||||
public byte ID { get; set; } = 1;
|
||||
|
||||
private ModbusSlaveEvent _event;
|
||||
|
||||
/// <summary>
|
||||
/// Modbus Slave is actually a server (TCPListener),<br/>
|
||||
/// that contain data and can be read or written by <br/>
|
||||
/// a Modbus Master (which actually a TCPClient)
|
||||
/// </summary>
|
||||
/// <param name="maxregister">Maximum Register that can be read or written</param>
|
||||
public ModbusSlave(ModbusSlaveEvent callback, UInt16 maxregister)
|
||||
{
|
||||
_event = callback;
|
||||
clientmap = new Dictionary<String, ModbusClientRecord>();
|
||||
Registers = new UInt16[maxregister];
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Register Count available in Modbus Slave
|
||||
/// </summary>
|
||||
/// <returns>Register Count</returns>
|
||||
public UInt16 GetRegisterCount()
|
||||
{
|
||||
return (UInt16)Registers.Length;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Set Register Value
|
||||
/// </summary>
|
||||
/// <param name="address">start from 0 until GetRegisterCount() - 1</param>
|
||||
/// <param name="value">between 0 until 65535</param>
|
||||
public void SetRegister(UInt16 address, UInt16 value)
|
||||
{
|
||||
if (address < Registers.Length)
|
||||
{
|
||||
Registers[address] = value;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Register Value
|
||||
/// </summary>
|
||||
/// <param name="address">start from 0 until GetRegisterCount() - 1 </param>
|
||||
/// <returns>between 0 until 65535</returns>
|
||||
public UInt16 GetRegister(UInt16 address)
|
||||
{
|
||||
if (address < Registers.Length)
|
||||
{
|
||||
return Registers[address];
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Set Bit Value in Register
|
||||
/// </summary>
|
||||
/// <param name="address">start from 0 until GetRegisterCount() -1 </param>
|
||||
/// <param name="bit">start from 0 until 15</param>
|
||||
/// <param name="value">true = ON, false = OFF</param>
|
||||
public void SetBit(UInt16 address, UInt16 bit, Boolean value)
|
||||
{
|
||||
if (address < Registers.Length)
|
||||
{
|
||||
UInt16 reg = Registers[address];
|
||||
if (value)
|
||||
{
|
||||
reg |= (UInt16)(1 << bit);
|
||||
}
|
||||
else
|
||||
{
|
||||
reg &= (UInt16)~(1 << bit);
|
||||
}
|
||||
Registers[address] = reg;
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Bit Value in Register
|
||||
/// </summary>
|
||||
/// <param name="address">start from 0 until 65535</param>
|
||||
/// <param name="bit"></param>
|
||||
/// <returns></returns>
|
||||
public Boolean GetBit(UInt16 address, UInt16 bit)
|
||||
{
|
||||
if (address < Registers.Length)
|
||||
{
|
||||
UInt16 reg = Registers[address];
|
||||
return (reg & (1 << bit)) != 0;
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Start Modbus Slave
|
||||
/// </summary>
|
||||
/// <param name="port">Listening Port</param>
|
||||
/// <returns>true if success</returns>
|
||||
public bool Start(UInt16 port)
|
||||
{
|
||||
|
||||
try
|
||||
{
|
||||
TcpListener newlistener = new TcpListener(System.Net.IPAddress.Any, port);
|
||||
newlistener.Start();
|
||||
|
||||
new Thread(()=> DoListening(newlistener, port)).Start();
|
||||
if (_event != null) _event.ConnectStatus(true, "Modbus Listener Started at port "+port);
|
||||
return true;
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
if (_event!=null) _event.ConnectStatus(false, "Modbus Listener failed to start at port "+port+", Message : "+ e.Message);
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Stop Modbus Slave
|
||||
/// </summary>
|
||||
public void Stop()
|
||||
{
|
||||
running = false;
|
||||
if (listener != null)
|
||||
{
|
||||
try
|
||||
{
|
||||
|
||||
listener.Stop();
|
||||
if (_event != null)
|
||||
{
|
||||
_event.ConnectStatus(true, "ModbusSlave: Stop Success");
|
||||
}
|
||||
} catch (Exception e)
|
||||
{
|
||||
if (_event != null)
|
||||
{
|
||||
_event.ConnectStatus(false, "ModbusSlave: Stop failed, exception: " + e.Message);
|
||||
}
|
||||
}
|
||||
listener = null;
|
||||
}
|
||||
foreach (ModbusClientRecord client in clientmap.Values)
|
||||
{
|
||||
CloseConnection(client.Client);
|
||||
if (_event != null)
|
||||
{
|
||||
_event.CloseConnection(client);
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
private void CloseConnection(TcpClient client)
|
||||
{
|
||||
if (client != null)
|
||||
{
|
||||
client.Close();
|
||||
}
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Transaction ID from Modbus Command<br/>
|
||||
/// TransactionID is 2 bytes at index [0,1]
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="cmd">Modbus Command</param>
|
||||
/// <returns>TransactionID or 0 if failed</returns>
|
||||
private UInt16 GetTransactionID(byte[] cmd)
|
||||
{
|
||||
if (cmd.Length >= 2)
|
||||
{
|
||||
return (UInt16)((cmd[0] << 8) + cmd[1]);
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Protocol ID from Modbus Command<br/>
|
||||
/// ProtocolID is 2 bytes at index [2,3]<br/>
|
||||
/// For TCP Modbus, ProtocolID is always 0
|
||||
/// </summary>
|
||||
/// <param name="cmd">Modbus Command</param>
|
||||
/// <returns>ProtocolID</returns>
|
||||
private UInt16 GetProtocolID(byte[] cmd)
|
||||
{
|
||||
if (cmd.Length >= 4)
|
||||
{
|
||||
return (UInt16)((cmd[2] << 8) + cmd[3]);
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Length from Modbus Command<br/>
|
||||
/// Length is 2 bytes at index [4,5]<br/>
|
||||
/// Length is Modbus Payload Length + 2 (Unit ID and Function Code)
|
||||
/// </summary>
|
||||
/// <param name="cmd">Modbus Command</param>
|
||||
/// <returns>Length or 0 if failed</returns>
|
||||
private UInt16 GetLength(byte[] cmd)
|
||||
{
|
||||
if (cmd.Length >= 6)
|
||||
{
|
||||
return (UInt16)((cmd[4] << 8) + cmd[5]);
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Unit ID from Modbus Command<br/>
|
||||
/// UnitID is 1 byte at index [6]
|
||||
/// For Modbus Slave, UnitID is always 1
|
||||
/// </summary>
|
||||
/// <param name="cmd">Modbus Command</param>
|
||||
/// <returns>Unit ID or 0 if failed</returns>
|
||||
private Byte GetUnitID(byte[] cmd)
|
||||
{
|
||||
if (cmd.Length >= 7)
|
||||
{
|
||||
return cmd[6];
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Function Code from Modbus Command<br/>
|
||||
/// Function Code is 1 byte at index [7]
|
||||
///
|
||||
/// </summary>
|
||||
/// <param name="cmd">Modbus Command</param>
|
||||
/// <returns>Function Code or 0 if failed</returns>
|
||||
private Byte GetFunctionCode(byte[] cmd)
|
||||
{
|
||||
if (cmd.Length >= 8)
|
||||
{
|
||||
return cmd[7];
|
||||
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
|
||||
/// <summary>
|
||||
/// Get Modbus Payload from Modbus Command<br/>
|
||||
/// Modbus Payload begins at index 8
|
||||
/// </summary>
|
||||
/// <param name="cmd">Modbus Command</param>
|
||||
/// <returns>Modbus Payload or empty array if failed</returns>
|
||||
private byte[] GetModbusPayload(byte[] cmd)
|
||||
{
|
||||
if (cmd.Length > 8)
|
||||
{
|
||||
byte[] data = new byte[cmd.Length - 8];
|
||||
Array.Copy(cmd,8, data, 0, data.Length);
|
||||
return data;
|
||||
}
|
||||
return Array.Empty<byte>();
|
||||
}
|
||||
|
||||
async void DoListening(TcpListener listener, int port)
|
||||
{
|
||||
running = true;
|
||||
if (_event != null) _event.Log("ModbusSlave: Start Accepting Connections on port " + port);
|
||||
while (running)
|
||||
{
|
||||
TcpClient client = null;
|
||||
String key = null;
|
||||
try
|
||||
{
|
||||
client = await listener.AcceptTcpClientAsync();
|
||||
key = client.Client.RemoteEndPoint?.ToString();
|
||||
if (key == null)
|
||||
{
|
||||
CloseConnection(client);
|
||||
client = null;
|
||||
}
|
||||
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
if (_event != null) _event.Log("ModbusSlave: Exception " + e.Message);
|
||||
}
|
||||
|
||||
if (key != null && client != null)
|
||||
{
|
||||
if (_event!=null) _event.Log("ModbusSlave: New Connection from " + key);
|
||||
ModbusClientRecord prev = clientmap[key];
|
||||
|
||||
if (prev != null)
|
||||
{
|
||||
CloseConnection(prev.Client);
|
||||
if (_event != null)
|
||||
{
|
||||
_event.CloseConnection(prev);
|
||||
}
|
||||
}
|
||||
|
||||
ModbusClientRecord newclient = new ModbusClientRecord(client);
|
||||
clientmap[key] = newclient;
|
||||
new Thread(() => DoCommunication(newclient)).Start();
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
if (_event!=null) _event.Log("ModbusSlave: Stop Accepting Connections on port " + port);
|
||||
}
|
||||
|
||||
async void DoCommunication(ModbusClientRecord client)
|
||||
{
|
||||
// komunikasi di sini
|
||||
String key = client.remoteEP ?? "";
|
||||
if (_event != null)
|
||||
{
|
||||
_event.NewConnection(client);
|
||||
_event.Log("ModbusSlave: Start Communication with " + key);
|
||||
}
|
||||
NetworkStream stream = client.GetStream();
|
||||
while (running && stream != null)
|
||||
{
|
||||
// spare 12 bytes untuk protocol modbus Read Register :
|
||||
// 2 bytes Transaction ID, index [0,1]
|
||||
// 2 bytes Protocol ID (always 0) , index [2,3]
|
||||
// 2 bytes Length , index [4,5]
|
||||
// 1 byte Unit ID, index [6]
|
||||
// 1 byte Function Code, index [7]
|
||||
// (3 = Read Holding Register)
|
||||
// 2 byte Register Address, index [8,9]
|
||||
// 2 byte Register Count, index [10,11]
|
||||
byte[] cmd = new byte[12];
|
||||
try {
|
||||
int readcount = await stream.ReadAsync(new byte[12], 0, 12);
|
||||
if (readcount >= 12)
|
||||
{
|
||||
client.RXBytes += (uint)readcount;
|
||||
UInt16 transactionID = GetTransactionID(cmd);
|
||||
UInt16 protocolID = GetProtocolID(cmd);
|
||||
UInt16 length = GetLength(cmd);
|
||||
Byte unitID = GetUnitID(cmd);
|
||||
Byte functionCode = GetFunctionCode(cmd);
|
||||
byte[] payload = GetModbusPayload(cmd);
|
||||
|
||||
if (protocolID == 0 && length == payload.Length + 2 && unitID == 1)
|
||||
{
|
||||
if (functionCode == 3)
|
||||
{
|
||||
//Implement Read Register
|
||||
UInt16 registerAddress = (UInt16)((payload[0] << 8) + payload[1]);
|
||||
UInt16 registerCount = (UInt16)((payload[2] << 8) + payload[3]);
|
||||
Boolean valid = (registerAddress + registerCount <= Registers.Length);
|
||||
byte[] response;
|
||||
if (valid)
|
||||
{
|
||||
response = new byte[9 + registerCount * 2];
|
||||
}
|
||||
else
|
||||
{
|
||||
// Register Address out of range
|
||||
response = new byte[9];
|
||||
}
|
||||
|
||||
// 2 bytes Transaction ID, index [0,1]
|
||||
response[0] = (byte)(transactionID >> 8);
|
||||
response[1] = (byte)(transactionID & 0xFF);
|
||||
// 2 bytes Protocol ID (always 0) , index [2,3]
|
||||
response[2] = 0;
|
||||
response[3] = 0;
|
||||
// 2 bytes Length , index [4,5]
|
||||
if (valid)
|
||||
{
|
||||
UInt16 _length = (UInt16)(3 + registerCount * 2);
|
||||
response[4] = (byte)(_length >> 8);
|
||||
response[5] = (byte)(_length & 0xFF);
|
||||
}
|
||||
else
|
||||
{
|
||||
response[4] = 0;
|
||||
response[5] = 3;
|
||||
}
|
||||
// Unit ID, index [6]
|
||||
response[6] = unitID;
|
||||
// Function Code, index [7]
|
||||
response[7] = valid ? functionCode : (byte)(functionCode + 0x80);
|
||||
|
||||
// Number of Register or Exception Code, index [8]
|
||||
if (valid)
|
||||
{
|
||||
response[8] = (byte)(registerCount * 2);
|
||||
// Register values, 2 bytes each , start from index [9]
|
||||
for (int i = 0; i < registerCount; i++)
|
||||
{
|
||||
UInt16 value = GetRegister((UInt16)(registerAddress + i));
|
||||
response[9 + i * 2] = (byte)(value >> 8);
|
||||
response[10 + i * 2] = (byte)(value & 0xFF);
|
||||
}
|
||||
client.RXValidRequest++;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Exception Code 2 : Illegal Data Address
|
||||
response[8] = 2;
|
||||
client.RXInvalidRequest++;
|
||||
}
|
||||
// Send Response
|
||||
await stream.WriteAsync(response, 0, response.Length);
|
||||
client.TXBytes += (uint)response.Length;
|
||||
client.TXResponse++;
|
||||
}
|
||||
else
|
||||
{
|
||||
client.RXInvalidRequest++;
|
||||
// exception, invalid function code
|
||||
if (_event != null) _event.Log("ModbusSlave: Invalid Function Code " + functionCode);
|
||||
byte[] response = new byte[9];
|
||||
// 2 bytes Transaction ID, index [0,1]
|
||||
response[0] = (byte)(transactionID >> 8);
|
||||
response[1] = (byte)(transactionID & 0xFF);
|
||||
// 2 bytes Protocol ID (always 0) , index [2,3]
|
||||
response[2] = (byte)(protocolID >> 8);
|
||||
response[3] = (byte)(protocolID & 0xFF);
|
||||
// 2 bytes Length , index [4,5]
|
||||
response[4] = 0;
|
||||
response[5] = 3;
|
||||
// Unit ID, index [6]
|
||||
response[6] = unitID;
|
||||
// Function Code, index [7]
|
||||
response[7] = (byte)(functionCode + 0x80);
|
||||
// Exception Code 1 : Illegal Function
|
||||
response[8] = 1;
|
||||
await stream.WriteAsync(response, 0, response.Length);
|
||||
client.TXBytes += (uint)response.Length;
|
||||
client.TXResponse++;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
client.RXInvalidRequest++;
|
||||
// exception, invalid protocol ID, length, or unit ID
|
||||
if (_event != null) _event.Log("ModbusSlave: Invalid Protocol ID, Length, or Unit ID");
|
||||
byte[] response = new byte[9];
|
||||
// 2 bytes Transaction ID, index [0,1]
|
||||
response[0] = (byte)(transactionID >> 8);
|
||||
response[1] = (byte)(transactionID & 0xFF);
|
||||
// 2 bytes Protocol ID (always 0) , index [2,3]
|
||||
response[2] = (byte)(protocolID >> 8);
|
||||
response[3] = (byte)(protocolID & 0xFF);
|
||||
// 2 bytes Length , index [4,5]
|
||||
response[4] = 0;
|
||||
response[5] = 3;
|
||||
// Unit ID, index [6]
|
||||
response[6] = unitID;
|
||||
// Function Code, index [7]
|
||||
response[7] = (byte)(functionCode + 0x80);
|
||||
// Exception Code 3 : Illegal Query
|
||||
response[8] = 3;
|
||||
await stream.WriteAsync(response, 0, response.Length);
|
||||
client.TXBytes += (uint)response.Length;
|
||||
client.TXResponse++;
|
||||
}
|
||||
}
|
||||
}
|
||||
catch (Exception e)
|
||||
{
|
||||
if (_event!=null) _event.Log("ModbusSlave: Exception " + e.Message);
|
||||
break;
|
||||
}
|
||||
|
||||
}
|
||||
if (_event != null) _event.Log("ModbusSlave: Stop Communication with " + key);
|
||||
CloseConnection(client.Client);
|
||||
clientmap.Remove(key);
|
||||
if (_event != null) _event.CloseConnection(client);
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
class ModbusClientRecord
|
||||
{
|
||||
public TcpClient Client;
|
||||
public UInt32 TXBytes, RXBytes;
|
||||
public UInt32 RXValidRequest, RXInvalidRequest, TXResponse;
|
||||
public String remoteEP;
|
||||
public ModbusClientRecord(TcpClient client)
|
||||
{
|
||||
Client = client;
|
||||
remoteEP = "";
|
||||
TXBytes = 0;
|
||||
RXBytes = 0;
|
||||
RXValidRequest = 0;
|
||||
RXInvalidRequest = 0;
|
||||
TXResponse = 0;
|
||||
if (Client != null)
|
||||
{
|
||||
if (Client.Connected)
|
||||
{
|
||||
if (Client.Client.RemoteEndPoint != null)
|
||||
{
|
||||
remoteEP = Client.Client.RemoteEndPoint.ToString();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public NetworkStream GetStream()
|
||||
{
|
||||
return Client?.GetStream() ?? null;
|
||||
}
|
||||
}
|
||||
|
||||
interface ModbusSlaveEvent
|
||||
{
|
||||
void Log(String msg);
|
||||
void NewConnection(ModbusClientRecord client);
|
||||
void CloseConnection(ModbusClientRecord client);
|
||||
void ConnectStatus(bool success, string message);
|
||||
void TXRXStatusUpdate(ModbusClientRecord client);
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user