546 lines
20 KiB
C#
546 lines
20 KiB
C#
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);
|
|
}
|
|
}
|