martino/MARTe-Integrated-Components
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix(udps): stop packets being dated from an earlier time base

Browse Source 
Reported as samples sporadically carrying a previous packet's timestamp:
holes on one side of the stream and collisions on the other, in both the
Go and the MARTe2 receiver. That it appeared in both is what located it
-- the shared cause is upstream of either client. Four independent
defects, all of which end in a packet's values being placed at a time
that is not theirs.

Reassembly slot exhaustion (the "Reassembly slots full; evicting oldest"
flood). Chunk size was learnt only from fragment 0, so an out-of-order
burst destroyed a packet whose bytes had all arrived and left the slot
occupied until the 2 s GC. Slots were keyed on the counter alone, but
DATA and CONFIG number independently, so equal counters merged the two
streams. The 32-byte received-mask covered 256 of the 512 fragments the
client accepts, so a duplicate above 255 was counted as new and the
packet was delivered with a hole of stale bytes in it. And one datagram
was read per Execute(), which cannot drain a fast producer. Fixed with a
pendingTail deferral, (counter, type) keying, a 64-byte mask, a
256-datagram drain, counter-age slot reclamation, and a 1 Hz aggregated
warning in place of the per-eviction flood.

UDPStreamer dropping whole Accumulate batches. EventSem::ResetWait is
Reset-then-Wait, so a Post() landing while the sender thread was inside
ServiceClients()/SendData() was destroyed by the next Reset. The batch
was then skipped with dataReady false, readyFill was never cleared, and
the following flush overwrote it: an entire run of RT cycles never
reached the wire. The record of pending work now lives in the buffers
rather than in the semaphore edge, which also removes up to
UDPS_DATA_WAIT_MS of latency; genuine backpressure overwrites are
counted and reported. Against the unfixed code the new test sees
2999/3000 batches never consumed.

Period inflation after loss. Accumulated scalars carry no SamplingRate,
so the receiver derives dt from the sender-clock gap -- but dividing it
by the previous packet's sample count is only right while nothing is
lost. One loss doubles the reported period, which spreads a batch a full
batch past its own end and into the range the next packet claims. That
is the hole and the collision, exactly. Inferring the cycle count from
the estimate's own period is not a way out: it has a stable fixed point
wherever gap/dt is an integer, so a real rate change locks it at the old
one for good (AccumDtGTest.FollowsSustainedRateChange).

The packet counter removes the ambiguity, so all three receivers now
order on it: a DATA packet that does not advance the counter is dropped
rather than delivered, because its values are older than data already
handed over. Ordering is on the signed difference so it survives the
uint32 wrap, and the sequence resets on reconnect, where the producer's
counter restarts independently of ours. The loss count that falls out of
the same delta feeds the period estimate as cycles = prevN * (1 + lost),
which reduces exactly to gap/prevN when nothing is lost and therefore
still tracks a genuine rate change. UDPSClient::AcceptDataCounter (C++),
udpsprotocol.SequenceGate (Go), decode_data (C).

The C client's existing gap counter was wrap-unsafe and let a stale
packet rewind last_counter, which made every subsequent gap wrong; it
uses the same code now. Docs/Protocol.md gains an Ordering DATA section
stating the requirement for any receiver, including ones outside this
repository.

Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Martino Ferrari
2026-09-02 01:18:49 +02:00
co-authored by Claude Opus 4.6
parent f334995865
commit fbae7d712c
23 changed files with 1975 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
Source/Applications/StreamHub/UDPSourceSession.cpp
+20 -7
View File
@@ -99,6 +99,8 @@ void UDPSourceSession::ResetCalibration() {
lastPktWallValid_[i] = false;
lastPktWallS_[i] = 0.0;
accScalarPrevN_[i] = 0u;
accScalarDtValid_[i] = false;
accScalarDtEMA_[i] = 0.0;
}
}
@@ -197,7 +199,8 @@ void UDPSourceSession::OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {
}
void UDPSourceSession::OnUDPSData(const uint8 *payload, uint32 payloadSize) {
ParseDataPayload(payload, payloadSize);
/* Valid only for the duration of this callback. */
ParseDataPayload(payload, payloadSize, client_.GetLastDataGap());
}
/*---------------------------------------------------------------------------*/
@@ -376,7 +379,8 @@ float64 UDPSourceSession::ProducerNewestTime() const {
/* DATA parsing */
/*---------------------------------------------------------------------------*/
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size,
uint32 lostPackets) {
if (size < 8u) { return; }
/* Copy metadata under lock */
@@ -581,16 +585,25 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
}
/* Per-sample dt: samplingRate if present, else derive it from
* the sender-HRT gap to the previous packet divided by that
* packet's sample count (the flushes carry contiguous RT
* cycles, so this is exactly one cycle period). */
* the sender-HRT gap to the previous packet.
*
* The gap is divided by the number of RT cycles it actually
* spans, not by the previous packet's sample count. Those two
* agree only while nothing is lost; once a packet goes missing
* the gap covers cycles the previous count never saw, and
* dividing by that count inflates dt until this packet's
* samples overrun into the next packet's range. lostPackets
* comes from the producer's packet counter, so the divisor
* widens with the gap and dt is unchanged. */
float64 dt;
if (desc.samplingRate > 0.0) {
dt = 1.0 / desc.samplingRate;
} else if (lastPktWallValid_[s] && (accScalarPrevN_[s] > 0u) &&
(hrt0Sec > lastPktWallS_[s])) {
dt = (hrt0Sec - lastPktWallS_[s]) /
static_cast<float64>(accScalarPrevN_[s]);
dt = UDPSEstimateAccumDt(hrt0Sec - lastPktWallS_[s],
accScalarPrevN_[s], lostPackets,
accScalarDtEMA_[s],
accScalarDtValid_[s]);
} else {
dt = 1.0e-3; /* 1 kHz default until the gap is known */
}
Source/Applications/StreamHub/UDPSourceSession.h
+76 -1
View File
@@ -39,6 +39,69 @@ using MARTe::ConfigurationDatabase;
/** Maximum number of signals per source session. */
static const uint32 UDPSS_MAX_SIGNALS = 256u;
/* Accumulated-scalar dt estimator tuning. */
/** Weight of a new observation; slow enough that one bad gap barely moves it. */
static const float64 UDPSS_DT_EMA_ALPHA = 0.05;
/** Observations outside [lo, hi] x the current estimate are treated as a
* mis-counted gap and discarded rather than smoothed in. */
static const float64 UDPSS_DT_ACCEPT_LO = 0.5;
static const float64 UDPSS_DT_ACCEPT_HI = 2.0;
/**
* @brief Per-sample period of an accumulated scalar packet, robust to loss.
*
* An Accumulate producer batches consecutive RT cycles, so the sender-clock
* gap between two packets' first samples covers exactly as many cycles as the
* earlier packet carried — but only while nothing is lost in between. Over UDP
* (and with a producer that can overwrite a batch the sender never took) that
* assumption fails, and dividing the gap by the previous packet's sample count
* then inflates the period. The packet's own samples are laid out as
* base + e*dt, so an inflated dt walks them past their real end and into the
* span the next packet will claim: samples collide there and leave a hole
* behind them.
*
* The number of packets that went missing is not guessed from the gap — that
* is circular, and an estimator that infers the cycle count from its own
* period has a stable fixed point wherever gap/dt is an integer, so a genuine
* rate change locks it at the old period forever. It comes instead from the
* UDPS packet counter, which the producer increments once per sent packet. The
* gap then spans (1 + lost) batches, each assumed to be prevN cycles, and with
* nothing lost the formula reduces exactly to gap/prevN.
*
* @param gap Sender-clock seconds since the previous packet's first sample.
* Must be > 0.
* @param prevN Samples in the previous packet. Must be > 0.
* @param lost Packets missing between the previous packet and this one,
* from the producer's counter.
* @param[in,out] dtEMA Smoothed period. Seeded on the first call.
* @param[in,out] dtValid False until dtEMA holds an estimate.
* @return The period to space this packet's samples by.
*/
inline float64 UDPSEstimateAccumDt(const float64 gap, const uint32 prevN,
const uint32 lost, float64 &dtEMA,
bool &dtValid) {
float64 cycles = static_cast<float64>(prevN) *
(1.0 + static_cast<float64>(lost));
if (cycles < 1.0) {
cycles = 1.0;
}
const float64 dtObs = gap / cycles;
if (!dtValid) {
dtEMA = dtObs;
dtValid = true;
} else if ((dtObs > (dtEMA * UDPSS_DT_ACCEPT_LO)) &&
(dtObs < (dtEMA * UDPSS_DT_ACCEPT_HI))) {
/* Track slow drift, but ignore observations far outside the current
* estimate: those are the signature of a mis-counted gap, and folding
* one in would drag the estimate towards the very error it exists to
* absorb. */
dtEMA = ((1.0 - UDPSS_DT_EMA_ALPHA) * dtEMA) +
(UDPSS_DT_EMA_ALPHA * dtObs);
}
return dtEMA;
}
/**
* @brief One connected UDPStreamer source.
*
@@ -229,7 +292,13 @@ private:
/* DATA payload parsing */
void ParseConfigPayload(const uint8 *payload, uint32 size);
void ParseDataPayload(const uint8 *payload, uint32 size);
/**
* @param lostPackets DATA packets missing immediately before this one, from
* the producer's counter; the accumulated-scalar period estimate
* needs it to know how many cycles the sender-clock gap spans.
*/
void ParseDataPayload(const uint8 *payload, uint32 size,
uint32 lostPackets);
void AllocateRingBuffers();
/** @brief Invalidate all wall-clock calibration state (receive thread only). */
@@ -401,6 +470,12 @@ private:
float64 hrtFreq_;
uint32 accScalarPrevN_[UDPSS_MAX_SIGNALS];
/* Per-signal state of UDPSEstimateAccumDt (see above): the smoothed
* per-sample period for accumulated scalars whose descriptor carries no
* SamplingRate. */
float64 accScalarDtEMA_[UDPSS_MAX_SIGNALS];
bool accScalarDtValid_[UDPSS_MAX_SIGNALS];
/* Scratch buffers for decoding arrays (receive thread only). */
float64 *timeScratch_; ///< Time values scratch
float64 *valScratch_; ///< Data values scratch
Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp
+105 -43
View File
@@ -107,6 +107,9 @@ UDPStreamer::UDPStreamer()
readyTimestamps = NULL_PTR(uint64 *);
scratchTimestamps = NULL_PTR(uint64 *);
readyFill = 0u;
readySnapshotPending = false;
droppedPublications = 0u;
lastDropReportTicks = 0u;
decimateRatio = 1u;
decimateCounter = 0u;
@@ -871,6 +874,11 @@ bool UDPStreamer::Synchronise() {
/* HI-3: if accumFill reached maxBatchCount, force-flush before writing */
if (accumFill >= maxBatchCount) {
uint32 filled = accumFill;
if (readyFill > 0u) {
/* The sender has not taken the previous batch: it is about to be
* overwritten and its cycles will never reach any receiver. */
droppedPublications++;
}
(void)MemoryOperationsHelper::Copy(readyBuffer, accumBuffer,
filled * totalSrcBytes);
(void)MemoryOperationsHelper::Copy(
@@ -901,6 +909,9 @@ bool UDPStreamer::Synchronise() {
if (sizeCondition || timeCondition) {
bufMutex.FastLock(TTInfiniteWait);
if (readyFill > 0u) {
droppedPublications++;
}
(void)MemoryOperationsHelper::Copy(readyBuffer, accumBuffer,
filled * totalSrcBytes);
(void)MemoryOperationsHelper::Copy(
@@ -922,16 +933,24 @@ bool UDPStreamer::Synchronise() {
if (decimateCounter >= decimateRatio) {
decimateCounter = 0u;
bufMutex.FastLock(TTInfiniteWait);
if (readySnapshotPending) {
droppedPublications++;
}
(void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
syncTimestamp = ts;
readySnapshotPending = true;
bufMutex.FastUnLock();
(void)dataSem.Post();
}
} else {
/* --- Strict path: post every call --- */
bufMutex.FastLock(TTInfiniteWait);
if (readySnapshotPending) {
droppedPublications++;
}
(void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
syncTimestamp = ts;
readySnapshotPending = true;
bufMutex.FastUnLock();
(void)dataSem.Post();
}
@@ -955,60 +974,71 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
}
if (info.GetStage() == ExecutionInfo::MainStage) {
/* --- Wait for RT thread to post new data ---
* ResetWait sleeps the background thread until the RT thread calls
* Synchronise() and posts dataSem, or until the timeout expires.
* Doing this FIRST means the thread spends nearly all its time here
* instead of spinning on the non-blocking select() below.
* Command latency is bounded by UDPS_DATA_WAIT_MS (acceptable for
* CONNECT / DISCONNECT). */
ErrorManagement::ErrorType waitErr =
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
bool dataReady = (waitErr == ErrorManagement::NoError);
/* --- Wait for the RT thread to publish new data ---
* dataSem is only a wake-up hint, never the record of pending work:
* EventSem::ResetWait resets the semaphore before waiting, so a Post that
* landed while this thread was inside ServiceClients()/SendData() is
* destroyed by the next Reset. Deciding what to send from the wait result
* would then skip that publication entirely, and the next flush would
* overwrite it — the receiver sees the batch's whole time span missing.
* The buffers therefore carry the state, and are only waited on when they
* are empty (which also avoids paying the wait when work is already
* queued). */
if (!HasPendingPublication()) {
(void)dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
}
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) ---
*/
server.ServiceClients();
if (dataReady && server.HasClients()) {
/* Synchronise() already gates posting dataSem to the correct rate
* (size/time for Accumulate, every-Nth for Decimate, every call for
* Strict). Execute() just sends whatever is in the ready buffers. */
if (publishMode == UDPStreamerPublishAccumulate) {
/* --- Accumulate batch send --- */
uint32 fill = 0u;
bufMutex.FastLock(TTInfiniteWait);
fill = readyFill;
if (fill > 0u) {
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
fill * totalSrcBytes);
(void)MemoryOperationsHelper::Copy(
reinterpret_cast<uint8 *>(scratchTimestamps),
reinterpret_cast<const uint8 *>(readyTimestamps),
fill * static_cast<uint32>(sizeof(uint64)));
}
bufMutex.FastUnLock();
/* Synchronise() already gates publication to the correct rate (size/time
* for Accumulate, every-Nth for Decimate, every call for Strict). The
* pending publication is consumed whether or not anyone is listening, so
* that a client-less streamer neither spins here nor delivers a stale
* snapshot to the next client that connects. */
if (publishMode == UDPStreamerPublishAccumulate) {
/* --- Accumulate batch send --- */
uint32 fill = 0u;
bufMutex.FastLock(TTInfiniteWait);
fill = readyFill;
if (fill > 0u) {
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
fill * totalSrcBytes);
(void)MemoryOperationsHelper::Copy(
reinterpret_cast<uint8 *>(scratchTimestamps),
reinterpret_cast<const uint8 *>(readyTimestamps),
fill * static_cast<uint32>(sizeof(uint64)));
readyFill = 0u;
}
bufMutex.FastUnLock();
if (fill > 0u) {
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
uint32 sendBytes =
UDPS_TIMESTAMP_BYTES + 4u + fill * singleCycleWireBytes;
packetCounter++;
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send Accumulate DATA packet (counter=%u).",
packetCounter);
}
if ((fill > 0u) && server.HasClients()) {
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
uint32 sendBytes =
UDPS_TIMESTAMP_BYTES + 4u + fill * singleCycleWireBytes;
packetCounter++;
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
REPORT_ERROR(ErrorManagement::Warning,
"Failed to send Accumulate DATA packet (counter=%u).",
packetCounter);
}
} else {
/* --- Single-snapshot send (Strict or Decimate) --- */
uint64 ts = 0u;
bufMutex.FastLock(TTInfiniteWait);
}
} else {
/* --- Single-snapshot send (Strict or Decimate) --- */
uint64 ts = 0u;
bool pending = false;
bufMutex.FastLock(TTInfiniteWait);
pending = readySnapshotPending;
if (pending) {
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
totalSrcBytes);
ts = syncTimestamp;
bufMutex.FastUnLock();
readySnapshotPending = false;
}
bufMutex.FastUnLock();
if (pending && server.HasClients()) {
QuantizeAndSerialize(scratchBuffer, ts);
packetCounter++;
@@ -1019,6 +1049,8 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
}
}
}
ReportDroppedPublications();
}
if (info.GetStage() == ExecutionInfo::TerminationStage) {
@@ -1327,6 +1359,36 @@ bool UDPStreamer::IsClientConnected() const { return server.HasClients(); }
bool UDPStreamer::IsMulticast() const { return server.IsMulticast(); }
uint32 UDPStreamer::GetDroppedPublications() const { return droppedPublications; }
bool UDPStreamer::HasPendingPublication() {
bool pending = false;
bufMutex.FastLock(TTInfiniteWait);
pending = (readyFill > 0u) || readySnapshotPending;
bufMutex.FastUnLock();
return pending;
}
void UDPStreamer::ReportDroppedPublications() {
uint64 now = HighResolutionTimer::Counter();
if ((now - lastDropReportTicks) < HighResolutionTimer::Frequency()) {
return;
}
lastDropReportTicks = now;
uint32 dropped = 0u;
bufMutex.FastLock(TTInfiniteWait);
dropped = droppedPublications;
bufMutex.FastUnLock();
if (dropped > 0u) {
REPORT_ERROR(ErrorManagement::Warning,
"Dropped %u unsent publication(s) so far: the sender thread is "
"not keeping up with the RT cycle.",
dropped);
}
}
CLASS_REGISTER(UDPStreamer, "1.0")
} /* namespace MARTe */
Source/Components/DataSources/UDPStreamer/UDPStreamer.h
+30
View File
@@ -322,6 +322,17 @@ public:
*/
bool IsMulticast() const;
/**
* @brief Number of publications the sender thread never put on the wire.
* @details Synchronise() promotes a snapshot (Strict/Decimate) or a batch
* (Accumulate) to the ready buffer for the sender thread. If the next
* promotion arrives before the sender has taken the previous one, that
* publication is overwritten and its cycles never reach any receiver —
* which a consumer sees as a hole in the time series. Counts those, so the
* loss is measurable rather than inferred from the plot.
*/
uint32 GetDroppedPublications() const;
private:
/**
* @brief Serializes the CONFIG payload into buf and sets payloadSize.
@@ -349,6 +360,18 @@ private:
*/
static uint8 TypeDescriptorToCode(TypeDescriptor td);
/**
* @brief True when the ready buffer holds data the sender has not taken yet.
* @details Read under bufMutex. The sender must consult this rather than
* rely on the dataSem edge, which ResetWait can destroy.
*/
bool HasPendingPublication();
/**
* @brief Emits at most one warning per second about overwritten publications.
*/
void ReportDroppedPublications();
/* Configuration parameters */
uint16 port; /**< UDP server port */
uint32 maxPayloadSize; /**< Max payload bytes per UDP packet (excluding header) */
@@ -367,6 +390,13 @@ private:
uint64 *readyTimestamps; /**< Heap: [maxBatchCount] HRT for completed ready batch */
uint64 *scratchTimestamps; /**< Heap: [maxBatchCount] background-thread local copy */
uint32 readyFill; /**< Snapshot count in the ready batch */
/** Strict/Decimate: readyBuffer holds a snapshot the sender has not taken
* yet. Publication state must live here rather than in dataSem, because
* EventSem::ResetWait resets before waiting and so destroys any Post that
* landed while the sender was busy. */
bool readySnapshotPending;
uint32 droppedPublications; /**< Publications overwritten before being sent */
uint64 lastDropReportTicks; /**< Sender-thread rate limit for the drop warning */
/* Decimate mode */
uint32 decimateRatio; /**< Send 1 packet every decimateRatio Synchronise() calls */
uint32 decimateCounter; /**< Current decimate cycle counter */
Source/Components/Interfaces/UDPStream/UDPSClient.cpp
+198 -65
View File
@@ -36,7 +36,13 @@ UDPSClient::UDPSClient()
disconnectTick(0u),
lastKeepAliveTicks(0u),
localPort(0u),
lastGcTicks(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;
@@ -46,7 +52,11 @@ UDPSClient::UDPSClient()
reassemblySlots[i].active = false;
reassemblySlots[i].firstSeenTicks = 0u;
reassemblySlots[i].chunkSize = 0u;
(void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0, 32u);
reassemblySlots[i].assembledBytes = 0u;
reassemblySlots[i].pendingTailBytes = 0u;
reassemblySlots[i].pendingTailValid = false;
(void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0,
UDPS_CLIENT_RECV_MASK_BYTES);
}
}
@@ -228,6 +238,12 @@ bool UDPSClient::Connect() {
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();
}
@@ -498,25 +514,45 @@ bool UDPSClient::ReceiveAndProcess() {
return true; // only the TCP socket was readable
}
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);
}
/* 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;
}
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);
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;
}
@@ -599,7 +635,7 @@ void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
if (hdr->type == UDPS_TYPE_CONFIG) {
listener->OnUDPSConfig(pl, payloadBytes);
}
else {
else if (AcceptDataCounter(hdr->counter)) {
listener->OnUDPSData(pl, payloadBytes);
}
}
@@ -616,21 +652,83 @@ void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
// 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) || (totalFrags > 512u)) {
if ((fragIdx >= totalFrags) ||
(static_cast<uint32>(totalFrags) > UDPS_CLIENT_MAX_FRAGMENTS)) {
return false; // sanity check
}
// Find existing slot for this counter
/* 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)) {
if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter) &&
(reassemblySlots[i].type == type)) {
slot = i;
break;
}
@@ -638,34 +736,20 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
// Allocate new slot if not found
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
if (!reassemblySlots[i].active) {
slot = i;
break;
}
}
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
// All slots occupied — evict the oldest
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
if (reassemblySlots[i].firstSeenTicks < oldestTick) {
oldestTick = reassemblySlots[i].firstSeenTicks;
slot = i;
}
}
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Reassembly slots full; evicting oldest.");
}
slot = AcquireReassemblySlot(counter, type);
reassemblySlots[slot].counter = counter;
reassemblySlots[slot].type = hdr->type;
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;
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0, 32u);
reassemblySlots[slot].pendingTailBytes = 0u;
reassemblySlots[slot].pendingTailValid = false;
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0,
UDPS_CLIENT_RECV_MASK_BYTES);
}
UDPSReassemblySlot &s = reassemblySlots[slot];
@@ -673,27 +757,56 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
// Skip duplicate
uint32 byteIdx = fragIdx / 8u;
uint8 bitMask = static_cast<uint8>(1u << (fragIdx % 8u));
if (byteIdx < 32u) {
if ((s.recvMask[byteIdx] & bitMask) != 0u) {
return false; // already have this fragment
}
if ((s.recvMask[byteIdx] & bitMask) != 0u) {
return false; // already have this fragment
}
// Compute placement offset
uint32 chunkSize = s.chunkSize;
if (chunkSize == 0u) {
// Learn chunk size from first non-last fragment
if (fragIdx == 0u) {
chunkSize = payloadBytes;
s.chunkSize = chunkSize;
}
else {
// Can't place yet without knowing chunk size — drop (rare edge case)
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
}
uint32 offset = static_cast<uint32>(fragIdx) * chunkSize;
// 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
}
@@ -709,9 +822,7 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
s.assembledBytes = offset + payloadBytes;
}
if (byteIdx < 32u) {
s.recvMask[byteIdx] |= bitMask;
}
s.recvMask[byteIdx] |= bitMask;
s.receivedFragments++;
// Check if complete
@@ -727,6 +838,30 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
// 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;
@@ -739,7 +874,7 @@ void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
if (s.type == UDPS_TYPE_CONFIG) {
listener->OnUDPSConfig(s.payload, totalSize);
}
else {
else if (AcceptDataCounter(s.counter)) {
listener->OnUDPSData(s.payload, totalSize);
}
}
@@ -757,10 +892,8 @@ void UDPSClient::GcReassemblySlots() {
continue;
}
if ((now - reassemblySlots[i].firstSeenTicks) > staleThreshold) {
REPORT_ERROR_STATIC(ErrorManagement::Warning,
"UDPSClient: Discarding stale reassembly slot (counter %u).",
reassemblySlots[i].counter);
reassemblySlots[i].active = false;
NoteDroppedIncomplete(reassemblySlots[i].counter);
}
}
}
Source/Components/Interfaces/UDPStream/UDPSClient.h
+84 -5
View File
@@ -82,10 +82,27 @@ public:
* update for one source is fragmented into MaxPayloadSize chunks; this is
* the ceiling on the reassembled total, so it bounds the largest multi-
* fragment packet the client can deliver. Sized for large array bursts
* (e.g. 8x10000 float32 ~= 320 KiB) with headroom; stays well within the
* 256-fragment span the recvMask[32] tracks at typical chunk sizes. */
* (e.g. 8x10000 float32 ~= 320 KiB) with headroom. */
static const uint32 UDPS_CLIENT_MAX_PACKET_BYTES = 1048576u; // 1 MiB
/** Maximum fragment count accepted for one packet. The received-fragment
* bitmask must cover this whole span: a fragment index the mask cannot
* represent has no duplicate detection, so a duplicated datagram counts
* twice and the packet is delivered with a fragment still missing. */
static const uint32 UDPS_CLIENT_MAX_FRAGMENTS = 512u;
/** Bytes of received-fragment bitmask (one bit per fragment). */
static const uint32 UDPS_CLIENT_RECV_MASK_BYTES =
UDPS_CLIENT_MAX_FRAGMENTS / 8u;
/** Size of the per-slot buffer that holds a last fragment which arrived
* before the chunk size was known. Fragments larger than this cannot be
* deferred and are dropped (the packet then fails to reassemble). */
static const uint32 UDPS_CLIENT_PENDING_TAIL_BYTES = 8192u;
/** Maximum datagrams drained from the socket per Execute() iteration. */
static const uint32 UDPS_CLIENT_MAX_DATAGRAMS_PER_CYCLE = 256u;
/** Default silence timeout before reconnect (seconds); sub-second values allowed. */
static const float32 UDPS_CLIENT_DEFAULT_SILENCE_TIMEOUT_S = 1.0f;
@@ -155,6 +172,22 @@ public:
*/
virtual ErrorManagement::ErrorType Execute(ExecutionInfo &info);
/**
* @brief DATA packets that went missing immediately before the one being
* delivered, from the gap in the producer's packet counter.
* @details Valid for the duration of the OnUDPSData() callback. A listener
* that reconstructs per-sample timestamps needs this: without it, the time
* elapsed since the previous packet looks like it covers only that
* packet's samples, so the inferred sample period comes out too long and
* the samples are spread past where they belong.
*/
uint32 GetLastDataGap() const;
/**
* @brief DATA packets discarded for arriving after a newer one.
*/
uint32 GetStaleDataPackets() const;
private:
// -------------------------------------------------------------------------
@@ -165,12 +198,19 @@ private:
uint8 type; ///< UDPS_TYPE_DATA or UDPS_TYPE_CONFIG
uint16 totalFragments; ///< Expected fragment count
uint16 receivedFragments; ///< How many we have so far
uint8 recvMask[32]; ///< Bitmask: bit f set iff fragment f received
uint8 recvMask[UDPS_CLIENT_RECV_MASK_BYTES]; ///< Bit f set iff fragment f received
uint8 payload[UDPS_CLIENT_MAX_PACKET_BYTES]; ///< Assembled payload buffer
uint64 firstSeenTicks; ///< For GC (2 s stale detection)
bool active; ///< Slot in use
uint32 chunkSize; ///< Payload bytes per fragment (from first fragment)
uint32 chunkSize; ///< Payload bytes per fragment (any non-last fragment)
uint32 assembledBytes; ///< Exact total payload bytes placed so far
/** Last fragment received before chunkSize was known: its offset is
* not yet computable, so it waits here until a full-size fragment
* reveals the chunk size. Only the last fragment can ever be short,
* hence one deferred fragment per slot is enough. */
uint8 pendingTail[UDPS_CLIENT_PENDING_TAIL_BYTES];
uint32 pendingTailBytes;
bool pendingTailValid;
};
// -------------------------------------------------------------------------
@@ -195,8 +235,39 @@ private:
bool ReadExactTCP(uint8 *dst, uint32 n);
/** @return true iff this fragment completed the reassembly (payload delivered). */
bool PlaceFragment(const UDPSPacketHeader *hdr, const uint8 *payload, uint32 payloadBytes);
/**
* @brief Reserve a reassembly slot for (@p counter, @p type), reclaiming
* one if none is free.
* @return the slot index (always valid).
*/
uint32 AcquireReassemblySlot(uint32 counter, uint8 type);
/**
* @brief Account one packet abandoned with fragments missing, and report
* it at most once per second.
* @details Fragment loss on a busy stream is chronic, not exceptional: an
* unconditional message per drop buries every other log line.
*/
void NoteDroppedIncomplete(uint32 counter);
void GcReassemblySlots();
void DeliverAssembled(UDPSReassemblySlot &slot);
/**
* @brief Sequence gate for DATA packets, applied just before delivery.
* @details The producer numbers DATA packets consecutively, so the counter
* reveals both how many packets went missing and whether this one is late.
* A late packet must not be delivered: its samples predate what the
* listener has already stored, so they land behind the current write
* position and collide with data that is already there — which is what a
* consumer sees as two signals occupying the same instant. Reassembly
* completes in arrival order, not counter order, so this ordering is not
* guaranteed upstream.
*
* Also records the number of packets missing immediately before this one,
* for GetLastDataGap().
*
* @param counter The candidate packet's UDPS counter.
* @return true if the packet is newer than the last delivered one.
*/
bool AcceptDataCounter(uint32 counter);
// -------------------------------------------------------------------------
// Configuration
@@ -235,7 +306,15 @@ private:
// Reassembly
UDPSReassemblySlot reassemblySlots[UDPS_CLIENT_MAX_REASSEMBLY_SLOTS];
uint64 lastGcTicks; ///< Ticks at last GC run
uint64 lastGcTicks; ///< Ticks at last GC run
uint64 lastDropWarnTicks;///< Ticks at last incomplete-packet report
uint32 droppedSinceWarn; ///< Incomplete packets since that report
// DATA sequencing (see AcceptDataCounter)
uint32 lastDataCounter; ///< Counter of the last delivered DATA packet
bool lastDataCounterValid; ///< False until the first DATA packet
uint32 lastDataGap; ///< Packets missing before the current one
uint32 staleDataPackets; ///< DATA packets discarded as late
// Receive scratch buffer
uint8 recvBuf[65535u + UDPS_HEADER_SIZE];