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:14:40 +02:00
co-authored by Claude Opus 4.6
parent 13fac79400
commit deabd257e5
23 changed files with 1975 additions and 163 deletions
Download Patch File Download Diff File Expand all files Collapse all files
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);
}
}
}