96 lines
3.4 KiB
C#
96 lines
3.4 KiB
C#
using Sick.SafetyScanners.DataStructures;
|
|
using Sick.SafetyScanners.Helpers;
|
|
|
|
namespace Sick.SafetyScanners.DataProcessing;
|
|
|
|
/// <summary>
|
|
/// Parser for incoming TCP packets in COLA2 format
|
|
/// Thread-safe implementation
|
|
/// </summary>
|
|
public sealed class ParseTcpPacket
|
|
{
|
|
private const int HeaderSize = 18; // COLA2 header size
|
|
|
|
/// <summary>
|
|
/// Gets the expected packet length from the header
|
|
/// Matches: ParseTCPPacket::getExpectedPacketLength in C++
|
|
/// </summary>
|
|
public uint GetExpectedPacketLength(PacketBuffer buffer)
|
|
{
|
|
var bufferData = buffer.GetBuffer();
|
|
var length = ReadWriteHelper.ReadUint32BigEndian(bufferData.Span, 4);
|
|
return length + 8; // for STX and Length which is not included in length datafield
|
|
}
|
|
|
|
/// <summary>
|
|
/// Gets the request ID from the packet header
|
|
/// Matches: ParseTCPPacket::getRequestID in C++
|
|
/// </summary>
|
|
public ushort GetRequestId(PacketBuffer buffer)
|
|
{
|
|
var bufferData = buffer.GetBuffer();
|
|
return ReadWriteHelper.ReadUint16BigEndian(bufferData.Span, 14);
|
|
}
|
|
|
|
/// <summary>
|
|
/// Parses the TCP sequence to extract COLA2 header information
|
|
/// Matches: ParseTCPPacket::parseTCPSequence in C++
|
|
/// </summary>
|
|
public ParseResult ParseTcpSequence(PacketBuffer buffer)
|
|
{
|
|
var bufferData = buffer.GetBuffer();
|
|
var span = bufferData.Span;
|
|
|
|
// Read header fields
|
|
var stx = ReadWriteHelper.ReadUint32BigEndian(span, 0);
|
|
var length = ReadWriteHelper.ReadUint32BigEndian(span, 4);
|
|
var hubCntr = ReadWriteHelper.ReadUint8BigEndian(span, 8);
|
|
var noc = ReadWriteHelper.ReadUint8BigEndian(span, 9);
|
|
var sessionId = ReadWriteHelper.ReadUint32BigEndian(span, 10);
|
|
var requestId = ReadWriteHelper.ReadUint16BigEndian(span, 14);
|
|
var commandType = ReadWriteHelper.ReadUint8BigEndian(span, 16);
|
|
var commandMode = ReadWriteHelper.ReadUint8BigEndian(span, 17);
|
|
|
|
// Read data payload (everything after header, starting at offset 20)
|
|
// Matches: ParseTCPPacket::readData in C++ - returns data from offset 20
|
|
ReadOnlyMemory<byte> data = ReadOnlyMemory<byte>.Empty;
|
|
if (bufferData.Length >= 20)
|
|
{
|
|
var dataStart = 20;
|
|
data = bufferData[dataStart..];
|
|
}
|
|
|
|
return new ParseResult
|
|
{
|
|
Stx = stx,
|
|
Length = length,
|
|
HubCntr = hubCntr,
|
|
NoC = noc,
|
|
SessionId = sessionId,
|
|
RequestId = requestId,
|
|
CommandType = commandType,
|
|
CommandMode = commandMode,
|
|
ErrorCode = null,
|
|
Data = data
|
|
};
|
|
}
|
|
|
|
/// <summary>
|
|
/// Result of parsing a TCP packet
|
|
/// </summary>
|
|
public sealed class ParseResult
|
|
{
|
|
public uint Stx { get; init; }
|
|
public uint Length { get; init; }
|
|
public byte HubCntr { get; init; }
|
|
public byte NoC { get; init; }
|
|
public uint SessionId { get; init; }
|
|
public ushort RequestId { get; init; }
|
|
public byte CommandType { get; init; }
|
|
public byte CommandMode { get; init; }
|
|
public ushort? ErrorCode { get; init; }
|
|
public ReadOnlyMemory<byte> Data { get; init; }
|
|
}
|
|
}
|
|
|