wwtools/mbproxy/src/Mbproxy/Proxy/BcdPduPipeline.cs

using Mbproxy.Bcd;

namespace Mbproxy.Proxy;

/// <summary>
/// BCD-rewriting PDU pipeline. Registered as the singleton <see cref="IPduPipeline"/>
/// in production.
///
/// FC scope (per docs/Features/BcdRewriting.md):
///   FC03 / FC04 response  — decode covered BCD slots from raw nibbles → binary integer.
///   FC06 request          — encode binary integer → BCD nibbles.
///   FC16 request          — per-register over the configured slots.
///   All other FCs         — pass through byte-for-byte.
///
/// MBAP transparency contract: the MBAP length field is NEVER modified. Re-encoded slots
/// are the same byte width as the originals (ushort → ushort), so the PDU length is stable.
///
/// <para><b>Request correlation:</b> FC03/FC04 responses do not carry the original
/// start address. The multiplexer builds an <see cref="Multiplexing.InFlightRequest"/>
/// on the request path, stores it in its <see cref="Multiplexing.CorrelationMap"/>, and
/// attaches it to the per-call <see cref="PerPlcContext.CurrentRequest"/> on the
/// response path. The rewriter consumes <c>CurrentRequest</c>, so concurrent responses
/// from different upstream clients each decode against their own request range without
/// cross-talk.</para>
///
/// <para>This class is stateless. All per-call state arrives via <see cref="PduContext"/>
/// (specifically <see cref="PerPlcContext.CurrentRequest"/> on response). It is safe to
/// call concurrently from multiple upstream-read tasks and the single backend reader task.</para>
/// </summary>
internal sealed class BcdPduPipeline : IPduPipeline
{
    // ── IPduPipeline.Process ─────────────────────────────────────────────────

    public void Process(
        MbapDirection direction,
        ReadOnlySpan<byte> mbapHeader,
        Span<byte> pdu,
        PduContext context)
    {
        // PerPlcContext carries the BCD map, counters, and logger.
        // If the caller passes a plain PduContext (e.g. in unit tests using NoopPduPipeline
        // alongside this one), we skip BCD processing gracefully.
        if (context is not PerPlcContext ctx)
            return;

        if (pdu.Length < 1)
            return;

        byte fc = pdu[0];
        ctx.Counters.IncrementPdusForwarded();
        ctx.Counters.IncrementFcCount(fc);

        if (direction == MbapDirection.RequestToBackend)
        {
            ProcessRequest(fc, pdu, ctx);
        }
        else
        {
            ProcessResponse(fc, pdu, ctx);
        }
    }

    // ── Request processing (FC06 / FC16) ────────────────────────────────────

    private static void ProcessRequest(byte fc, Span<byte> pdu, PerPlcContext ctx)
    {
        switch (fc)
        {
            case 0x06:
                ProcessFc06Request(pdu, ctx);
                break;

            case 0x10:
                ProcessFc16Request(pdu, ctx);
                break;

            // All other FCs: transparent pass-through.
        }
    }

    /// <summary>
    /// FC06 Write Single Register request: [fc=06][addrHi][addrLo][valHi][valLo]
    /// If the address is a configured 16-bit BCD tag, encode the client's binary integer
    /// as BCD nibbles before forwarding to the PLC.
    /// Partial-overlap (address is part of a 32-bit pair): warn + pass through raw.
    /// </summary>
    private static void ProcessFc06Request(Span<byte> pdu, PerPlcContext ctx)
    {
        if (pdu.Length < 5)
            return;

        ushort address = (ushort)((pdu[1] << 8) | pdu[2]);
        ushort value   = (ushort)((pdu[3] << 8) | pdu[4]);

        // Direct point lookup at the exact address.
        if (!ctx.TagMap.TryGet(address, out var tag))
        {
            // Not a BCD address — but check whether this address is the HIGH register
            // of a 32-bit pair (Address+1 where Address is configured as 32-bit).
            // TryGetForRange with qty=1 will catch that partial-overlap case.
            if (ctx.TagMap.TryGetForRange(address, 1, out var hits) && hits.Count > 0)
            {
                // The only hit should be a 32-bit tag whose high register is at `address`.
                foreach (var hit in hits)
                {
                    if (hit.Tag.IsThirtyTwoBit && hit.OffsetWords < 0)
                    {
                        // This address is the high register of the 32-bit pair.
                        RewriterLogEvents.PartialBcd(ctx.Logger, ctx.PlcName, address, address, 1);
                        ctx.Counters.IncrementPartialBcd();
                        return;
                    }
                }
            }
            return;
        }

        if (tag.IsThirtyTwoBit)
        {
            // FC06 writes exactly one register. If this is the LOW address of a 32-bit tag,
            // that's a partial write. Per design partial-overlap policy: warn + pass through.
            RewriterLogEvents.PartialBcd(ctx.Logger, ctx.PlcName, address, address, 1);
            ctx.Counters.IncrementPartialBcd();
            return;
        }

        // 16-bit tag: encode client's binary integer as BCD nibbles.
        ushort encoded;
        try
        {
            encoded = BcdCodec.Encode16(value);
        }
        catch (ArgumentOutOfRangeException)
        {
            // Value is outside [0, 9999] — cannot represent as 4-digit BCD.
            RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, address, value, "Write");
            ctx.Counters.IncrementInvalidBcd();
            return; // pass through raw
        }

        pdu[3] = (byte)(encoded >> 8);
        pdu[4] = (byte)(encoded & 0xFF);
        ctx.Counters.AddRewrittenSlots(1);
    }

    /// <summary>
    /// FC16 Write Multiple Registers request:
    /// [fc=10][startHi][startLo][qtyHi][qtyLo][byteCount][reg0Hi][reg0Lo]...
    /// Re-encodes binary integers at configured BCD addresses to BCD nibbles.
    /// </summary>
    private static void ProcessFc16Request(Span<byte> pdu, PerPlcContext ctx)
    {
        // Minimum FC16 request PDU: fc(1) + start(2) + qty(2) + byteCount(1) = 6 bytes.
        if (pdu.Length < 6)
            return;

        ushort startAddress = (ushort)((pdu[1] << 8) | pdu[2]);
        ushort qty          = (ushort)((pdu[3] << 8) | pdu[4]);

        // Validate the request is fully sized for `qty` registers (each 2 bytes after
        // the byteCount byte). A client claiming qty=10 with only 4 bytes of register
        // data would otherwise have its BCD slots silently skipped by the per-slot
        // bounds check below — half the request rewritten, half not. Returning here
        // passes the malformed PDU through unchanged so the PLC's own validator
        // surfaces the protocol error.
        if (pdu.Length < 6 + qty * 2)
            return;

        if (!ctx.TagMap.TryGetForRange(startAddress, qty, out var hits))
            return; // no BCD tags in this range

        int dataOffset = 6; // pdu[6..] = register data, 2 bytes per register

        foreach (var hit in hits)
        {
            int offsetWords = hit.OffsetWords;
            var tag = hit.Tag;

            if (tag.IsThirtyTwoBit)
            {
                // Full 32-bit pair fits if both low (offsetWords) and high (offsetWords+1)
                // are within the [0, qty) range.
                bool lowInRange  = offsetWords >= 0 && offsetWords < qty;
                bool highInRange = (offsetWords + 1) >= 0 && (offsetWords + 1) < qty;

                if (!lowInRange || !highInRange)
                {
                    // Partial overlap — one of the two registers is outside the write range.
                    RewriterLogEvents.PartialBcd(ctx.Logger, ctx.PlcName,
                        tag.Address, startAddress, qty);
                    ctx.Counters.IncrementPartialBcd();
                    continue;
                }

                // Both registers are in range. Read the low/high words from the PDU.
                int lowByteOff  = dataOffset + offsetWords * 2;
                int highByteOff = dataOffset + (offsetWords + 1) * 2;

                if (lowByteOff + 2 > pdu.Length || highByteOff + 2 > pdu.Length)
                    continue; // malformed PDU — skip safely

                // Per CDAB layout:
                //   pdu[lowByteOff..+2]  = low register  (low 4 BCD digits of value)
                //   pdu[highByteOff..+2] = high register (high 4 BCD digits of value)
                // The client sends binary integers; encode to BCD nibbles.
                //
                // Design note: for a 32-bit write the client sends a 32-bit binary value
                // split across two registers in CDAB order (low word at Address,
                // high word at Address+1). We reconstruct the int and encode it.
                ushort clientLow  = (ushort)((pdu[lowByteOff]  << 8) | pdu[lowByteOff  + 1]);
                ushort clientHigh = (ushort)((pdu[highByteOff] << 8) | pdu[highByteOff + 1]);

                // Validate that BOTH input words are within the base-10000-digit range
                // BEFORE reconstructing. Without this guard, a client writing
                // (high=9999, low=9999) silently mutates to (high=9998, low=9999)
                // because `9999 * 10_000 + 9999 = 99_989_999` is still <= the 32-bit
                // BCD ceiling, so Encode32 accepts it and rewrites — losing 1 from the
                // high word. The unconventional wire format ("two base-10000 CDAB
                // digits", per docs/Features/BcdRewriting.md) means each word independently must be 0..9999
                // to round-trip cleanly.
                if (clientLow > 9999 || clientHigh > 9999)
                {
                    RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, tag.Address,
                        clientLow, "Write");
                    ctx.Counters.IncrementInvalidBcd();
                    continue;
                }

                // Reconstruct the 32-bit binary value (CDAB: low-word = low digits).
                int binaryValue = clientHigh * 10_000 + clientLow;

                ushort bcdLow, bcdHigh;
                try
                {
                    (bcdLow, bcdHigh) = BcdCodec.Encode32(binaryValue);
                }
                catch (ArgumentOutOfRangeException)
                {
                    RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, tag.Address,
                        clientLow, "Write");
                    ctx.Counters.IncrementInvalidBcd();
                    continue;
                }

                pdu[lowByteOff]      = (byte)(bcdLow  >> 8);
                pdu[lowByteOff  + 1] = (byte)(bcdLow  & 0xFF);
                pdu[highByteOff]     = (byte)(bcdHigh >> 8);
                pdu[highByteOff + 1] = (byte)(bcdHigh & 0xFF);
                ctx.Counters.AddRewrittenSlots(2);
            }
            else
            {
                // 16-bit tag.
                if (offsetWords < 0 || offsetWords >= qty)
                    continue; // outside range (shouldn't happen for 16-bit but be defensive)

                int byteOff = dataOffset + offsetWords * 2;
                if (byteOff + 2 > pdu.Length)
                    continue;

                ushort clientValue = (ushort)((pdu[byteOff] << 8) | pdu[byteOff + 1]);

                ushort encoded;
                try
                {
                    encoded = BcdCodec.Encode16(clientValue);
                }
                catch (ArgumentOutOfRangeException)
                {
                    RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, tag.Address,
                        clientValue, "Write");
                    ctx.Counters.IncrementInvalidBcd();
                    continue;
                }

                pdu[byteOff]     = (byte)(encoded >> 8);
                pdu[byteOff + 1] = (byte)(encoded & 0xFF);
                ctx.Counters.AddRewrittenSlots(1);
            }
        }
    }

    // ── Response processing (FC03 / FC04) ───────────────────────────────────

    private static void ProcessResponse(byte fc, Span<byte> pdu, PerPlcContext ctx)
    {
        // Check for Modbus exception response (high bit of FC is set).
        if ((fc & 0x80) != 0)
        {
            // Exception response: [fc|0x80][exceptionCode]
            byte originalFc    = (byte)(fc & 0x7F);
            byte exceptionCode = pdu.Length >= 2 ? pdu[1] : (byte)0;

            RewriterLogEvents.ExceptionPassthrough(ctx.Logger, ctx.PlcName, originalFc, exceptionCode);
            ctx.Counters.IncrementBackendException(exceptionCode);
            return; // pass through raw
        }

        switch (fc)
        {
            case 0x03:
            case 0x04:
                // Handled below.
                break;

            case 0x06:
                // FC06 response echoes [fc][addrHi][addrLo][valHi][valLo].
                // Since the proxy re-encoded the request (binary→BCD), the PLC echoes back
                // BCD nibbles. The client expects its original binary value. Decode here.
                ProcessFc06Response(pdu, ctx);
                return;

            case 0x10:
                // FC16 response: [fc][startHi][startLo][qtyHi][qtyLo] — no register data.
                return;

            default:
                return; // all other FCs pass through
        }

        // FC03/04 response: [fc][byteCount][reg0Hi][reg0Lo]...
        // The start address is NOT in the response — the multiplexer attaches the matched
        // InFlightRequest to ctx.CurrentRequest on the response path. Without it (e.g., a
        // unit-test fixture invoking the pipeline directly without correlation) we cannot
        // decode safely; pass the bytes through.
        var currentReq = ctx.CurrentRequest;
        if (currentReq is null)
            return;

        // Only FC03/04 responses should consult start/qty.
        if (currentReq.Fc != 0x03 && currentReq.Fc != 0x04)
            return;

        ushort startAddress = currentReq.StartAddress;
        ushort qty          = currentReq.Qty;

        if (pdu.Length < 2)
            return;

        int byteCount = pdu[1];
        int wordsInResponse = byteCount / 2;

        // Sanity: the qty in the request should match the words in the response.
        // Use the smaller of the two to stay in bounds.
        ushort effectiveQty = (ushort)Math.Min(qty, wordsInResponse);

        if (!ctx.TagMap.TryGetForRange(startAddress, effectiveQty, out var hits))
            return;

        int dataOffset = 2; // pdu[2..] = register data

        foreach (var hit in hits)
        {
            int offsetWords = hit.OffsetWords;
            var tag = hit.Tag;

            if (tag.IsThirtyTwoBit)
            {
                bool lowInRange  = offsetWords >= 0 && offsetWords < effectiveQty;
                bool highInRange = (offsetWords + 1) >= 0 && (offsetWords + 1) < effectiveQty;

                if (!lowInRange || !highInRange)
                {
                    RewriterLogEvents.PartialBcd(ctx.Logger, ctx.PlcName,
                        tag.Address, startAddress, qty);
                    ctx.Counters.IncrementPartialBcd();
                    continue;
                }

                int lowByteOff  = dataOffset + offsetWords * 2;
                int highByteOff = dataOffset + (offsetWords + 1) * 2;

                if (lowByteOff + 2 > pdu.Length || highByteOff + 2 > pdu.Length)
                    continue;

                // CDAB: Address = low register (low 4 BCD digits), Address+1 = high register
                ushort rawLow  = (ushort)((pdu[lowByteOff]  << 8) | pdu[lowByteOff  + 1]);
                ushort rawHigh = (ushort)((pdu[highByteOff] << 8) | pdu[highByteOff + 1]);

                int decoded;
                try
                {
                    decoded = BcdCodec.Decode32(rawLow, rawHigh);
                }
                catch (FormatException)
                {
                    // Emit invalid_bcd for the low register (first bad word we'd encounter).
                    ushort badRaw  = BcdCodec.HasBadNibble(rawLow) ? rawLow : rawHigh;
                    ushort badAddr = BcdCodec.HasBadNibble(rawLow) ? tag.Address : tag.HighRegister;
                    RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, badAddr, badRaw, "Read");
                    ctx.Counters.IncrementInvalidBcd();
                    continue;
                }

                // Write decoded binary value back as a 32-bit value in CDAB layout.
                // The client receives low 4 digits at Address and high 4 digits at Address+1.
                int decodedLow  = decoded % 10_000;
                int decodedHigh = decoded / 10_000;

                pdu[lowByteOff]      = (byte)(decodedLow  >> 8);
                pdu[lowByteOff  + 1] = (byte)(decodedLow  & 0xFF);
                pdu[highByteOff]     = (byte)(decodedHigh >> 8);
                pdu[highByteOff + 1] = (byte)(decodedHigh & 0xFF);
                ctx.Counters.AddRewrittenSlots(2);
            }
            else
            {
                // 16-bit tag.
                if (offsetWords < 0 || offsetWords >= effectiveQty)
                    continue;

                int byteOff = dataOffset + offsetWords * 2;
                if (byteOff + 2 > pdu.Length)
                    continue;

                ushort raw = (ushort)((pdu[byteOff] << 8) | pdu[byteOff + 1]);

                int decoded;
                try
                {
                    decoded = BcdCodec.Decode16(raw);
                }
                catch (FormatException)
                {
                    RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, tag.Address, raw, "Read");
                    ctx.Counters.IncrementInvalidBcd();
                    continue;
                }

                pdu[byteOff]     = (byte)(decoded >> 8);
                pdu[byteOff + 1] = (byte)(decoded & 0xFF);
                ctx.Counters.AddRewrittenSlots(1);
            }
        }
    }

    /// <summary>
    /// FC06 response: [fc=06][addrHi][addrLo][valHi][valLo] — echoes the register address
    /// and the value the PLC wrote (which is now BCD-encoded if the request was rewritten).
    /// Decode the BCD nibbles back to the client's original binary integer so the client
    /// sees the value it sent and library validation (e.g. NModbus echo-check) passes.
    /// </summary>
    private static void ProcessFc06Response(Span<byte> pdu, PerPlcContext ctx)
    {
        if (pdu.Length < 5)
            return;

        ushort address = (ushort)((pdu[1] << 8) | pdu[2]);
        ushort raw     = (ushort)((pdu[3] << 8) | pdu[4]);

        if (!ctx.TagMap.TryGet(address, out var tag))
            return; // not a BCD address

        if (tag.IsThirtyTwoBit)
            return; // partial-write echo — pass through (already warned on request)

        // 16-bit tag: the PLC echoed back BCD nibbles. Decode them back to binary.
        int decoded;
        try
        {
            decoded = BcdCodec.Decode16(raw);
        }
        catch (FormatException)
        {
            RewriterLogEvents.InvalidBcd(ctx.Logger, ctx.PlcName, address, raw, "Read");
            ctx.Counters.IncrementInvalidBcd();
            return;
        }

        pdu[3] = (byte)(decoded >> 8);
        pdu[4] = (byte)(decoded & 0xFF);
        // Note: the RewrittenSlots counter is NOT incremented here because the request
        // already counted this slot on the way out. Incrementing again would double-count.
    }

}