MARTe-Integrated-Components/Source/Components/Interfaces/UDPStream/UDPSClient.cpp

/**
 * @file UDPSClient.cpp
 * @brief Implementation of UDPSClient — auto-reconnecting UDPS receiver.
 */

#include "UDPSClient.h"

#include "AdvancedErrorManagement.h"
#include "MemoryOperationsHelper.h"

#include <sys/select.h>
#include <sys/socket.h>
#include <errno.h>

namespace MARTe {

// ---------------------------------------------------------------------------
// Constructor / Destructor
// ---------------------------------------------------------------------------

UDPSClient::UDPSClient()
    : serverPort(0u),
      dataPort(0u),
      useMulticast(false),
      silenceTimeoutTicks(0u),
      reconnectDelayTicks(0u),
      keepAliveIntervalTicks(0u),
      maxPayloadSize(UDPS_CLIENT_DEFAULT_MAX_PAYLOAD),
      cpuMask(0xFFFFFFFFu),
      stackSize(65536u),
      recvBufferSize(UDPS_CLIENT_DEFAULT_RECV_BUFFER),
      listener(NULL_PTR(UDPSClientListener *)),
      threadService(*this),
      connected(false),
      lastDataTicks(0u),
      disconnectTick(0u),
      lastKeepAliveTicks(0u),
      localPort(0u),
      lastGcTicks(0u),
      lastDropWarnTicks(0u),
      droppedSinceWarn(0u),
      lastDataCounter(0u),
      lastDataCounterValid(false),
      lastDataGap(0u),
      staleDataPackets(0u) {

    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        reassemblySlots[i].counter           = 0u;
        reassemblySlots[i].type              = 0u;
        reassemblySlots[i].totalFragments    = 0u;
        reassemblySlots[i].receivedFragments = 0u;
        reassemblySlots[i].active            = false;
        reassemblySlots[i].firstSeenTicks    = 0u;
        reassemblySlots[i].chunkSize         = 0u;
        reassemblySlots[i].assembledBytes    = 0u;
        reassemblySlots[i].pendingTailBytes  = 0u;
        reassemblySlots[i].pendingTailValid  = false;
        (void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0,
                                           UDPS_CLIENT_RECV_MASK_BYTES);
    }
}

UDPSClient::~UDPSClient() {
    (void) Stop();
}

// ---------------------------------------------------------------------------
// Initialise
// ---------------------------------------------------------------------------

bool UDPSClient::Initialise(StructuredDataI &data) {
    StreamString saddr;
    if (!data.Read("ServerAddr", saddr) || (saddr.Size() == 0u)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "UDPSClient: ServerAddr not specified.");
        return false;
    }
    serverAddr = saddr;

    uint32 portU32 = 0u;
    if (!data.Read("Port", portU32)) {
        REPORT_ERROR_STATIC(ErrorManagement::ParametersError,
                            "UDPSClient: Port not specified.");
        return false;
    }
    serverPort = static_cast<uint16>(portU32);

    StreamString mcGroup;
    if (data.Read("MulticastGroup", mcGroup) && (mcGroup.Size() > 0u)) {
        multicastGroup = mcGroup;
        useMulticast   = true;
    }

    if (useMulticast) {
        uint32 dpU32 = static_cast<uint32>(serverPort) + 1u;
        (void) data.Read("DataPort", dpU32);
        dataPort = static_cast<uint16>(dpU32);
        StreamString iface;
        (void) data.Read("Interface", iface);
        multicastInterface = iface;
    }

    float32 silenceS = UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S;
    (void) data.Read("SilenceTimeout", silenceS);
    /* float64 math: the tick rate (~1e9) exceeds float32's 24-bit mantissa */
    silenceTimeoutTicks = static_cast<uint64>(static_cast<float64>(silenceS) *
                                              static_cast<float64>(HighResolutionTimer::Frequency()));

    uint32 reconnectS = UDPS_CLIENT_DEFAULT_RECONNECT_DELAY_S;
    (void) data.Read("ReconnectDelay", reconnectS);
    reconnectDelayTicks = static_cast<uint64>(reconnectS) * HighResolutionTimer::Frequency();

    uint32 keepAliveS = UDPS_CLIENT_DEFAULT_KEEPALIVE_INTERVAL_S;
    (void) data.Read("KeepAliveInterval", keepAliveS);
    keepAliveIntervalTicks = static_cast<uint64>(keepAliveS) * HighResolutionTimer::Frequency();

    uint32 mps = UDPS_CLIENT_DEFAULT_MAX_PAYLOAD;
    (void) data.Read("MaxPayloadSize", mps);
    maxPayloadSize = mps;

    (void) data.Read("CPUMask", cpuMask);
    (void) data.Read("StackSize", stackSize);

    recvBufferSize = UDPS_CLIENT_DEFAULT_RECV_BUFFER;
    (void) data.Read("RecvBufferSize", recvBufferSize);

    return true;
}

// ---------------------------------------------------------------------------
// SetListener / Start / Stop
// ---------------------------------------------------------------------------

void UDPSClient::SetListener(UDPSClientListener *l) {
    listener = l;
}

bool UDPSClient::Start() {
    threadService.SetCPUMask(cpuMask);
    threadService.SetStackSize(stackSize);
    ErrorManagement::ErrorType err = threadService.Start();
    return (err == ErrorManagement::NoError);
}

bool UDPSClient::Stop() {
    ErrorManagement::ErrorType err = threadService.Stop();
    Disconnect();
    return (err == ErrorManagement::NoError);
}

// ---------------------------------------------------------------------------
// Execute (thread entry)
// ---------------------------------------------------------------------------

ErrorManagement::ErrorType UDPSClient::Execute(ExecutionInfo &info) {
    if (info.GetStage() == ExecutionInfo::StartupStage) {
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSClient: Receive thread started.");
        return ErrorManagement::NoError;
    }

    if (info.GetStage() == ExecutionInfo::TerminationStage) {
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSClient: Receive thread stopping.");
        Disconnect();
        return ErrorManagement::NoError;
    }

    // MainStage
    uint64 now = HighResolutionTimer::Counter();

    if (!connected) {
        // Wait reconnectDelay before retrying
        if ((disconnectTick == 0u) ||
            ((now - disconnectTick) >= reconnectDelayTicks)) {
            if (!Connect()) {
                disconnectTick = HighResolutionTimer::Counter();
            }
        }
        return ErrorManagement::NoError;
    }

    // Connected: receive + process
    bool ok = ReceiveAndProcess();
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Receive error; disconnecting.");
        Disconnect();
        return ErrorManagement::NoError;
    }

    /* Re-sample time AFTER ReceiveAndProcess() so that lastDataTicks (which may
     * have been updated inside ReceiveAndProcess) is never newer than `now`.
     * Without this, if a packet arrives during ReceiveAndProcess(), `now` (captured
     * before the call) < lastDataTicks, causing unsigned wraparound in the
     * subtraction and a spurious silence timeout. */
    now = HighResolutionTimer::Counter();

    // Check silence timeout
    if (silenceTimeoutTicks > 0u) {
        uint64 lastSeen = lastDataTicks;
        if ((lastSeen > 0u) && ((now - lastSeen) >= silenceTimeoutTicks)) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: Server silent; disconnecting.");
            Disconnect();
        }
    }

    // Unicast keepalive: UDPSServer evicts silent unicast clients after its
    // ClientTimeout (default 30 s). Re-sending CONNECT would also re-trigger
    // a CONFIG resend; an ACK refreshes the server's last-seen with no side
    // effects, so it is the keepalive packet of choice. Multicast clients
    // hold a persistent TCP control connection and need no keepalive.
    if (!useMulticast && (keepAliveIntervalTicks > 0u)) {
        if ((now - lastKeepAliveTicks) >= keepAliveIntervalTicks) {
            SendKeepAlive();
            lastKeepAliveTicks = now;
        }
    }

    // Periodic GC of stale reassembly slots (~every 1 s)
    uint64 gcFreq = HighResolutionTimer::Frequency();
    if ((now - lastGcTicks) >= gcFreq) {
        GcReassemblySlots();
        lastGcTicks = now;
    }

    return ErrorManagement::NoError;
}

// ---------------------------------------------------------------------------
// Private: Connect
// ---------------------------------------------------------------------------

bool UDPSClient::Connect() {
    bool ok = useMulticast ? ConnectMulticast() : ConnectUnicast();
    if (ok) {
        connected     = true;
        lastDataTicks = HighResolutionTimer::Counter();
        lastKeepAliveTicks = lastDataTicks;
        /* The producer's packetCounter restarts independently of ours, so a
         * counter carried over from the previous connection would make the
         * sequence gate reject the whole new stream as stale. */
        lastDataCounterValid = false;
        lastDataCounter      = 0u;
        lastDataGap          = 0u;
        if (listener != NULL_PTR(UDPSClientListener *)) {
            listener->OnUDPSConnected();
        }
        REPORT_ERROR_STATIC(ErrorManagement::Information,
                            "UDPSClient: Connected to %s:%u.",
                            serverAddr.Buffer(), static_cast<uint32>(serverPort));
    }
    return ok;
}

void UDPSClient::SetRecvBufferSize(BasicUDPSocket &sock) {
    /* BasicUDPSocket exposes no SO_RCVBUF API; the OS default (Linux
     * rmem_default, typically ~208 KiB) is easily overrun by high-throughput
     * sources, causing silent kernel-level datagram drops. Work around this
     * by calling setsockopt() directly on the raw handle. Best-effort: a
     * failure here just leaves the OS default in place. */
    Handle fd = sock.GetReadHandle();
    if (fd >= 0) {
        int32 sz = static_cast<int32>(recvBufferSize);
        if (setsockopt(fd, SOL_SOCKET, SO_RCVBUF, &sz, sizeof(sz)) != 0) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: Could not set SO_RCVBUF to %u bytes.",
                                recvBufferSize);
        }
    }
}

bool UDPSClient::ConnectUnicast() {
    // Open a local UDP socket bound to an ephemeral port
    if (!recvSocket.Open()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not open receive socket.");
        return false;
    }
    SetRecvBufferSize(recvSocket);

    if (!recvSocket.Listen(0u)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not bind receive socket.");
        (void) recvSocket.Close();
        return false;
    }

    // Build and send CONNECT packet to server
    uint8  connectPkt[UDPS_HEADER_SIZE];
    UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);

    InternetHost serverDest(serverPort, serverAddr.Buffer());
    (void) recvSocket.SetDestination(serverDest);
    uint32 sendSize = UDPS_HEADER_SIZE;
    bool   ok       = recvSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not send CONNECT to %s:%u.",
                            serverAddr.Buffer(), static_cast<uint32>(serverPort));
        (void) recvSocket.Close();
        return false;
    }

    return true;
}

bool UDPSClient::ConnectMulticast() {
    /* Join the multicast group BEFORE sending CONNECT over TCP.
     * The UDPStreamer broadcasts CONFIG via multicast immediately when it
     * receives a CONNECT packet.  If the multicast socket is not yet joined,
     * that CONFIG packet is dropped by the kernel and the session never becomes
     * configured.  Correct order: join → CONNECT → receive CONFIG. */

    // Open and bind the multicast receive socket first
    if (!mcastSocket.Open()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not open multicast socket.");
        return false;
    }
    SetRecvBufferSize(mcastSocket);

    bool ok = mcastSocket.Listen(dataPort);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not bind multicast socket on port %u.",
                            static_cast<uint32>(dataPort));
        (void) mcastSocket.Close();
        return false;
    }

    if (multicastInterface.Size() > 0u) {
        ok = mcastSocket.Join(multicastGroup.Buffer(), multicastInterface.Buffer());
    }
    else {
        ok = mcastSocket.Join(multicastGroup.Buffer());
    }
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not join multicast group %s.",
                            multicastGroup.Buffer());
        (void) mcastSocket.Close();
        return false;
    }
    REPORT_ERROR_STATIC(ErrorManagement::Information,
                        "UDPSClient: Joined multicast group %s on port %u via interface %s.",
                        multicastGroup.Buffer(), static_cast<uint32>(dataPort),
                        (multicastInterface.Size() > 0u) ? multicastInterface.Buffer() : "default");

    // Now open the TCP control connection and announce ourselves
    if (!tcpSocket.Open()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not open TCP socket.");
        (void) mcastSocket.Close();
        return false;
    }

    ok = tcpSocket.Connect(serverAddr.Buffer(), serverPort);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: TCP connect to %s:%u failed.",
                            serverAddr.Buffer(), static_cast<uint32>(serverPort));
        (void) tcpSocket.Close();
        (void) mcastSocket.Close();
        return false;
    }

    // Send CONNECT — UDPStreamer will now multicast CONFIG, which we are ready to receive
    uint8  connectPkt[UDPS_HEADER_SIZE];
    UDPSBuildHeader(connectPkt, UDPS_TYPE_CONNECT, 0u, 0u, 1u, 0u);
    uint32 sendSize = UDPS_HEADER_SIZE;
    ok = tcpSocket.Write(reinterpret_cast<const char8 *>(connectPkt), sendSize);
    if (!ok) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: Could not send CONNECT over TCP.");
        (void) tcpSocket.Close();
        (void) mcastSocket.Close();
        return false;
    }

    return true;
}

// ---------------------------------------------------------------------------
// Private: Disconnect
// ---------------------------------------------------------------------------

void UDPSClient::Disconnect() {
    if (!connected) {
        return;
    }

    // Send DISCONNECT
    if (useMulticast) {
        if (tcpSocket.IsValid()) {
            uint8  disconnPkt[UDPS_HEADER_SIZE];
            UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
            uint32 sendSize = UDPS_HEADER_SIZE;
            (void) tcpSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
            (void) tcpSocket.Close();
        }
        if (mcastSocket.IsValid()) {
            (void) mcastSocket.Close();
        }
    }
    else {
        if (recvSocket.IsValid()) {
            uint8  disconnPkt[UDPS_HEADER_SIZE];
            UDPSBuildHeader(disconnPkt, UDPS_TYPE_DISCONNECT, 0u, 0u, 1u, 0u);
            InternetHost serverDest(serverPort, serverAddr.Buffer());
            (void) recvSocket.SetDestination(serverDest);
            uint32 sendSize = UDPS_HEADER_SIZE;
            (void) recvSocket.Write(reinterpret_cast<const char8 *>(disconnPkt), sendSize);
            (void) recvSocket.Close();
        }
    }

    connected      = false;
    disconnectTick = HighResolutionTimer::Counter();

    if (listener != NULL_PTR(UDPSClientListener *)) {
        listener->OnUDPSDisconnected();
    }
}

// ---------------------------------------------------------------------------
// Private: SendKeepAlive
// ---------------------------------------------------------------------------

void UDPSClient::SendKeepAlive() {
    if (useMulticast || !recvSocket.IsValid()) {
        return;
    }
    uint8  ackPkt[UDPS_HEADER_SIZE];
    UDPSBuildHeader(ackPkt, UDPS_TYPE_ACK, 0u, 0u, 1u, 0u);
    InternetHost serverDest(serverPort, serverAddr.Buffer());
    (void) recvSocket.SetDestination(serverDest);
    uint32 sendSize = UDPS_HEADER_SIZE;
    if (!recvSocket.Write(reinterpret_cast<const char8 *>(ackPkt), sendSize)) {
        /* Non-fatal: if the server is truly gone, the silence timeout
         * triggers the usual disconnect + reconnect. */
    }
}

// ---------------------------------------------------------------------------
// Private: ReceiveAndProcess
// ---------------------------------------------------------------------------

bool UDPSClient::ReceiveAndProcess() {
    // Select the receive socket(s)
    int fd = -1;
    int tcpFd = -1;
    if (useMulticast) {
        fd = mcastSocket.GetReadHandle();
        /* In multicast mode the server delivers CONFIG over the TCP control
         * connection — it must be polled too, otherwise the session never
         * becomes configured. */
        tcpFd = tcpSocket.GetReadHandle();
    }
    else {
        fd = recvSocket.GetReadHandle();
    }
    if (fd < 0) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: ReceiveAndProcess: socket fd < 0 (socket invalid).");
        return false;
    }

    /* HI-6: guard against FD_SETSIZE overflow */
    if (fd < 0 || fd >= FD_SETSIZE ||
        (tcpFd >= 0 && tcpFd >= FD_SETSIZE)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: fd >= FD_SETSIZE (%d/%d) — skipping select.",
                            fd, tcpFd);
        return false;
    }

    fd_set rset;
    FD_ZERO(&rset);
    FD_SET(fd, &rset);
    int maxFd = fd;
    if (tcpFd >= 0) {
        FD_SET(tcpFd, &rset);
        if (tcpFd > maxFd) {
            maxFd = tcpFd;
        }
    }
    // 10 ms timeout so Execute() doesn't busy-spin
    struct timeval tv;
    tv.tv_sec = 0; tv.tv_usec = 10000;
    int nready = select(maxFd + 1, &rset, NULL, NULL, &tv);
    if (nready < 0) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: ReceiveAndProcess: select() returned %d (errno=%d).",
                            nready, errno);
        return false; // socket error
    }
    if (nready == 0) {
        return true;  // timeout, no data
    }

    // TCP control frame (multicast CONFIG path)
    if ((tcpFd >= 0) && FD_ISSET(tcpFd, &rset)) {
        if (!ReceiveTCPFrame()) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: TCP control connection lost.");
            return false;
        }
        lastDataTicks = HighResolutionTimer::Counter();
    }

    if (!FD_ISSET(fd, &rset)) {
        return true;  // only the TCP socket was readable
    }

    /* Drain the socket rather than taking one datagram per Execute() iteration:
     * a fragmented high-rate source delivers datagrams far faster than the
     * select/read round trip retires them, and the resulting kernel-buffer
     * overflow shows up as lost fragments — i.e. as packets that can never be
     * reassembled. Bounded so the silence and keepalive checks in Execute()
     * still run under a sustained flood. */
    uint32 drained = 0u;
    while (drained < UDPS_CLIENT_MAX_DATAGRAMS_PER_CYCLE) {
        uint32 recvSize = static_cast<uint32>(sizeof(recvBuf));
        bool   ok;
        if (useMulticast) {
            ok = mcastSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
        }
        else {
            ok = recvSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
        }

        if (!ok || (recvSize < UDPS_HEADER_SIZE)) {
            REPORT_ERROR_STATIC(ErrorManagement::Warning,
                                "UDPSClient: ReceiveAndProcess: Read() failed or short packet "
                                "(ok=%s, recvSize=%u, HEADER_SIZE=%u).",
                                ok ? "true" : "false", recvSize, UDPS_HEADER_SIZE);
            return false;
        }

        lastDataTicks = HighResolutionTimer::Counter();
        ProcessDatagram(recvBuf, recvSize);
        drained++;

        /* Stop as soon as the socket runs dry: Read() would otherwise block. */
        fd_set dset;
        FD_ZERO(&dset);
        FD_SET(fd, &dset);
        struct timeval zero;
        zero.tv_sec = 0; zero.tv_usec = 0;
        if (select(fd + 1, &dset, NULL, NULL, &zero) <= 0) {
            break;
        }
    }
    return true;
}

// ---------------------------------------------------------------------------
// Private: ReceiveTCPFrame / ReadExactTCP
// ---------------------------------------------------------------------------

bool UDPSClient::ReceiveTCPFrame() {
    /* TCP is a byte stream: read exactly one UDPS frame (17-byte header
     * followed by payloadBytes of payload) and hand it to ProcessDatagram.
     * Fragmented CONFIGs arrive as consecutive frames and go through the
     * normal reassembly path. */
    if (!ReadExactTCP(recvBuf, UDPS_HEADER_SIZE)) {
        return false;
    }
    const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(recvBuf);
    if (hdr->magic != UDPS_MAGIC) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: TCP stream desynchronised (bad magic).");
        return false;
    }
    uint32 payloadBytes = hdr->payloadBytes;
    if (payloadBytes > (static_cast<uint32>(sizeof(recvBuf)) - UDPS_HEADER_SIZE)) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: TCP frame payload too large (%u).", payloadBytes);
        return false;
    }
    if (payloadBytes > 0u) {
        if (!ReadExactTCP(&recvBuf[UDPS_HEADER_SIZE], payloadBytes)) {
            return false;
        }
    }
    ProcessDatagram(recvBuf, UDPS_HEADER_SIZE + payloadBytes);
    return true;
}

bool UDPSClient::ReadExactTCP(uint8 *dst, uint32 n) {
    uint32 got = 0u;
    while (got < n) {
        uint32 chunk = n - got;
        if (!tcpSocket.Read(reinterpret_cast<char8 *>(&dst[got]), chunk)) {
            return false;
        }
        if (chunk == 0u) {
            return false;  // orderly close
        }
        got += chunk;
    }
    return true;
}

// ---------------------------------------------------------------------------
// Private: ProcessDatagram
// ---------------------------------------------------------------------------

void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
    if (size < UDPS_HEADER_SIZE) {
        return;
    }
    const UDPSPacketHeader *hdr = reinterpret_cast<const UDPSPacketHeader *>(buf);
    if (hdr->magic != UDPS_MAGIC) {
        return;
    }
    if ((hdr->type != UDPS_TYPE_DATA) && (hdr->type != UDPS_TYPE_CONFIG)) {
        return;
    }

    uint32 payloadBytes = hdr->payloadBytes;
    if ((payloadBytes + UDPS_HEADER_SIZE) > size) {
        return; // truncated
    }

    if (hdr->totalFragments == 1u) {
        if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
            listener->OnUDPSFragment(hdr->counter, size, true);
        }
        // Single-fragment shortcut: deliver immediately
        if (listener != NULL_PTR(UDPSClientListener *)) {
            const uint8 *pl = buf + UDPS_HEADER_SIZE;
            if (hdr->type == UDPS_TYPE_CONFIG) {
                listener->OnUDPSConfig(pl, payloadBytes);
            }
            else if (AcceptDataCounter(hdr->counter)) {
                listener->OnUDPSData(pl, payloadBytes);
            }
        }
        return;
    }

    bool completed = PlaceFragment(hdr, buf + UDPS_HEADER_SIZE, payloadBytes);
    if ((hdr->type == UDPS_TYPE_DATA) && (listener != NULL_PTR(UDPSClientListener *))) {
        listener->OnUDPSFragment(hdr->counter, size, completed);
    }
}

// ---------------------------------------------------------------------------
// Private: PlaceFragment
// ---------------------------------------------------------------------------

uint32 UDPSClient::AcquireReassemblySlot(uint32 counter, uint8 type) {
    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        if (!reassemblySlots[i].active) {
            return i;
        }
    }

    /* All slots busy. The producer emits packets sequentially, so a slot
     * holding an OLDER counter of the SAME stream is provably dead: its
     * missing fragments were sent before the ones arriving now and will never
     * turn up. Reclaiming it immediately — instead of waiting out the 2 s GC —
     * is what keeps a handful of lost fragments from wedging the whole table.
     * The counter is a wrapping uint32, so compare via the signed difference. */
    uint32 victim   = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
    int32  bestDist = 0;
    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        if (reassemblySlots[i].type != type) {
            continue;
        }
        int32 dist = static_cast<int32>(counter - reassemblySlots[i].counter);
        if ((dist > 0) && (dist > bestDist)) {
            bestDist = dist;
            victim   = i;
        }
    }

    if (victim >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
        /* Nothing is provably dead (e.g. the other stream owns every slot):
         * fall back to the least recently started. */
        uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
        victim = 0u;
        for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
            if (reassemblySlots[i].firstSeenTicks < oldestTick) {
                oldestTick = reassemblySlots[i].firstSeenTicks;
                victim     = i;
            }
        }
    }

    NoteDroppedIncomplete(reassemblySlots[victim].counter);
    return victim;
}

void UDPSClient::NoteDroppedIncomplete(uint32 counter) {
    droppedSinceWarn++;
    uint64 now = HighResolutionTimer::Counter();
    if ((now - lastDropWarnTicks) >= HighResolutionTimer::Frequency()) {
        REPORT_ERROR_STATIC(ErrorManagement::Warning,
                            "UDPSClient: dropped %u incomplete packet(s) in the last "
                            "second (latest counter %u); fragments are being lost.",
                            droppedSinceWarn, counter);
        droppedSinceWarn  = 0u;
        lastDropWarnTicks = now;
    }
}

bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
                                const uint8 *payload,
                                uint32 payloadBytes) {
    uint32 counter      = hdr->counter;
    uint8  type         = hdr->type;
    uint16 fragIdx      = hdr->fragmentIdx;
    uint16 totalFrags   = hdr->totalFragments;

    if ((fragIdx >= totalFrags) ||
        (static_cast<uint32>(totalFrags) > UDPS_CLIENT_MAX_FRAGMENTS)) {
        return false; // sanity check
    }

    /* Slots are keyed on (counter, type): DATA and CONFIG carry independent
     * counter sequences, so the same counter value legitimately appears on
     * both, and matching on the counter alone merges the two streams into one
     * slot — one payload is delivered under the wrong type, the other is lost. */
    uint32 slot = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter) &&
            (reassemblySlots[i].type == type)) {
            slot = i;
            break;
        }
    }

    // Allocate new slot if not found
    if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
        slot = AcquireReassemblySlot(counter, type);

        reassemblySlots[slot].counter           = counter;
        reassemblySlots[slot].type              = type;
        reassemblySlots[slot].totalFragments    = totalFrags;
        reassemblySlots[slot].receivedFragments = 0u;
        reassemblySlots[slot].active            = true;
        reassemblySlots[slot].firstSeenTicks    = HighResolutionTimer::Counter();
        reassemblySlots[slot].chunkSize         = 0u;
        reassemblySlots[slot].assembledBytes    = 0u;
        reassemblySlots[slot].pendingTailBytes  = 0u;
        reassemblySlots[slot].pendingTailValid  = false;
        (void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0,
                                           UDPS_CLIENT_RECV_MASK_BYTES);
    }

    UDPSReassemblySlot &s = reassemblySlots[slot];

    // Skip duplicate
    uint32 byteIdx = fragIdx / 8u;
    uint8  bitMask = static_cast<uint8>(1u << (fragIdx % 8u));
    if ((s.recvMask[byteIdx] & bitMask) != 0u) {
        return false; // already have this fragment
    }

    const bool isLastFragment = ((static_cast<uint32>(fragIdx) + 1u) ==
                                 static_cast<uint32>(totalFrags));

    /* Every fragment but the last carries a full chunk, so any of them reveals
     * the chunk size — waiting specifically for fragment 0 means a merely
     * reordered burst, with nothing lost, destroys the packet. */
    if ((s.chunkSize == 0u) && !isLastFragment) {
        s.chunkSize = payloadBytes;
    }

    if (s.chunkSize == 0u) {
        /* The last fragment arrived before any full-size one: its offset is
         * not computable yet, so hold it until the chunk size is known. */
        if (payloadBytes > UDPS_CLIENT_PENDING_TAIL_BYTES) {
            return false;
        }
        if (payloadBytes > 0u) {
            (void) MemoryOperationsHelper::Copy(s.pendingTail, payload, payloadBytes);
        }
        s.pendingTailBytes = payloadBytes;
        s.pendingTailValid = true;
        s.recvMask[byteIdx] |= bitMask;
        s.receivedFragments++;
        return false; // totalFrags > 1 here, so this can never complete a packet
    }

    // Flush a deferred last fragment now that the chunk size is known.
    if (s.pendingTailValid) {
        uint32 tailOffset = (static_cast<uint32>(s.totalFragments) - 1u) * s.chunkSize;
        if ((tailOffset + s.pendingTailBytes) > static_cast<uint32>(sizeof(s.payload))) {
            s.active = false;
            NoteDroppedIncomplete(s.counter);
            return false; // overflow guard
        }
        if (s.pendingTailBytes > 0u) {
            (void) MemoryOperationsHelper::Copy(s.payload + tailOffset,
                                                s.pendingTail, s.pendingTailBytes);
        }
        if ((tailOffset + s.pendingTailBytes) > s.assembledBytes) {
            s.assembledBytes = tailOffset + s.pendingTailBytes;
        }
        s.pendingTailValid = false;
        s.pendingTailBytes = 0u;
    }

    uint32 offset = static_cast<uint32>(fragIdx) * s.chunkSize;
    if ((offset + payloadBytes) > static_cast<uint32>(sizeof(s.payload))) {
        return false; // overflow guard
    }

    if (payloadBytes > 0u) {
        (void) MemoryOperationsHelper::Copy(s.payload + offset, payload, payloadBytes);
    }

    // Track the exact assembled size: the highest byte written across all
    // fragments. The last fragment is usually smaller than chunkSize, so the
    // total is not chunkSize*totalFragments.
    if ((offset + payloadBytes) > s.assembledBytes) {
        s.assembledBytes = offset + payloadBytes;
    }

    s.recvMask[byteIdx] |= bitMask;
    s.receivedFragments++;

    // Check if complete
    if (s.receivedFragments >= s.totalFragments) {
        DeliverAssembled(s);
        s.active = false;
        return true;
    }
    return false;
}

// ---------------------------------------------------------------------------
// Private: DeliverAssembled
// ---------------------------------------------------------------------------

bool UDPSClient::AcceptDataCounter(uint32 counter) {
    if (!lastDataCounterValid) {
        lastDataCounterValid = true;
        lastDataCounter      = counter;
        lastDataGap          = 0u;
        return true;
    }
    // The counter is a wrapping uint32, so order it by the signed difference:
    // that stays correct across the wrap, where a plain comparison would call
    // the first packet after it stale and reject the stream from then on.
    int32 delta = static_cast<int32>(counter - lastDataCounter);
    if (delta <= 0) {
        staleDataPackets++;
        return false;
    }
    lastDataGap     = static_cast<uint32>(delta) - 1u;
    lastDataCounter = counter;
    return true;
}

uint32 UDPSClient::GetLastDataGap() const { return lastDataGap; }

uint32 UDPSClient::GetStaleDataPackets() const { return staleDataPackets; }

void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
    if (listener == NULL_PTR(UDPSClientListener *)) {
        return;
    }
    // The exact assembled size is the highest byte offset written across all
    // fragments (the last fragment is typically smaller than chunkSize), tracked
    // incrementally in PlaceFragment.
    uint32 totalSize = s.assembledBytes;

    if (s.type == UDPS_TYPE_CONFIG) {
        listener->OnUDPSConfig(s.payload, totalSize);
    }
    else if (AcceptDataCounter(s.counter)) {
        listener->OnUDPSData(s.payload, totalSize);
    }
}

// ---------------------------------------------------------------------------
// Private: GcReassemblySlots
// ---------------------------------------------------------------------------

void UDPSClient::GcReassemblySlots() {
    uint64 staleThreshold = 2u * HighResolutionTimer::Frequency(); // 2 seconds
    uint64 now            = HighResolutionTimer::Counter();

    for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
        if (!reassemblySlots[i].active) {
            continue;
        }
        if ((now - reassemblySlots[i].firstSeenTicks) > staleThreshold) {
            reassemblySlots[i].active = false;
            NoteDroppedIncomplete(reassemblySlots[i].counter);
        }
    }
}

} // namespace MARTe