lmxopcua/src/Drivers/ZB.MOM.WW.OtOpcUa.Driver.FOCAS/Wire/FocasWireProtocol.cs

using System.Buffers.Binary;
using System.Net.Sockets;
using System.Text;

namespace ZB.MOM.WW.OtOpcUa.Driver.FOCAS.Wire;

/// <summary>
///     Framing primitives for the FOCAS/2 Ethernet wire protocol — magic-prefixed PDU
///     header + request/response block envelopes. Read-only subset: every call OtOpcUa
///     issues maps to one of the command IDs documented in
///     <c>docs/v2/implementation/focas-wire-protocol.md</c>.
/// </summary>
/// <remarks>
///     <para>All multi-byte integer fields are big-endian on the wire. The 10-byte header is
///     <c>a0 a0 a0 a0</c> magic + 2-byte version + type byte + direction byte + 2-byte body
///     length. Version 1 is the only version this implementation supports.</para>
///     <para>Type <c>0x01</c> is the initiate handshake, <c>0x02</c> is the session close,
///     <c>0x21</c> is a request/response data PDU carrying one or more request blocks.</para>
/// </remarks>
internal static class FocasWireProtocol
{
    /// <summary>The PDU version this client emits in every outgoing request header.</summary>
    public const ushort Version = 1;

    /// <summary>
    ///     PDU versions accepted on inbound PDUs. The 10-byte header framing is identical across
    ///     these (only the version field differs), so the framing layer accepts both while we keep
    ///     emitting <see cref="Version"/> (v1) on requests. The docker mock + older controls answer
    ///     v1; modern controls answer v3 — FANUC 30i-B validated live 2026-06-25 (macro reads OK).
    ///     See <c>docs/plans/2026-06-25-focas-pdu-v3-30i-b-support.md</c>.
    /// </summary>
    private static readonly ushort[] SupportedReadVersions = [1, 3];

    /// <summary>True when <paramref name="version"/> is a PDU version this client can frame-parse.</summary>
    internal static bool IsSupportedReadVersion(ushort version) =>
        Array.IndexOf(SupportedReadVersions, version) >= 0;

    public const byte DirectionRequest = 0x01;
    public const byte DirectionResponse = 0x02;
    public const byte TypeInitiate = 0x01;
    public const byte TypeClose = 0x02;
    public const byte TypeData = 0x21;

    private static readonly byte[] Magic = [0xa0, 0xa0, 0xa0, 0xa0];

    /// <summary>Assemble a full PDU (10-byte header + body) for transmission.</summary>
    /// <param name="type">The PDU type byte.</param>
    /// <param name="direction">The direction byte (request or response).</param>
    /// <param name="body">The PDU body bytes.</param>
    /// <returns>The complete PDU bytes including header and body.</returns>
    public static byte[] BuildPdu(byte type, byte direction, ReadOnlySpan<byte> body)
    {
        if (body.Length > ushort.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(body), "FOCAS PDU body is limited to 65535 bytes.");

        var bytes = new byte[10 + body.Length];
        Magic.CopyTo(bytes, 0);
        BinaryPrimitives.WriteUInt16BigEndian(bytes.AsSpan(4, 2), Version);
        bytes[6] = type;
        bytes[7] = direction;
        BinaryPrimitives.WriteUInt16BigEndian(bytes.AsSpan(8, 2), (ushort)body.Length);
        body.CopyTo(bytes.AsSpan(10));
        return bytes;
    }

    /// <summary>
    ///     Initiate-body shape — just the 2-byte socket index (1 or 2). <c>cnc_allclibhndl3</c>
    ///     opens two TCP sockets in sequence and each sends its own initiate PDU carrying its
    ///     index.
    /// </summary>
    /// <param name="socketIndex">The socket index (1 or 2).</param>
    /// <returns>The initiate body bytes.</returns>
    public static byte[] BuildInitiateBody(ushort socketIndex)
    {
        var body = new byte[2];
        BinaryPrimitives.WriteUInt16BigEndian(body, socketIndex);
        return body;
    }

    /// <summary>Assemble a type-<c>0x21</c> body carrying one or more request blocks.</summary>
    /// <param name="blocks">The request blocks to assemble.</param>
    /// <returns>The assembled body bytes.</returns>
    public static byte[] BuildRequestBody(IReadOnlyList<RequestBlock> blocks)
    {
        if (blocks.Count > ushort.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(blocks), "Too many request blocks.");

        var blockBytes = blocks.Select(BuildRequestBlock).ToArray();
        var bodyLength = 2 + blockBytes.Sum(block => block.Length);
        if (bodyLength > ushort.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(blocks), "FOCAS request body is too large.");

        var body = new byte[bodyLength];
        BinaryPrimitives.WriteUInt16BigEndian(body.AsSpan(0, 2), (ushort)blocks.Count);
        var offset = 2;
        foreach (var block in blockBytes)
        {
            block.CopyTo(body.AsSpan(offset));
            offset += block.Length;
        }
        return body;
    }

    /// <summary>Async read of one full PDU off a stream. Throws <see cref="FocasWireException"/> on invalid magic / version / truncation.</summary>
    /// <param name="stream">The network stream to read from.</param>
    /// <param name="cancellationToken">Cancellation token.</param>
    /// <returns>The read PDU.</returns>
    public static async Task<Pdu> ReadPduAsync(NetworkStream stream, CancellationToken cancellationToken)
    {
        var header = new byte[10];
        await ReadExactlyAsync(stream, header, cancellationToken).ConfigureAwait(false);

        if (!header.AsSpan(0, 4).SequenceEqual(Magic))
            throw new FocasWireException("Invalid FOCAS PDU magic.");

        var version = BinaryPrimitives.ReadUInt16BigEndian(header.AsSpan(4, 2));
        if (!IsSupportedReadVersion(version))
            throw new FocasWireException($"Unsupported FOCAS PDU version {version}.");

        var bodyLength = BinaryPrimitives.ReadUInt16BigEndian(header.AsSpan(8, 2));
        var body = new byte[bodyLength];
        if (bodyLength > 0)
            await ReadExactlyAsync(stream, body, cancellationToken).ConfigureAwait(false);

        return new Pdu(header[6], header[7], body);
    }

    /// <summary>Synchronous counterpart to <see cref="ReadPduAsync"/> — used by <see cref="FocasWireClient"/>'s sync dispose.</summary>
    /// <param name="stream">The network stream to read from.</param>
    /// <returns>The read PDU.</returns>
    public static Pdu ReadPdu(NetworkStream stream)
    {
        var header = new byte[10];
        ReadExactly(stream, header);

        if (!header.AsSpan(0, 4).SequenceEqual(Magic))
            throw new FocasWireException("Invalid FOCAS PDU magic.");

        var version = BinaryPrimitives.ReadUInt16BigEndian(header.AsSpan(4, 2));
        if (!IsSupportedReadVersion(version))
            throw new FocasWireException($"Unsupported FOCAS PDU version {version}.");

        var bodyLength = BinaryPrimitives.ReadUInt16BigEndian(header.AsSpan(8, 2));
        var body = new byte[bodyLength];
        if (bodyLength > 0)
            ReadExactly(stream, body);

        return new Pdu(header[6], header[7], body);
    }

    private static async Task ReadExactlyAsync(NetworkStream stream, byte[] buffer, CancellationToken cancellationToken)
    {
        // NetworkStream.ReadAsync's CancellationToken does not reliably abort a socket read that is
        // blocked waiting for bytes the peer never sends — a CNC that TCP-accepts then stalls
        // mid-PDU (the cnc_rdsvmeter "hang" the 31i-B work chased). Register a hard abort that
        // disposes the stream on cancellation so a stalled read throws instead of wedging the
        // caller's poll loop, and normalize the resulting failure to OperationCanceledException so
        // the request path tears the transport down as a transient. See
        // docs/plans/2026-06-25-focas-pdu-v3-30i-b-support.md (Phase 2).
        await using var abort = cancellationToken.Register(static s => ((IDisposable)s!).Dispose(), stream);
        var offset = 0;
        try
        {
            while (offset < buffer.Length)
            {
                var read = await stream.ReadAsync(buffer.AsMemory(offset, buffer.Length - offset), cancellationToken).ConfigureAwait(false);
                if (read == 0)
                    throw new EndOfStreamException("FOCAS socket closed before the expected number of bytes were read.");
                offset += read;
            }
        }
        catch (Exception ex) when (cancellationToken.IsCancellationRequested && ex is not OperationCanceledException)
        {
            // The stalled read was aborted by the dispose-on-cancel registration above.
            throw new OperationCanceledException(cancellationToken);
        }
    }

    private static void ReadExactly(NetworkStream stream, byte[] buffer)
    {
        var offset = 0;
        while (offset < buffer.Length)
        {
            var read = stream.Read(buffer, offset, buffer.Length - offset);
            if (read == 0)
                throw new EndOfStreamException("FOCAS socket closed before the expected number of bytes were read.");
            offset += read;
        }
    }

    /// <summary>
    ///     Unpack a type-<c>0x21</c> response body into its constituent response blocks. Each
    ///     block carries the command ID, the FOCAS <c>EW_*</c> return code, and the payload
    ///     bytes.
    /// </summary>
    /// <param name="body">The response body bytes.</param>
    /// <returns>The parsed response blocks.</returns>
    public static IReadOnlyList<ResponseBlock> ParseResponseBlocks(ReadOnlySpan<byte> body)
    {
        if (body.Length < 2)
            return Array.Empty<ResponseBlock>();

        var count = BinaryPrimitives.ReadUInt16BigEndian(body.Slice(0, 2));
        var blocks = new List<ResponseBlock>(count);
        var offset = 2;
        for (var index = 0; index < count; index++)
        {
            if (offset + 2 > body.Length)
                throw new FocasWireException("Truncated FOCAS response block length.");

            var blockLength = BinaryPrimitives.ReadUInt16BigEndian(body.Slice(offset, 2));
            if (blockLength < 0x10 || offset + blockLength > body.Length)
                throw new FocasWireException($"Invalid FOCAS response block length {blockLength}.");

            var block = body.Slice(offset, blockLength);
            var command = BinaryPrimitives.ReadUInt16BigEndian(block.Slice(6, 2));
            var payloadLength = BinaryPrimitives.ReadUInt16BigEndian(block.Slice(14, 2));
            if (0x10 + payloadLength > blockLength)
                throw new FocasWireException("Invalid FOCAS response payload length.");

            var rc = BinaryPrimitives.ReadInt16BigEndian(block.Slice(8, 2));
            blocks.Add(new ResponseBlock(command, rc, block.Slice(16, payloadLength).ToArray()));
            offset += blockLength;
        }

        return blocks;
    }

    /// <summary>Read an ASCII string out of a payload span, stopping at the first NUL and trimming trailing spaces.</summary>
    /// <param name="bytes">The bytes to decode.</param>
    /// <returns>The decoded ASCII string.</returns>
    public static string ReadAscii(ReadOnlySpan<byte> bytes)
    {
        var end = bytes.IndexOf((byte)0);
        if (end >= 0) bytes = bytes.Slice(0, end);
        return Encoding.ASCII.GetString(bytes.ToArray()).TrimEnd(' ', '\0');
    }

    /// <summary>
    ///     Read an axis/spindle name record — the first 2 bytes of a 2-byte (axis) or 4-byte
    ///     (spindle) slot. Trailing spaces and NULs are stripped so <c>"X "</c> becomes
    ///     <c>"X"</c>.
    /// </summary>
    /// <param name="bytes">The bytes to decode.</param>
    /// <returns>The decoded name record.</returns>
    public static string ReadNameRecord(ReadOnlySpan<byte> bytes)
    {
        if (bytes.Length < 2) return string.Empty;
        var buffer = bytes.Slice(0, Math.Min(2, bytes.Length)).ToArray();
        return Encoding.ASCII.GetString(buffer).TrimEnd(' ', '\0');
    }

    private static byte[] BuildRequestBlock(RequestBlock request)
    {
        var extra = request.ExtraPayload ?? Array.Empty<byte>();
        if (extra.Length > ushort.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(request), "FOCAS request extra payload is too large.");

        var blockLength = 0x1c + extra.Length;
        if (blockLength > ushort.MaxValue)
            throw new ArgumentOutOfRangeException(nameof(request), "FOCAS request block is too large.");

        var block = new byte[blockLength];
        BinaryPrimitives.WriteUInt16BigEndian(block.AsSpan(0, 2), (ushort)blockLength);
        BinaryPrimitives.WriteUInt16BigEndian(block.AsSpan(2, 2), request.RequestClass);
        BinaryPrimitives.WriteUInt16BigEndian(block.AsSpan(4, 2), request.PathId);
        BinaryPrimitives.WriteUInt16BigEndian(block.AsSpan(6, 2), request.Command);
        BinaryPrimitives.WriteInt32BigEndian(block.AsSpan(8, 4), request.Arg1);
        BinaryPrimitives.WriteInt32BigEndian(block.AsSpan(12, 4), request.Arg2);
        BinaryPrimitives.WriteInt32BigEndian(block.AsSpan(16, 4), request.Arg3);
        BinaryPrimitives.WriteInt32BigEndian(block.AsSpan(20, 4), request.Arg4);
        BinaryPrimitives.WriteUInt16BigEndian(block.AsSpan(24, 2), request.Arg5);
        BinaryPrimitives.WriteUInt16BigEndian(block.AsSpan(26, 2), (ushort)extra.Length);
        extra.CopyTo(block.AsSpan(28));
        return block;
    }
}

/// <summary>One raw PDU off the wire — header bytes plus the body.</summary>
internal sealed record Pdu(byte Type, byte Direction, byte[] Body);

/// <summary>
///     One request block within a type-<c>0x21</c> PDU body. <see cref="Command"/> is the
///     FOCAS command ID (e.g. <c>0x0018</c> for sysinfo); <see cref="Arg1"/>..<see cref="Arg5"/>
///     are the command-specific scalar arguments; <see cref="ExtraPayload"/> carries the
///     optional extra bytes for writes.
/// </summary>
internal sealed record RequestBlock(
    ushort Command,
    int Arg1 = 0,
    int Arg2 = 0,
    int Arg3 = 0,
    int Arg4 = 0,
    ushort Arg5 = 0,
    ushort RequestClass = 1,
    ushort PathId = 1,
    byte[]? ExtraPayload = null);

/// <summary>One response block — command ID + FOCAS return code + payload bytes.</summary>
internal sealed record ResponseBlock(ushort Command, short Rc, byte[] Payload);