DungTT/RobotNet
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

first commit -push

Browse Source
This commit is contained in:
dungtt
2025-10-15 15:15:53 +07:00
parent 674ae395be
commit a9577c5756
885 changed files with 74595 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

679
RobotNet.ScriptManager/Connections/CcLinkIeBasicClient.cs Normal file
View File

@@ -0,0 +1,679 @@
using RobotNet.Script;
using System.Buffers.Binary;
using System.Net;
using System.Net.Sockets;
namespace RobotNet.ScriptManager.Connections;
public class CcLinkIeBasicClient(IeBasicClientOptions options) : ICcLinkIeBasicClient
{
private readonly SemaphoreSlim _sendLock = new(1, 1);
private UdpClient? _udp;
private TcpClient? _tcp;
private NetworkStream? _tcpStream;
private IPEndPoint? _remoteEp;
private CancellationTokenSource? _pollCts;
private IeBasicStatus _status = new()
{
LinkUp = false,
ConsecutiveErrors = 0,
LastEndCode = null,
LastErrorText = null,
AvgRtt = null,
MaxRtt = null
};
private volatile bool _connected;
private long _rttCount;
private long _rttSumTicks;
public bool IsConnected => _connected;
public int RetryCount { get => options.RetryCount; set => options = options with { RetryCount = value }; }
public event EventHandler<PollUpdatedEventArgs>? Polled;
// ====== Public API ======
public async Task ConnectAsync(CancellationToken ct = default)
{
if (options.FrameFormat != SlmpFrameFormat.Format3E)
throw new NotSupportedException("Triển khai tham chiếu này hiện hỗ trợ 3E binary frame.");
if (options.Transport == IeBasicTransport.Udp)
{
_udp?.Dispose();
_udp = new UdpClient();
_udp.Client.ReceiveTimeout = (int)options.Timeout.TotalMilliseconds;
_udp.Client.SendTimeout = (int)options.Timeout.TotalMilliseconds;
var ip = await ResolveHostAsync(options.Host, ct);
_remoteEp = new IPEndPoint(ip, options.Port);
_udp.Connect(_remoteEp);
}
else
{
_tcp?.Close();
_tcp = new TcpClient
{
NoDelay = true,
ReceiveTimeout = (int)options.Timeout.TotalMilliseconds,
SendTimeout = (int)options.Timeout.TotalMilliseconds
};
var ip = await ResolveHostAsync(options.Host, ct);
await _tcp.ConnectAsync(ip, options.Port, ct);
_tcpStream = _tcp.GetStream();
}
_connected = true;
_status = _status with { LinkUp = true, ConsecutiveErrors = 0, LastErrorText = null, LastEndCode = null };
}
public Task DisconnectAsync(CancellationToken ct = default)
{
StopPollingInternal();
_connected = false;
try { _tcpStream?.Dispose(); } catch { }
try { _tcp?.Close(); } catch { }
try { _udp?.Dispose(); } catch { }
_tcpStream = null;
_tcp = null;
_udp = null;
_status = _status with { LinkUp = false };
return Task.CompletedTask;
}
public async Task<bool[]> ReadBitsAsync(DeviceCode device, int startAddress, int count, CancellationToken ct = default)
{
// 3E binary: Command 0x0401, Subcommand 0x0001 (bit units)
var payload = BuildBatchRead(device, startAddress, count, isBit: true);
var resp = await SendSlmpAsync(SlmpCommand.ReadDevice, payload, ct);
EnsureOk(resp);
// Bit data: packed per bit (LSB-first) theo manual. Ở đây đơn giản dùng từng byte -> từng bit.
return UnpackBits(resp.Payload.Span, count);
}
public async Task WriteBitsAsync(DeviceCode device, int startAddress, ReadOnlyMemory<bool> values, CancellationToken ct = default)
{
// 3E binary: Command 0x1401, Subcommand 0x0001 (bit units)
var payload = BuildBatchWriteBits(device, startAddress, values.Span);
var resp = await SendSlmpAsync(SlmpCommand.WriteDevice, payload, ct);
EnsureOk(resp);
}
public async Task<ushort[]> ReadWordsAsync(DeviceCode device, int startAddress, int wordCount, CancellationToken ct = default)
{
// 3E binary: Command 0x0401, Subcommand 0x0000 (word units)
var payload = BuildBatchRead(device, startAddress, wordCount, isBit: false);
var resp = await SendSlmpAsync(SlmpCommand.ReadDevice, payload, ct);
EnsureOk(resp);
// Word data: little-endian UInt16
if (resp.Payload.Length < wordCount * 2)
throw new InvalidOperationException("Payload ngắn hơn mong đợi.");
var span = resp.Payload.Span;
var result = new ushort[wordCount];
for (int i = 0; i < wordCount; i++)
result[i] = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(i * 2, 2));
return result;
}
public async Task WriteWordsAsync(DeviceCode device, int startAddress, ReadOnlyMemory<ushort> words, CancellationToken ct = default)
{
// 3E binary: Command 0x1401, Subcommand 0x0000 (word units)
var payload = BuildBatchWriteWords(device, startAddress, words.Span);
var resp = await SendSlmpAsync(SlmpCommand.WriteDevice, payload, ct);
EnsureOk(resp);
}
public async Task<uint[]> ReadDWordsAsync(DeviceCode device, int startAddress, int dwordCount, CancellationToken ct = default)
{
if (SupportsDirectDWordRead(device))
{
// 3E binary: Command 0x0401, Subcommand 0x0002 (dword units)
var payload = BuildBatchRead(device, startAddress, dwordCount, isBit: false, isDWord: true);
var resp = await SendSlmpAsync(SlmpCommand.ReadDevice, payload, ct);
EnsureOk(resp);
if (resp.Payload.Length < dwordCount * 4)
throw new InvalidOperationException("Payload ngắn hơn mong đợi.");
var span = resp.Payload.Span;
var result = new uint[dwordCount];
for (int i = 0; i < dwordCount; i++)
result[i] = BinaryPrimitives.ReadUInt32LittleEndian(span.Slice(i * 4, 4));
return result;
}
else
{
// Đọc dword như đọc 2 word/liên tiếp
var words = await ReadWordsAsync(device, startAddress, dwordCount * 2, ct);
var result = new uint[dwordCount];
if (options.LittleEndianWordOrder)
{
for (int i = 0, j = 0; i < dwordCount; i++, j += 2)
result[i] = (uint)(words[j] | ((uint)words[j + 1] << 16));
}
else
{
for (int i = 0, j = 0; i < dwordCount; i++, j += 2)
result[i] = (uint)(words[j + 1] | ((uint)words[j] << 16));
}
return result;
}
}
public async Task WriteDWordsAsync(DeviceCode device, int startAddress, ReadOnlyMemory<uint> dwords, CancellationToken ct = default)
{
var span = dwords.Span;
var buf = new ushort[span.Length * 2];
if (options.LittleEndianWordOrder)
{
for (int i = 0, j = 0; i < span.Length; i++, j += 2)
{
buf[j] = (ushort)(span[i] & 0xFFFF);
buf[j + 1] = (ushort)((span[i] >> 16) & 0xFFFF);
}
}
else
{
for (int i = 0, j = 0; i < span.Length; i++, j += 2)
{
buf[j] = (ushort)((span[i] >> 16) & 0xFFFF);
buf[j + 1] = (ushort)(span[i] & 0xFFFF);
}
}
await WriteWordsAsync(device, startAddress, buf, ct);
}
public async Task<ushort[]> ReadRandomWordsAsync((DeviceCode Device, int Address)[] points, CancellationToken ct = default)
{
// 3E binary: Read Random (0x0403). Payload: điểm (addr+devcode)
var payload = BuildRandomRead(points);
var resp = await SendSlmpAsync(SlmpCommand.ReadRandom, payload, ct);
EnsureOk(resp);
// Trả về word liên tiếp
var span = resp.Payload.Span;
if (span.Length < points.Length * 2) throw new InvalidOperationException("Payload ngắn hơn mong đợi.");
var result = new ushort[points.Length];
for (int i = 0; i < points.Length; i++)
result[i] = BinaryPrimitives.ReadUInt16LittleEndian(span.Slice(i * 2, 2));
return result;
}
public async Task WriteRandomWordsAsync((DeviceCode Device, int Address)[] points, ReadOnlyMemory<ushort> values, CancellationToken ct = default)
{
if (points.Length != values.Length)
throw new ArgumentException("Số điểm và số giá trị không khớp.");
var payload = BuildRandomWrite(points, values.Span);
var resp = await SendSlmpAsync(SlmpCommand.WriteRandom, payload, ct);
EnsureOk(resp);
}
public async Task<IeBasicStatus> GetStatusAsync(CancellationToken ct = default)
{
// Có thể gửi NodeMonitor để “làm nóng” trạng thái; ở đây trả về snapshot hiện có
await Task.Yield();
return _status;
}
public async Task<bool> PingAsync(CancellationToken ct = default)
{
try
{
// Node monitor: dùng CMD DeviceInfo/NodeMonitor rỗng để kiểm phản hồi
var resp = await SendSlmpAsync(SlmpCommand.NodeMonitor, ReadOnlyMemory<byte>.Empty, ct);
return resp.EndCode == SlmpEndCode.Completed;
}
catch
{
return false;
}
}
public async Task<ModuleIdentity> IdentifyAsync(CancellationToken ct = default)
{
// Tối giản: gửi DeviceInfo và parse sơ bộ (tuỳ PLC mà payload khác nhau)
var resp = await SendSlmpAsync(SlmpCommand.DeviceInfo, ReadOnlyMemory<byte>.Empty, ct);
// Ở đây không chuẩn hoá do payload phụ thuộc model; trả về khung rỗng để người dùng tự giải.
return new ModuleIdentity
{
Vendor = "Mitsubishi Electric",
AdditionalInfo = $"Raw bytes: {resp.Payload.Length} (xem manual model để parse)"
};
}
public async Task<SlmpResponse> SendSlmpAsync(SlmpCommand command, ReadOnlyMemory<byte> payload, CancellationToken ct = default)
{
if (!_connected) throw new InvalidOperationException("Chưa kết nối.");
// Dựng 3E frame
var req = Build3EFrame(options, (ushort)command, subcommand: CurrentSubcommandHint, payload);
// Gửi/nhận với retry
var sw = System.Diagnostics.Stopwatch.StartNew();
for (int attempt = 0; attempt <= RetryCount; attempt++)
{
try
{
var rawResp = await SendAndReceiveAsync(req, ct);
var resp = Parse3EResponse(rawResp);
sw.Stop();
TrackRtt(sw.Elapsed);
_status = _status with
{
LinkUp = true,
ConsecutiveErrors = 0,
LastEndCode = resp.EndCode,
LastErrorText = null,
AvgRtt = TimeSpan.FromTicks((_rttCount > 0) ? _rttSumTicks / _rttCount : 0),
MaxRtt = (_status.MaxRtt == null || sw.Elapsed > _status.MaxRtt) ? sw.Elapsed : _status.MaxRtt
};
return resp;
}
catch (Exception ex) when (attempt < RetryCount)
{
_status = _status with
{
LinkUp = false,
ConsecutiveErrors = _status.ConsecutiveErrors + 1,
LastErrorText = ex.Message,
LastEndCode = SlmpEndCode.TransportError
};
// thử lại
await Task.Delay(10, ct);
}
}
sw.Stop();
throw new TimeoutException("SLMP: Hết retry mà vẫn lỗi.");
}
public async ValueTask DisposeAsync()
{
await DisconnectAsync();
_sendLock.Dispose();
GC.SuppressFinalize(this);
}
public void Dispose()
{
DisposeAsync().AsTask().GetAwaiter().GetResult();
GC.SuppressFinalize(this);
}
public void StartPolling(PollOptions options, CancellationToken ct = default)
{
StopPollingInternal();
_pollCts = CancellationTokenSource.CreateLinkedTokenSource(ct);
var token = _pollCts.Token;
_ = Task.Run(async () =>
{
// Strategy: gom vùng bit và word/dword riêng để tối ưu
while (!token.IsCancellationRequested && _connected)
{
try
{
// Đơn giản: chỉ đọc word; nếu có bit thì đọc riêng
var wordsAreas = options.Areas;
ushort[]? words = null;
bool[]? bits = null;
// (Minh hoạ) Đọc tất cả vùng word liên tiếp bằng cách gộp, bạn có thể tối ưu thêm theo thiết bị.
if (wordsAreas.Length > 0)
{
// Đây đọc lần lượt từng vùng cho đơn giản (dễ hiểu; bạn có thể hợp nhất để giảm gói)
var aggWords = new System.Collections.Generic.List<ushort>();
foreach (var (Device, Start, Count) in wordsAreas)
{
if (Device == DeviceCode.X || Device == DeviceCode.Y || Device == DeviceCode.M ||
Device == DeviceCode.L || Device == DeviceCode.S || Device == DeviceCode.B)
{
// Bit area
var b = await ReadBitsAsync(Device, Start, Count, token);
// Nối bit (minh hoạ): không ghép; đẩy riêng
bits = b;
}
else
{
var w = await ReadWordsAsync(Device, Start, Count, token);
aggWords.AddRange(w);
}
}
if (aggWords.Count > 0) words = [.. aggWords];
}
Polled?.Invoke(this, new PollUpdatedEventArgs
{
Timestamp = DateTimeOffset.UtcNow,
Bits = bits,
Words = words,
DWords = null
});
await Task.Delay(options.Interval, token);
}
catch (OperationCanceledException) { }
catch
{
// bỏ qua vòng lỗi và tiếp tục (tuỳ nhu cầu: bạn có thể tăng backoff)
await Task.Delay(TimeSpan.FromMilliseconds(50), token);
}
}
}, token);
}
public Task StopPollingAsync(CancellationToken ct = default)
{
StopPollingInternal();
return Task.CompletedTask;
}
// ====== Internal helpers ======
private void StopPollingInternal()
{
try { _pollCts?.Cancel(); } catch { }
try { _pollCts?.Dispose(); } catch { }
_pollCts = null;
}
private static async Task<IPAddress> ResolveHostAsync(string host, CancellationToken ct)
{
if (IPAddress.TryParse(host, out var ip)) return ip;
var entry = await Dns.GetHostEntryAsync(host, ct);
foreach (var addr in entry.AddressList)
if (addr.AddressFamily == AddressFamily.InterNetwork) return addr;
throw new InvalidOperationException("Không tìm thấy IPv4 cho host.");
}
private void TrackRtt(TimeSpan elapsed)
{
var ticks = elapsed.Ticks;
System.Threading.Interlocked.Add(ref _rttSumTicks, ticks);
System.Threading.Interlocked.Increment(ref _rttCount);
}
private static void EnsureOk(SlmpResponse resp)
{
if (resp.EndCode != SlmpEndCode.Completed)
throw new InvalidOperationException($"SLMP EndCode: 0x{(ushort)resp.EndCode:X4}");
}
// ====== SLMP 3E Binary Build/Parse ======
// Gợi ý subcommand hiện hành (đặt trước khi Build3EFrame): 0x0000=word, 0x0001=bit
private ushort CurrentSubcommandHint = 0x0000;
private static byte[] Build3EFrame(IeBasicClientOptions opt, ushort command, ushort subcommand, ReadOnlyMemory<byte> userData)
{
// Tài liệu 3E binary (TCP): Subheader 0x5000; (UDP): 0x5400
// Header:
// [0-1] Subheader (LE)
// [2] Network No
// [3] PC No (để 0x00)
// [4-5] Module I/O No (LE)
// [6] Multidrop No
// [7-8] Data Length (LE) = 2(timer) + 2(cmd) + 2(subcmd) + payload.Length
// [9-10] Timer (LE) đơn vị 250 ms hoặc 1? (tuỳ manual; thường 0x0010 ~ 4s). Ở đây lấy theo Timeout ~ ms/10.
// [11-12] Command (LE)
// [13-14] Subcommand (LE)
// [15..] Payload
var subheader = (opt.Transport == IeBasicTransport.Udp) ? (ushort)0x5400 : (ushort)0x5000;
var dataLen = (ushort)(2 + 2 + 2 + userData.Length); // timer + cmd + subcmd + payload
// Timer: convert từ Timeout ~ ms -> giá trị phù hợp. Ở đây minh hoạ: ms/10 (tuỳ manual).
var timer = (ushort)Math.Clamp((int)(opt.Timeout.TotalMilliseconds / 10.0), 1, 0xFFFE);
var buf = new byte[15 + userData.Length];
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(0, 2), subheader);
buf[2] = opt.NetworkNo;
buf[3] = 0x00; // PC No
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), opt.ModuleIoNo);
buf[6] = opt.MultidropNo;
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(7, 2), dataLen);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(9, 2), timer);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(11, 2), command);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(13, 2), subcommand);
userData.Span.CopyTo(buf.AsSpan(15));
return buf;
}
private async Task<byte[]> SendAndReceiveAsync(byte[] request, CancellationToken ct)
{
await _sendLock.WaitAsync(ct).ConfigureAwait(false);
try
{
if (options.Transport == IeBasicTransport.Udp)
{
if (_udp is null) throw new InvalidOperationException("UDP client null.");
await _udp.SendAsync(request, request.Length);
var recv = await _udp.ReceiveAsync(ct);
return recv.Buffer;
}
else
{
if (_tcpStream is null) throw new InvalidOperationException("TCP stream null.");
// Gửi request
await _tcpStream.WriteAsync(request, ct);
await _tcpStream.FlushAsync(ct);
// Đọc header tối thiểu 15 bytes để biết data length
var header = new byte[15];
await ReadExactAsync(_tcpStream, header, ct);
// Lấy data length
var dataLen = BinaryPrimitives.ReadUInt16LittleEndian(header.AsSpan(7, 2));
var rest = new byte[dataLen]; // bao gồm timer/cmd/subcmd/endcode/payload (ở response thay timer bằng EndCode)
await ReadExactAsync(_tcpStream, rest, ct);
// Gộp lại thành frame đầy đủ cho parser
var combined = new byte[header.Length + rest.Length];
Buffer.BlockCopy(header, 0, combined, 0, header.Length);
Buffer.BlockCopy(rest, 0, combined, header.Length, rest.Length);
return combined;
}
}
finally
{
_sendLock.Release();
}
}
private static async Task ReadExactAsync(NetworkStream stream, byte[] buffer, CancellationToken ct)
{
int read = 0;
while (read < buffer.Length)
{
var n = await stream.ReadAsync(buffer.AsMemory(read, buffer.Length - read), ct);
if (n == 0) throw new SocketException((int)SocketError.ConnectionReset);
read += n;
}
}
private static SlmpResponse Parse3EResponse(byte[] frame)
{
// Response 3E:
// [0-1] Subheader
// [2] NetworkNo
// [3] PCNo
// [4-5] ModuleIoNo
// [6] MultidropNo
// [7-8] Data Length (LE)
// [9-10] End Code (LE) <-- khác request (timer)
// [11..] Payload
if (frame.Length < 11)
throw new InvalidOperationException("Frame quá ngắn.");
var dataLen = BinaryPrimitives.ReadUInt16LittleEndian(frame.AsSpan(7, 2));
if (frame.Length < 11 + dataLen)
throw new InvalidOperationException("Frame không đủ chiều dài.");
var endCode = (SlmpEndCode)BinaryPrimitives.ReadUInt16LittleEndian(frame.AsSpan(9, 2));
var payload = new ReadOnlyMemory<byte>(frame, 11, dataLen - 2); // trừ endcode (2B)
return new SlmpResponse { EndCode = endCode, Payload = payload, Timestamp = DateTimeOffset.UtcNow };
}
// ====== Payload builders (3E binary) ======
private static byte DevCodeBinary(DeviceCode device) => device switch
{
// Mã phổ biến trong MC 3E binary (tham chiếu; kiểm tra manual cho chắc theo CPU):
DeviceCode.X => 0x9C,
DeviceCode.Y => 0x9D,
DeviceCode.M => 0x90,
DeviceCode.L => 0x92,
DeviceCode.B => 0xA0,
DeviceCode.S => 0x98,
DeviceCode.D => 0xA8,
DeviceCode.W => 0xB4,
DeviceCode.R => 0xAF,
DeviceCode.ZR => 0xB0,
_ => 0x00
};
private ReadOnlyMemory<byte> BuildBatchWriteWords(DeviceCode device, int start, ReadOnlySpan<ushort> words)
{
CurrentSubcommandHint = 0x0000; // word units
var buf = new byte[3 + 1 + 2 + words.Length * 2];
buf[0] = (byte)(start & 0xFF);
buf[1] = (byte)((start >> 8) & 0xFF);
buf[2] = (byte)((start >> 16) & 0xFF);
buf[3] = DevCodeBinary(device);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), (ushort)words.Length);
var off = 6;
for (int i = 0; i < words.Length; i++, off += 2)
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(off, 2), words[i]);
return buf;
}
private ReadOnlyMemory<byte> BuildBatchWriteBits(DeviceCode device, int start, ReadOnlySpan<bool> bits)
{
CurrentSubcommandHint = 0x0001; // bit units
// Bit packing theo MC (1 bit/byte hoặc 2 bit/byte tuỳ subcmd). Ở đây dùng 1 bit/bit (1 byte/bit) để đơn giản;
// nhiều PLC chấp nhận. Nếu cần tối ưu, gói bit theo nibble.
var buf = new byte[3 + 1 + 2 + bits.Length];
buf[0] = (byte)(start & 0xFF);
buf[1] = (byte)((start >> 8) & 0xFF);
buf[2] = (byte)((start >> 16) & 0xFF);
buf[3] = DevCodeBinary(device);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), (ushort)bits.Length);
for (int i = 0; i < bits.Length; i++)
buf[6 + i] = (byte)(bits[i] ? 0x01 : 0x00);
return buf;
}
private static ReadOnlyMemory<byte> BuildRandomRead((DeviceCode Device, int Address)[] points)
{
// Format tối giản: [count(2)] + N * (addr(3) + dev(1))
var buf = new byte[2 + points.Length * 4];
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(0, 2), (ushort)points.Length);
var off = 2;
foreach (var (Device, Address) in points)
{
buf[off + 0] = (byte)(Address & 0xFF);
buf[off + 1] = (byte)((Address >> 8) & 0xFF);
buf[off + 2] = (byte)((Address >> 16) & 0xFF);
buf[off + 3] = DevCodeBinary(Device);
off += 4;
}
return buf;
}
private static ReadOnlyMemory<byte> BuildRandomWrite((DeviceCode Device, int Address)[] points, ReadOnlySpan<ushort> values)
{
// [count(2)] + N*(addr(3)+dev(1)+value(2))
var buf = new byte[2 + points.Length * (4 + 2)];
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(0, 2), (ushort)points.Length);
var off = 2;
for (int i = 0; i < points.Length; i++)
{
var (Device, Address) = points[i];
buf[off + 0] = (byte)(Address & 0xFF);
buf[off + 1] = (byte)((Address >> 8) & 0xFF);
buf[off + 2] = (byte)((Address >> 16) & 0xFF);
buf[off + 3] = DevCodeBinary(Device);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(off + 4, 2), values[i]);
off += 6;
}
return buf;
}
private static bool[] UnpackBits(ReadOnlySpan<byte> data, int expectedCount)
{
// Đơn giản hoá: nếu payload trả về mỗi bit = 1 byte (0x00/0x01)
// Nếu PLC trả về packed bits, bạn cần giải theo bit-level. Ở đây xử lý cả hai:
if (data.Length == expectedCount)
{
var res = new bool[expectedCount];
for (int i = 0; i < expectedCount; i++) res[i] = data[i] != 0;
return res;
}
else
{
// Giải packed: LSB-first trong từng byte
var res = new bool[expectedCount];
int idx = 0;
for (int b = 0; b < data.Length && idx < expectedCount; b++)
{
byte val = data[b];
for (int bit = 0; bit < 8 && idx < expectedCount; bit++, idx++)
res[idx] = ((val >> bit) & 0x01) != 0;
}
return res;
}
}
private ReadOnlyMemory<byte> BuildBatchRead(DeviceCode device, int start, int count, bool isBit, bool isDWord = false)
{
// Request data:
// [0-2] Address (3 bytes, little-endian 24-bit)
// [3] Device Code (1 byte)
// [4-5] Points/Count (LE, word/bit/dword tuỳ subcmd)
// [6-7] Reserved? (tuỳ lệnh) - KHÔNG cần cho 0x0401
// Ở đây dùng định dạng tối giản: addr(3) + dev(1) + count(2) + subcmd đã set ở header
if (isDWord)
CurrentSubcommandHint = 0x0002; // dword units
else
CurrentSubcommandHint = isBit ? (ushort)0x0001 : (ushort)0x0000;
var buf = new byte[3 + 1 + 2];
// 24-bit little-endian
buf[0] = (byte)(start & 0xFF);
buf[1] = (byte)((start >> 8) & 0xFF);
buf[2] = (byte)((start >> 16) & 0xFF);
buf[3] = DevCodeBinary(device);
BinaryPrimitives.WriteUInt16LittleEndian(buf.AsSpan(4, 2), (ushort)count);
return buf;
}
// 5. Hàm kiểm tra device có hỗ trợ đọc dword trực tiếp không (tuỳ CPU, ví dụ D, W, R, ZR thường hỗ trợ)
private static bool SupportsDirectDWordRead(DeviceCode device)
{
return device == DeviceCode.D || device == DeviceCode.W || device == DeviceCode.R || device == DeviceCode.ZR;
}
}

488
RobotNet.ScriptManager/Connections/ModbusTcpClient.cs Normal file
View File

@@ -0,0 +1,488 @@
using RobotNet.Script;
using System.Buffers.Binary;
using System.Net.Sockets;
namespace RobotNet.ScriptManager.Connections;
public sealed class ModbusTcpClient(string host, int port = 502, byte unitId = 1) : IModbusTcpClient
{
public string Host { get; } = host;
public int Port { get; } = port;
public byte UnitId { get; } = unitId;
public TimeSpan RequestTimeout { get; set; } = TimeSpan.FromSeconds(2);
public TimeSpan ConnectTimeout { get; set; } = TimeSpan.FromSeconds(3);
public int MaxRetries { get; set; } = 2; // per request
public bool AutoReconnect { get; set; } = true;
public bool KeepAlive { get; set; } = true; // enable TCP keepalive
public TimeSpan HeartbeatInterval { get; set; } = TimeSpan.FromSeconds(10);
public ushort HeartbeatRegisterAddress { get; set; } = 0; // dummy read address for heartbeat
private TcpClient? _client;
private NetworkStream? _stream;
private readonly SemaphoreSlim _sendLock = new(1, 1);
private int _transactionId = 0;
private CancellationTokenSource? _cts;
private Task? _heartbeatTask;
#region Connect/Dispose
public async Task ConnectAsync(CancellationToken ct = default)
{
await CloseAsync().ConfigureAwait(false);
_cts = CancellationTokenSource.CreateLinkedTokenSource(ct);
var cts = _cts;
_client = new TcpClient
{
NoDelay = true,
LingerState = new LingerOption(true, 0)
};
using var connectCts = CancellationTokenSource.CreateLinkedTokenSource(ct);
connectCts.CancelAfter(ConnectTimeout);
var connectTask = _client.ConnectAsync(Host, Port);
using (connectCts.Token.Register(() => SafeCancel(connectTask)))
{
await connectTask.ConfigureAwait(false);
}
if (KeepAlive)
{
try
{
SetKeepAlive(_client.Client, true, keepAliveTimeMs: 15_000, keepAliveIntervalMs: 5_000);
}
catch { /* best-effort */ }
}
_stream = _client.GetStream();
_stream.ReadTimeout = (int)RequestTimeout.TotalMilliseconds;
_stream.WriteTimeout = (int)RequestTimeout.TotalMilliseconds;
if (HeartbeatInterval > TimeSpan.Zero)
{
_heartbeatTask = Task.Run(() => HeartbeatLoopAsync(cts!.Token), CancellationToken.None);
}
}
public bool IsConnected => _client?.Connected == true && _stream != null;
public async ValueTask DisposeAsync()
{
await CloseAsync().ConfigureAwait(false);
_sendLock.Dispose();
GC.SuppressFinalize(this);
}
public void Dispose() => DisposeAsync().AsTask().GetAwaiter().GetResult();
private async Task CloseAsync()
{
try { _cts?.Cancel(); } catch { }
try { if (_heartbeatTask != null) await Task.WhenAny(_heartbeatTask, Task.Delay(50)); } catch { }
try { _stream?.Dispose(); } catch { }
try { _client?.Close(); } catch { }
_stream = null;
_client = null;
_cts?.Dispose();
_cts = null;
_heartbeatTask = null;
}
#endregion
#region Public Modbus API
// Coils & Discretes
public Task<bool[]> ReadCoilsAsync(ushort startAddress, ushort count, CancellationToken ct = default)
=> ReadBitsAsync(0x01, startAddress, count, ct);
public Task<bool[]> ReadDiscreteInputsAsync(ushort startAddress, ushort count, CancellationToken ct = default)
=> ReadBitsAsync(0x02, startAddress, count, ct);
// Registers
public Task<ushort[]> ReadHoldingRegistersAsync(ushort startAddress, ushort count, CancellationToken ct = default)
=> ReadRegistersAsync(0x03, startAddress, count, ct);
public Task<ushort[]> ReadInputRegistersAsync(ushort startAddress, ushort count, CancellationToken ct = default)
=> ReadRegistersAsync(0x04, startAddress, count, ct);
public Task WriteSingleCoilAsync(ushort address, bool value, CancellationToken ct = default)
=> WriteSingleAsync(0x05, address, value ? (ushort)0xFF00 : (ushort)0x0000, ct);
public Task WriteSingleRegisterAsync(ushort address, ushort value, CancellationToken ct = default)
=> WriteSingleAsync(0x06, address, value, ct);
public Task WriteMultipleCoilsAsync(ushort startAddress, bool[] values, CancellationToken ct = default)
=> WriteMultipleCoilsCoreAsync(startAddress, values, ct);
public Task WriteMultipleRegistersAsync(ushort startAddress, ushort[] values, CancellationToken ct = default)
=> WriteMultipleRegistersCoreAsync(startAddress, values, ct);
// FC 22: Mask Write Register
public Task MaskWriteRegisterAsync(ushort address, ushort andMask, ushort orMask, CancellationToken ct = default)
=> SendExpectEchoAsync(0x16, writer: span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], address);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), andMask);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(4, 2), orMask);
return 6;
}, expectedEchoLength: 6, ct);
// FC 23: Read/Write Multiple Registers
public async Task<ushort[]> ReadWriteMultipleRegistersAsync(
ushort readStart, ushort readCount,
ushort writeStart, IReadOnlyList<ushort> writeValues,
CancellationToken ct = default)
{
return await SendAsync(0x17, readCount * 2, span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], readStart);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), readCount);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(4, 2), writeStart);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(6, 2), (ushort)writeValues.Count);
span[8] = (byte)(writeValues.Count * 2);
int pos = 9;
for (int i = 0; i < writeValues.Count; i++)
{
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(pos, 2), writeValues[i]);
pos += 2;
}
return pos; // payload length
}, parse: resp =>
{
int byteCount = resp[0];
if (resp.Length != byteCount + 1) throw new ModbusException("Invalid byte count in response");
var result = new ushort[byteCount / 2];
for (int i = 0; i < result.Length; i++)
result[i] = BinaryPrimitives.ReadUInt16BigEndian(resp.Slice(1 + i * 2, 2));
return result;
}, ct).ConfigureAwait(false);
}
// FC 43/14: Read Device Identification (basic)
public async Task<Dictionary<byte, string>> ReadDeviceIdentificationAsync(byte category = 0x01 /* Basic */ , CancellationToken ct = default)
{
return await SendAsync(0x2B, 0, span =>
{
span[0] = 0x0E; // MEI type
span[1] = 0x01; // Read Device ID
span[2] = category; // category
span[3] = 0x00; // object id
return 4;
}, parse: resp =>
{
// resp: [MEI, ReadDevId, conformity, moreFollows, nextObjectId, numObjects, objects...]
if (resp.Length < 6) throw new ModbusException("Invalid Device ID response");
int pos = 0;
byte mei = resp[pos++];
if (mei != 0x0E) throw new ModbusException("Invalid MEI type");
pos++; // ReadDevId
pos++; // conformity
pos++; // moreFollows
pos++; // nextObjectId
byte num = resp[pos++];
var dict = new Dictionary<byte, string>();
for (int i = 0; i < num; i++)
{
byte id = resp[pos++];
byte len = resp[pos++];
if (pos + len > resp.Length) throw new ModbusException("Invalid object length");
string val = System.Text.Encoding.ASCII.GetString(resp.ToArray(), pos, len);
pos += len;
dict[id] = val;
}
return dict;
}, ct).ConfigureAwait(false);
}
#endregion
#region Core Send Helpers
private async Task<bool[]> ReadBitsAsync(byte function, ushort startAddress, ushort count, CancellationToken ct)
{
if (count is 0 or > 2000) throw new ArgumentOutOfRangeException(nameof(count));
var data = await SendAsync(function, expectedLength: (count + 7) / 8 + 1, writer: span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], startAddress);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), count);
return 4;
}, parse: resp => resp.ToArray(), ct).ConfigureAwait(false);
int byteCount = data[0];
if (byteCount != data.Length - 1) throw new ModbusException("Unexpected byte count");
var result = new bool[count];
for (int i = 0; i < count; i++)
{
int b = data[1 + (i / 8)];
result[i] = ((b >> (i % 8)) & 0x01) == 1;
}
return result;
}
private async Task<ushort[]> ReadRegistersAsync(byte function, ushort startAddress, ushort count, CancellationToken ct)
{
if (count is 0 or > 125) throw new ArgumentOutOfRangeException(nameof(count));
var data = await SendAsync(function, expectedLength: count * 2 + 1, writer: span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], startAddress);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), count);
return 4;
}, parse: resp => resp.ToArray(), ct).ConfigureAwait(false);
int byteCount = data[0];
if (byteCount != count * 2 || data.Length != byteCount + 1)
throw new ModbusException("Unexpected byte count");
var result = new ushort[count];
for (int i = 0; i < count; i++)
result[i] = BinaryPrimitives.ReadUInt16BigEndian(data.AsSpan(1 + i * 2, 2));
return result;
}
private Task WriteSingleAsync(byte function, ushort address, ushort value, CancellationToken ct)
=> SendExpectEchoAsync(function, span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], address);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), value);
return 4;
}, expectedEchoLength: 4, ct);
private Task WriteMultipleCoilsCoreAsync(ushort startAddress, bool[] values, CancellationToken ct)
{
if (values == null || values.Length == 0 || values.Length > 1968)
throw new ArgumentOutOfRangeException(nameof(values));
int byteCount = (values.Length + 7) / 8;
return SendExpectEchoAsync(0x0F, span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], startAddress);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), (ushort)values.Length);
span[4] = (byte)byteCount;
int pos = 5;
int bit = 0;
for (int i = 0; i < byteCount; i++)
{
byte b = 0;
for (int j = 0; j < 8 && bit < values.Length; j++, bit++)
if (values[bit]) b |= (byte)(1 << j);
span[pos++] = b;
}
return pos;
}, expectedEchoLength: 4, ct);
}
private Task WriteMultipleRegistersCoreAsync(ushort startAddress, ushort[] values, CancellationToken ct)
{
if (values == null || values.Length == 0 || values.Length > 123)
throw new ArgumentOutOfRangeException(nameof(values));
return SendExpectEchoAsync(0x10, span =>
{
BinaryPrimitives.WriteUInt16BigEndian(span[..2], startAddress);
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(2, 2), (ushort)values.Length);
span[4] = (byte)(values.Length * 2);
int pos = 5;
for (int i = 0; i < values.Length; i++)
{
BinaryPrimitives.WriteUInt16BigEndian(span.Slice(pos, 2), values[i]);
pos += 2;
}
return pos;
}, expectedEchoLength: 4, ct);
}
private async Task SendExpectEchoAsync(byte function, Func<Span<byte>, int> writer, int expectedEchoLength, CancellationToken ct)
{
_ = await SendAsync(function, expectedEchoLength + 0, writer: writer, parse: resp =>
{
if (resp.Length != expectedEchoLength) throw new ModbusException("Unexpected echo length");
return 0;
}, ct).ConfigureAwait(false);
}
private async Task<T> SendAsync<T>(byte function, int expectedLength, Func<Span<byte>, int> writer, Func<ReadOnlySpan<byte>, T> parse, CancellationToken ct)
{
int attempts = 0;
while (true)
{
attempts++;
try
{
await EnsureConnectedAsync(ct).ConfigureAwait(false);
await _sendLock.WaitAsync(ct).ConfigureAwait(false);
try
{
var reqPayload = new byte[260]; // generous for payload
int payloadLen = writer(reqPayload);
var pdu = new byte[payloadLen + 1];
pdu[0] = function;
Buffer.BlockCopy(reqPayload, 0, pdu, 1, payloadLen);
var adu = BuildMbap(UnitId, pdu);
await _stream!.WriteAsync(adu, ct).ConfigureAwait(false);
// Read MBAP (7 bytes), then body
byte[] mbap = await ReadExactAsync(7, ct).ConfigureAwait(false);
ushort transId = BinaryPrimitives.ReadUInt16BigEndian(mbap.AsSpan(0, 2));
ushort proto = BinaryPrimitives.ReadUInt16BigEndian(mbap.AsSpan(2, 2));
ushort len = BinaryPrimitives.ReadUInt16BigEndian(mbap.AsSpan(4, 2));
byte unit = mbap[6];
if (proto != 0 || unit != UnitId) throw new ModbusException("Invalid MBAP header");
if (len < 2) throw new ModbusException("Invalid length");
byte[] body = await ReadExactAsync(len - 1, ct).ConfigureAwait(false); // len includes unitId
byte fc = body[0];
if ((fc & 0x80) != 0)
{
byte ex = body[1];
throw new ModbusException($"Exception (FC={(function):X2}): {ex}", (ModbusExceptionCode)ex);
}
if (fc != function) throw new ModbusException("Mismatched function in response");
var pduData = body.AsSpan(1);
// If caller supplied an expectedLength, do a soft sanity check when applicable
if (expectedLength > 0 && pduData.Length < expectedLength)
{
// some functions have variable lengths; we avoid hard-failing here
}
return parse(pduData);
}
finally
{
_sendLock.Release();
}
}
catch (Exception ex) when (attempts <= MaxRetries && IsTransient(ex))
{
if (AutoReconnect)
{
await ReconnectAsync(ct).ConfigureAwait(false);
continue; // retry
}
throw;
}
}
}
private async Task EnsureConnectedAsync(CancellationToken ct)
{
if (IsConnected) return;
await ConnectAsync(ct).ConfigureAwait(false);
}
private async Task ReconnectAsync(CancellationToken ct)
{
try { await CloseAsync(); } catch { }
await Task.Delay(100, ct).ConfigureAwait(false);
await ConnectAsync(ct).ConfigureAwait(false);
}
private byte[] BuildMbap(byte unitId, byte[] pdu)
{
// MBAP: Transaction(2) Protocol(2=0) Length(2) UnitId(1)
// Length = PDU length + 1 (UnitId)
ushort trans = unchecked((ushort)Interlocked.Increment(ref _transactionId));
var adu = new byte[7 + pdu.Length];
BinaryPrimitives.WriteUInt16BigEndian(adu.AsSpan(0, 2), trans);
BinaryPrimitives.WriteUInt16BigEndian(adu.AsSpan(2, 2), 0);
BinaryPrimitives.WriteUInt16BigEndian(adu.AsSpan(4, 2), (ushort)(pdu.Length + 1));
adu[6] = unitId;
Buffer.BlockCopy(pdu, 0, adu, 7, pdu.Length);
return adu;
}
private async Task<byte[]> ReadExactAsync(int length, CancellationToken ct)
{
byte[] buf = new byte[length];
int read = 0;
while (read < length)
{
int n = await _stream!.ReadAsync(buf.AsMemory(read, length - read), ct).ConfigureAwait(false);
if (n <= 0) throw new IOException("Remote closed the connection");
read += n;
}
return buf;
}
private static bool IsTransient(Exception ex)
=> ex is SocketException or IOException or TimeoutException;
private async Task HeartbeatLoopAsync(CancellationToken ct)
{
while (!ct.IsCancellationRequested)
{
try
{
await Task.Delay(HeartbeatInterval, ct).ConfigureAwait(false);
if (ct.IsCancellationRequested) break;
if (IsConnected)
{
// best-effort heartbeat: read one register
using var timeoutCts = new CancellationTokenSource(RequestTimeout);
using var linked = CancellationTokenSource.CreateLinkedTokenSource(ct, timeoutCts.Token);
await ReadHoldingRegistersAsync(HeartbeatRegisterAddress, 1, linked.Token).ConfigureAwait(false);
}
}
catch (OperationCanceledException) when (ct.IsCancellationRequested)
{
break;
}
catch
{
if (AutoReconnect)
{
try { await ReconnectAsync(ct).ConfigureAwait(false); } catch { }
}
}
}
}
private static void SafeCancel(Task _)
{
try { } catch { }
}
private static void SetKeepAlive(Socket socket, bool on, int keepAliveTimeMs, int keepAliveIntervalMs)
{
// Windows & Linux support via IOControl (best-effort)
// On Linux, consider sysctl or TCP_KEEP* sockopts when available.
if (!on) return;
byte[] inOptionValues = new byte[12];
BinaryPrimitives.WriteUInt32LittleEndian(inOptionValues.AsSpan(0), 1);
BinaryPrimitives.WriteUInt32LittleEndian(inOptionValues.AsSpan(4), (uint)keepAliveTimeMs);
BinaryPrimitives.WriteUInt32LittleEndian(inOptionValues.AsSpan(8), (uint)keepAliveIntervalMs);
const int SIO_KEEPALIVE_VALS = -1744830460;
try { socket.IOControl(SIO_KEEPALIVE_VALS, inOptionValues, null); } catch { }
}
#endregion
}
public enum ModbusExceptionCode : byte
{
IllegalFunction = 0x01,
IllegalDataAddress = 0x02,
IllegalDataValue = 0x03,
SlaveDeviceFailure = 0x04,
Acknowledge = 0x05,
SlaveDeviceBusy = 0x06,
MemoryParityError = 0x08,
GatewayPathUnavailable = 0x0A,
GatewayTargetFailedToRespond = 0x0B
}
public sealed class ModbusException(string message, ModbusExceptionCode? code = null) : Exception(message)
{
public ModbusExceptionCode? Code { get; } = code;
public static string DescribeException(byte code) => code switch
{
0x01 => "Illegal Function",
0x02 => "Illegal Data Address",
0x03 => "Illegal Data Value",
0x04 => "Slave Device Failure",
0x05 => "Acknowledge",
0x06 => "Slave Device Busy",
0x08 => "Memory Parity Error",
0x0A => "Gateway Path Unavailable",
0x0B => "Gateway Target Failed To Respond",
_ => $"Unknown Exception 0x{code:X2}"
};
}

64
RobotNet.ScriptManager/Connections/UnixDevice.cs Normal file
View File

@@ -0,0 +1,64 @@
using RobotNet.Script;
namespace RobotNet.ScriptManager.Connections;
public class UnixDevice : IUnixDevice
{
public static readonly UnixDevice Instance = new();
private UnixDevice() { }
public byte[] ReadDev(string name, int length)
{
if (Environment.OSVersion.Platform != PlatformID.Unix)
{
throw new PlatformNotSupportedException("This method is only supported on linux.");
}
if (length <= 0)
{
throw new ArgumentOutOfRangeException(nameof(length), "Length must be greater than zero.");
}
if (string.IsNullOrWhiteSpace(name) || name.Any(c => Path.GetInvalidFileNameChars().Contains(c)))
{
throw new ArgumentException("Invalid device name.", nameof(name));
}
var path = $"/dev/robotnet/{name}";
if (!File.Exists(path))
{
throw new FileNotFoundException($"Device '{name}' not found.", path);
}
using var fs = new FileStream(path, FileMode.Open, FileAccess.Read, FileShare.Read);
var buffer = new byte[length];
var bytesRead = fs.Read(buffer, 0, length);
fs.Close();
return buffer;
}
public void WriteDev(string name, byte[] data)
{
if (Environment.OSVersion.Platform != PlatformID.Unix)
{
throw new PlatformNotSupportedException("This method is only supported on linux.");
}
if (data == null || data.Length == 0)
{
throw new ArgumentNullException(nameof(data), "Data cannot be null or empty.");
}
if (string.IsNullOrWhiteSpace(name) || name.Any(c => Path.GetInvalidFileNameChars().Contains(c)))
{
throw new ArgumentException("Invalid device name.", nameof(name));
}
var path = $"/dev/robotnet/{name}";
if (!File.Exists(path))
{
throw new FileNotFoundException($"Device '{name}' not found.", path);
}
using var fs = new FileStream(path, FileMode.Open, FileAccess.Write, FileShare.Read);
fs.Write(data, 0, data.Length);
fs.Close();
}
}