diff --git a/Common/Client/c/udps_client.c b/Common/Client/c/udps_client.c index f56e1e1..64925c4 100644 --- a/Common/Client/c/udps_client.c +++ b/Common/Client/c/udps_client.c @@ -482,11 +482,36 @@ static int decode_data(udps_client_t *c, const uint8_t *pl, size_t len, size_t total = 0u; size_t written = 0u; uint32_t i; + uint32_t lost = 0u; udps_frame_t frame; if (c->num_sigs == 0u) { return 0; /* DATA before CONFIG: nothing to decode against. */ } + + /* Order the sequence before spending anything on the payload. + * + * Reassembly completes in arrival order, not counter order, so a packet + * delayed or duplicated on the wire surfaces after a newer one has already + * been delivered. Its samples carry an older time base: they land on top + * of data the consumer already has and leave the span they should have + * filled empty. Nothing in the payload distinguishes such a packet from a + * good one, only the counter does. + * + * The counter is a wrapping uint32, so it is ordered by the signed + * difference; comparing the values directly would call the first packet + * after the wrap stale and reject the stream from then on. */ + if (c->have_counter) { + int32_t delta = (int32_t)(counter - c->last_counter); + if (delta <= 0) { + c->stats.stale_packets++; + return 0; + } + lost = (uint32_t)delta - 1u; + c->stats.counter_gaps += lost; + } + c->last_counter = counter; + c->have_counter = 1; if (len < 8u) { return fail(c, "DATA payload too short (%lu bytes)", (unsigned long)len); } @@ -528,15 +553,11 @@ static int decode_data(udps_client_t *c, const uint8_t *pl, size_t len, written += count; } - if (c->have_counter && counter > c->last_counter + 1u) { - c->stats.counter_gaps += counter - c->last_counter - 1u; - } - c->last_counter = counter; - c->have_counter = 1; c->stats.frames_delivered++; if (c->on_data != NULL) { frame.counter = counter; + frame.lost = lost; frame.hrt = rd_u64(pl); frame.recv_time = recv_time; frame.publish_mode = c->publish_mode; diff --git a/Common/Client/c/udps_client.h b/Common/Client/c/udps_client.h index d23a09c..0d3fe8b 100644 --- a/Common/Client/c/udps_client.h +++ b/Common/Client/c/udps_client.h @@ -140,6 +140,16 @@ typedef struct { /** One fully decoded DATA packet. */ typedef struct { uint32_t counter; /**< Packet counter; gaps mean lost datagrams. */ + /** + * DATA packets missing immediately before this one, from the counter. + * + * Needed to space samples correctly: the elapsed time since the previous + * frame covers the lost packets' cycles too, so dividing it by this + * frame's sample count alone gives a period too long by exactly + * @c lost + 1, which walks the samples past their own end and into the + * range the next frame claims. + */ + uint32_t lost; uint64_t hrt; /**< Producer's high-resolution timer at send. */ double recv_time; /**< Wall-clock seconds (CLOCK_REALTIME) at arrival. */ uint8_t publish_mode; /**< UDPS_PUBLISH_*. */ @@ -164,6 +174,12 @@ typedef struct { uint64_t config_updates; uint64_t fragments_dropped; /**< Duplicate, stale or unplaceable fragments. */ uint64_t counter_gaps; /**< DATA packets missing from the sequence. */ + /** + * DATA packets dropped for not advancing the counter: reordered or + * duplicated on the wire. Delivering one would stamp its values with a + * time base older than data already handed over. + */ + uint64_t stale_packets; uint64_t reconnects; } udps_stats_t; diff --git a/Common/Client/go/udpsprotocol/accumulate_array_test.go b/Common/Client/go/udpsprotocol/accumulate_array_test.go new file mode 100644 index 0000000..50c3aaf --- /dev/null +++ b/Common/Client/go/udpsprotocol/accumulate_array_test.go @@ -0,0 +1,146 @@ +package udpsprotocol + +// Accumulate mode ships one full snapshot of EVERY signal per RT cycle — +// arrays included. See UDPStreamer.cpp pass 5 ("ALL signals (scalars and +// arrays alike) are tagged accumulated = true") and SerializeAccumulated, +// which writes, for each signal in CONFIG order, numSamples consecutive +// snapshots of that signal's full element set. +// +// The tests below build a payload byte-for-byte the way the C++ producer +// does, so a decoding regression shows up here rather than as a mangled +// waveform three components downstream. + +import ( + "encoding/binary" + "math" + "testing" + "time" +) + +// buildAccumulatePayload lays out an Accumulate DATA payload exactly as +// UDPStreamer::SerializeAccumulated does: +// +// [8 HRT][4 numSamples] then, per signal, numSamples × NumElements float64. +// +// slots[i][k] holds signal i's element set for cycle k. +func buildAccumulatePayload(hrt uint64, slots [][][]float64) []byte { + numSamples := 0 + if len(slots) > 0 { + numSamples = len(slots[0]) + } + out := make([]byte, 12) + binary.LittleEndian.PutUint64(out[0:8], hrt) + binary.LittleEndian.PutUint32(out[8:12], uint32(numSamples)) + for _, sig := range slots { + for _, elems := range sig { + for _, v := range elems { + var b [8]byte + binary.LittleEndian.PutUint64(b[:], math.Float64bits(v)) + out = append(out, b[:]...) + } + } + } + return out +} + +// TestParseDataAccumulateGivesEachSlotItsOwnArray pins the array case: with an +// accumulated batch, slot k's array signal must decode to the values the +// producer captured on cycle k, not to some other cycle's. Handing every slot +// slot 0's array would stamp one cycle's data with every slot's timestamp — +// the same samples drawn repeatedly at advancing times, with the cycles they +// displaced missing entirely. +func TestParseDataAccumulateGivesEachSlotItsOwnArray(t *testing.T) { + sigs := []SignalInfo{ + {Name: "Time", TypeCode: 9, NumRows: 1, NumCols: 1, QuantType: QuantNone}, + {Name: "Wave", TypeCode: 9, NumRows: 4, NumCols: 1, QuantType: QuantNone}, + } + // Three RT cycles. "Wave" carries a different ramp each cycle so a + // mix-up is unambiguous. + timeSlots := [][]float64{{10}, {20}, {30}} + waveSlots := [][]float64{ + {1, 2, 3, 4}, + {5, 6, 7, 8}, + {9, 10, 11, 12}, + } + payload := buildAccumulatePayload(777, [][][]float64{timeSlots, waveSlots}) + + samples, err := ParseData(payload, sigs, PublishModeAccumulate, time.Now()) + if err != nil { + t.Fatalf("ParseData: %v", err) + } + if len(samples) != 3 { + t.Fatalf("expected 3 slots, got %d", len(samples)) + } + for k, s := range samples { + got := s.Values["Wave"] + want := waveSlots[k] + if len(got) != len(want) { + t.Fatalf("slot %d: Wave has %d elements, want %d", k, len(got), len(want)) + } + for i := range want { + if got[i] != want[i] { + t.Fatalf("slot %d: Wave = %v, want %v (slot %d's data has been "+ + "served for this slot's timestamp)", k, got, want, + indexOfSlot(waveSlots, got)) + } + } + if tv := s.Values["Time"]; len(tv) != 1 || tv[0] != timeSlots[k][0] { + t.Fatalf("slot %d: Time = %v, want %v", k, tv, timeSlots[k]) + } + } +} + +// TestParseDataAccumulateConsumesTheWholeArrayBlock catches the same defect +// from the other side: a signal following an array must be read at the right +// offset. Under-reading the array block slides every later signal backwards +// into the array's tail, which decodes as plausible-looking but wrong values +// rather than as an error. +func TestParseDataAccumulateConsumesTheWholeArrayBlock(t *testing.T) { + sigs := []SignalInfo{ + {Name: "Wave", TypeCode: 9, NumRows: 4, NumCols: 1, QuantType: QuantNone}, + {Name: "Tail", TypeCode: 9, NumRows: 1, NumCols: 1, QuantType: QuantNone}, + } + waveSlots := [][]float64{ + {1, 2, 3, 4}, + {5, 6, 7, 8}, + {9, 10, 11, 12}, + } + tailSlots := [][]float64{{100}, {200}, {300}} + payload := buildAccumulatePayload(0, [][][]float64{waveSlots, tailSlots}) + + samples, err := ParseData(payload, sigs, PublishModeAccumulate, time.Now()) + if err != nil { + t.Fatalf("ParseData: %v", err) + } + if len(samples) != 3 { + t.Fatalf("expected 3 slots, got %d", len(samples)) + } + for k, s := range samples { + tv := s.Values["Tail"] + if len(tv) != 1 || tv[0] != tailSlots[k][0] { + t.Fatalf("slot %d: Tail = %v, want %v — the array block before it "+ + "was not fully consumed", k, tv, tailSlots[k]) + } + } +} + +// indexOfSlot reports which slot's data a decoded array actually matches, so a +// failure message can name the culprit instead of just showing numbers. +func indexOfSlot(slots [][]float64, got []float64) int { + for k, want := range slots { + if len(want) != len(got) { + continue + } + same := true + for i := range want { + if want[i] != got[i] { + same = false + break + } + } + if same { + return k + } + } + return -1 +} diff --git a/Common/Client/go/udpsprotocol/protocol.go b/Common/Client/go/udpsprotocol/protocol.go index f213b3a..8365987 100644 --- a/Common/Client/go/udpsprotocol/protocol.go +++ b/Common/Client/go/udpsprotocol/protocol.go @@ -290,28 +290,37 @@ type DataSample struct { HRTTimestamp uint64 WallTime time.Time // wall-clock time at UDP arrival; used as x-axis Values map[string][]float64 // key = signal name, value = []float64 with NumElements entries + // Lost is the number of DATA packets missing between the previous sample + // and this one, taken from the producer's packet counter (see + // SequenceGate). Consumers that derive a per-element period from the + // inter-packet gap need it: the gap widens with every lost packet, and + // dividing it by this packet's element count alone reports a period too + // long by exactly that factor — which walks the packet's elements past + // their own end and into the range the next packet claims. + Lost uint32 } // parseElems reads n elements for sig from payload at offset, advancing offset. // Returns the slice of float64 values and the new offset. func parseElems(payload []byte, offset, n int, sig SignalInfo) ([]float64, int, error) { + sz := rawTypeSize(sig.TypeCode) + if sig.QuantType != QuantNone { + sz = quantSize(sig.QuantType) + } + // Bounds-check before allocating. In Accumulate mode n is numSamples × + // NumElements, so a malformed packet could otherwise ask for an allocation + // far larger than its own payload could ever justify. + if n < 0 || n > (len(payload)-offset)/sz { + return nil, offset, fmt.Errorf("data payload truncated for signal %q", sig.Name) + } elems := make([]float64, n) + needed := n * sz if sig.QuantType == QuantNone { - sz := rawTypeSize(sig.TypeCode) - needed := n * sz - if offset+needed > len(payload) { - return nil, offset, fmt.Errorf("data payload truncated for signal %q", sig.Name) - } for i := 0; i < n; i++ { elems[i] = readRawElement(payload, offset+i*sz, sig.TypeCode) } offset += needed } else { - sz := quantSize(sig.QuantType) - needed := n * sz - if offset+needed > len(payload) { - return nil, offset, fmt.Errorf("data payload truncated (quant) for signal %q", sig.Name) - } for i := 0; i < n; i++ { var raw uint16 if sz == 1 { @@ -331,7 +340,13 @@ func parseElems(payload []byte, offset, n int, sig SignalInfo) ([]float64, int, // // For PublishModeAccumulate the payload format is: // -// [8 HRT][4 numSamples][for each signal: accumulated scalars → numSamples elems; arrays → NumElements elems] +// [8 HRT][4 numSamples][for each signal: numSamples × NumElements elems] +// +// Every signal is accumulated, arrays included: the producer captures one full +// snapshot of the whole signal set per RT cycle and lays the cycles out +// contiguously per signal (UDPStreamer::SerializeAccumulated). Reading only +// NumElements for an array would hand every slot the first cycle's data and +// slide all later signals into that array's tail. // // The function returns one DataSample per accumulated snapshot so the hub can // process each slot independently with its own timestamp. @@ -357,28 +372,18 @@ func ParseData(payload []byte, sigs []SignalInfo, publishMode uint8, arrivalTime } // Parse per-signal data blocks (all slots for a signal are contiguous). - accumVals := make(map[string][]float64, len(sigs)) // scalars: numSamples values - fixedVals := make(map[string][]float64, len(sigs)) // arrays: NumElements values + accumVals := make(map[string][]float64, len(sigs)) // numSamples × NumElements + accumElems := make(map[string]int, len(sigs)) for _, sig := range sigs { n := sig.NumElements() - if n == 1 { - // Accumulated scalar: read numSamples back-to-back elements. - elems, newOff, err := parseElems(payload, offset, numSamples, sig) - if err != nil { - return nil, err - } - offset = newOff - accumVals[sig.Name] = elems - } else { - // Fixed array (non-accumulated): one set of NumElements values. - elems, newOff, err := parseElems(payload, offset, n, sig) - if err != nil { - return nil, err - } - offset = newOff - fixedVals[sig.Name] = elems + elems, newOff, err := parseElems(payload, offset, numSamples*n, sig) + if err != nil { + return nil, err } + offset = newOff + accumVals[sig.Name] = elems + accumElems[sig.Name] = n } // Build one DataSample per slot. @@ -386,10 +391,10 @@ func ParseData(payload []byte, sigs []SignalInfo, publishMode uint8, arrivalTime for k := 0; k < numSamples; k++ { vals := make(map[string][]float64, len(sigs)) for sigName, av := range accumVals { - vals[sigName] = []float64{av[k]} - } - for sigName, fv := range fixedVals { - vals[sigName] = fv // shared read-only reference; hub does not modify + n := accumElems[sigName] + // Sub-slice of the decoded block; the hub treats values as + // read-only, so no copy is needed. + vals[sigName] = av[k*n : (k+1)*n : (k+1)*n] } samples[k] = DataSample{HRTTimestamp: hrt, WallTime: arrivalTime, Values: vals} } diff --git a/Common/Client/go/udpsprotocol/sequence.go b/Common/Client/go/udpsprotocol/sequence.go new file mode 100644 index 0000000..cdaa0f9 --- /dev/null +++ b/Common/Client/go/udpsprotocol/sequence.go @@ -0,0 +1,49 @@ +package udpsprotocol + +// SequenceGate orders DATA packets by the producer's packet counter. +// +// Reassembly completes in arrival order, not counter order, so a packet that +// was delayed or duplicated on the wire is handed up after a newer one has +// already been consumed. Its samples then carry an older time base than the +// data already in the ring: they land on top of samples that are already +// there, and the span they should have filled stays empty. That is a hole on +// one side and a collision on the other, from a packet that is entirely +// well-formed — the counter is the only thing that distinguishes it. +// +// A SequenceGate is not safe for concurrent use; each receive loop owns one. +type SequenceGate struct { + last uint32 + valid bool + // Stale counts packets rejected for not advancing the counter (reordered + // or duplicated), for diagnostics. + Stale uint64 +} + +// Reset forgets the sequence. Call it on (re)connect: the producer's counter +// restarts independently of ours, so a counter carried over from the previous +// connection would reject the whole new stream. +func (g *SequenceGate) Reset() { + g.last = 0 + g.valid = false +} + +// Accept reports whether a DATA packet with this counter should be delivered, +// and how many packets went missing immediately before it. +// +// The counter is a wrapping uint32, so ordering is done on the signed +// difference: a plain comparison would call the first packet after the wrap +// stale and reject everything from then on. +func (g *SequenceGate) Accept(counter uint32) (ok bool, lost uint32) { + if !g.valid { + g.valid = true + g.last = counter + return true, 0 + } + delta := int32(counter - g.last) + if delta <= 0 { + g.Stale++ + return false, 0 + } + g.last = counter + return true, uint32(delta) - 1 +} diff --git a/Common/Client/go/udpsprotocol/sequence_test.go b/Common/Client/go/udpsprotocol/sequence_test.go new file mode 100644 index 0000000..e90969f --- /dev/null +++ b/Common/Client/go/udpsprotocol/sequence_test.go @@ -0,0 +1,82 @@ +package udpsprotocol + +import "testing" + +// A packet older than one already delivered carries an older time base. Its +// samples land on top of data that is already in the ring and leave the span +// they should have filled empty, so it must not get through. +func TestSequenceGateRejectsStaleAndDuplicate(t *testing.T) { + var g SequenceGate + + if ok, lost := g.Accept(10); !ok || lost != 0 { + t.Fatalf("first packet: got (%v, %d), want (true, 0)", ok, lost) + } + if ok, _ := g.Accept(11); !ok { + t.Fatal("counter 11 advances past 10 and must be accepted") + } + if ok, _ := g.Accept(9); ok { + t.Error("counter 9 is older than the delivered 11 and must be dropped") + } + if ok, _ := g.Accept(11); ok { + t.Error("a repeat of the delivered counter must be dropped") + } + if g.Stale != 2 { + t.Errorf("Stale = %d, want 2", g.Stale) + } + // The rejections must not have moved the sequence on. + if ok, lost := g.Accept(12); !ok || lost != 0 { + t.Errorf("after rejections: got (%v, %d), want (true, 0)", ok, lost) + } +} + +// The loss count is what lets a consumer tell a widened gap from a slowed +// producer, so it must exclude the packet being delivered and must not persist +// into the next one. +func TestSequenceGateReportsLoss(t *testing.T) { + var g SequenceGate + + g.Accept(100) + if _, lost := g.Accept(104); lost != 3 { + t.Errorf("101..103 missing: lost = %d, want 3", lost) + } + if _, lost := g.Accept(105); lost != 0 { + t.Errorf("consecutive packet: lost = %d, want 0", lost) + } + if g.Stale != 0 { + t.Errorf("Stale = %d, want 0", g.Stale) + } +} + +// The counter is a wrapping uint32. Ordering it by plain comparison would call +// every packet after the wrap older than 0xFFFFFFFF and kill the stream. +func TestSequenceGateSurvivesWraparound(t *testing.T) { + var g SequenceGate + + for _, c := range []uint32{0xFFFFFFFD, 0xFFFFFFFE, 0xFFFFFFFF, 0, 1, 2} { + ok, lost := g.Accept(c) + if !ok { + t.Fatalf("counter %#x rejected across the wrap", c) + } + if lost != 0 { + t.Errorf("counter %#x: lost = %d, want 0", c, lost) + } + } + // Loss must still be measured correctly across the wrap. + var h SequenceGate + h.Accept(0xFFFFFFFE) + if _, lost := h.Accept(1); lost != 2 { + t.Errorf("0xFFFFFFFF and 0 missing: lost = %d, want 2", lost) + } +} + +// A reconnect restarts the producer's counter independently of ours; a carried +// over counter would reject the entire new stream. +func TestSequenceGateResetAcceptsLowerCounter(t *testing.T) { + var g SequenceGate + + g.Accept(5000) + g.Reset() + if ok, lost := g.Accept(3); !ok || lost != 0 { + t.Errorf("after Reset: got (%v, %d), want (true, 0)", ok, lost) + } +} diff --git a/Common/Client/go/wshub/hub.go b/Common/Client/go/wshub/hub.go index e3924ee..eef1228 100644 --- a/Common/Client/go/wshub/hub.go +++ b/Common/Client/go/wshub/hub.go @@ -1216,15 +1216,24 @@ func (h *Hub) buildBinaryDataMessageForSource(src *sourceHubState, batch []udpsp wallNs := s.WallTime.UnixNano() wallSec := float64(wallNs) / 1e9 var dtSec float64 + // A gap spans the elements of every packet that went missing + // inside it as well as this packet's own, so the divisor has + // to widen with it. Without this a single loss halves the + // apparent rate and the elements overrun into the next + // packet's range. The loss count belongs to the packet the + // gap ends at. if bi+1 < len(batch) { // Two consecutive packets in this tick → exact dt. - dtSec = (float64(batch[bi+1].WallTime.UnixNano()) - float64(wallNs)) / 1e9 / float64(n) + span := float64(n) * float64(1+batch[bi+1].Lost) + dtSec = (float64(batch[bi+1].WallTime.UnixNano()) - float64(wallNs)) / 1e9 / span } else if bi > 0 { // Last of multiple packets → use diff from previous. - dtSec = (float64(wallNs) - float64(batch[bi-1].WallTime.UnixNano())) / 1e9 / float64(n) + span := float64(n) * float64(1+s.Lost) + dtSec = (float64(wallNs) - float64(batch[bi-1].WallTime.UnixNano())) / 1e9 / span } else if prevNs, ok2 := src.lastPktNs[sig.Name]; ok2 && prevNs > 0 && wallNs > prevNs { // Single packet this tick → gap from the previous tick's packet. - dtSec = (float64(wallNs) - float64(prevNs)) / 1e9 / float64(n) + span := float64(n) * float64(1+s.Lost) + dtSec = (float64(wallNs) - float64(prevNs)) / 1e9 / span } else { // Truly first packet ever — inter-packet timing unknown. // Skip to avoid poisoning the ring with wrongly-spaced timestamps; diff --git a/Common/Client/go/wshub/packet_dt_loss_test.go b/Common/Client/go/wshub/packet_dt_loss_test.go new file mode 100644 index 0000000..b180985 --- /dev/null +++ b/Common/Client/go/wshub/packet_dt_loss_test.go @@ -0,0 +1,139 @@ +package wshub + +import ( + "math" + "testing" + "time" + + "marte2/common/udpsprotocol" +) + +// pktSignal is a 4-element array with no time signal and no declared sampling +// rate, i.e. the TimeModePacket path where dt has to be inferred from the gap +// between packets. +func pktSignal(name string) udpsprotocol.SignalInfo { + return udpsprotocol.SignalInfo{ + Name: name, + TypeCode: 8, // float64 + NumDimensions: 1, + NumRows: 4, + NumCols: 1, + TimeMode: udpsprotocol.TimeModePacket, + TimeSignalIdx: udpsprotocol.NoTimeSignal, + } +} + +// newPacketDtHub builds a Hub with a ring for one packet-timed array signal and +// returns both. It does not start Run(): buildBinaryDataMessageForSource is +// called directly so the timestamps it produces can be read back verbatim. +func newPacketDtHub(t *testing.T, sigName string) (*Hub, *sourceHubState, *sigRing) { + t.Helper() + h := NewHub() + src := &sourceHubState{ + id: "s1", + signals: []udpsprotocol.SignalInfo{pktSignal(sigName)}, + timeSigCalib: map[string]float64{}, + lastPktNs: map[string]int64{}, + lastFrameMeasured: map[string]float64{}, + lastFrameEndT: map[string]float64{}, + gapEMA: map[string]float64{}, + } + rb := newSigRing(4096) + h.rings["s1:"+sigName] = rb + return h, src, rb +} + +// packet builds a one-signal batch entry arriving at t0 with the given loss +// count; the values are irrelevant, only the timestamps are under test. +func packet(sigName string, at time.Time, lost uint32, n int) udpsprotocol.DataSample { + vals := make([]float64, n) + return udpsprotocol.DataSample{WallTime: at, Values: map[string][]float64{sigName: vals}, Lost: lost} +} + +// ringTimes returns the timestamps written to the ring, in order. +func ringTimes(rb *sigRing) []float64 { + rb.mu.RLock() + defer rb.mu.RUnlock() + out := make([]float64, 0, rb.size) + start := (rb.head - rb.size + rb.cap) % rb.cap + for i := 0; i < rb.size; i++ { + out = append(out, rb.t[(start+i)%rb.cap]) + } + return out +} + +// A lost packet widens the inter-packet gap without adding elements to the +// packet that follows it. Dividing the gap by that packet's element count +// alone reports a period too long by exactly the number of packets missing, +// which walks the elements past their own end and into the range the next +// packet claims: they collide there, and the span they vacated stays empty. +func TestPacketDtIgnoresLostPacketWidening(t *testing.T) { + const sig = "Wave" + const n = 4 + const dt = 1 * time.Millisecond + base := time.Unix(1700000000, 0) + + h, src, rb := newPacketDtHub(t, sig) + + // One clean packet establishes lastPktNs. + h.buildBinaryDataMessageForSource(src, []udpsprotocol.DataSample{ + packet(sig, base, 0, n)}) + + // The next producer packet is lost, so the one after it arrives a full + // extra batch later and reports Lost=1. + arrival := base.Add(2 * n * dt) + h.buildBinaryDataMessageForSource(src, []udpsprotocol.DataSample{ + packet(sig, arrival, 1, n)}) + + ts := ringTimes(rb) + if len(ts) != n { + t.Fatalf("ring holds %d points, want %d (the first packet is skipped: no gap yet)", len(ts), n) + } + got := ts[1] - ts[0] + if !nearSec(got, dt.Seconds()) { + t.Errorf("dt = %v s, want %v s (the gap spans two batches, not one)", got, dt.Seconds()) + } + // Elements run forward from the packet's own arrival, so a doubled dt + // would stretch this batch across two batch periods and into the range the + // next packet claims. + span := ts[len(ts)-1] - ts[0] + if !nearSec(span, float64(n-1)*dt.Seconds()) { + t.Errorf("batch spans %v s, want %v s: it overruns into the next packet's range", + span, float64(n-1)*dt.Seconds()) + } +} + +// nearSec compares two intervals in seconds. The hub carries timestamps as +// float64 seconds derived from UnixNano, whose spacing near the current epoch +// is a couple of hundred nanoseconds, so exact equality is not available. The +// defect under test moves the period by a factor of two, three orders of +// magnitude outside this tolerance. +func nearSec(got, want float64) bool { return math.Abs(got-want) <= 1e-6 } + +// The correction must be driven by the reported loss and nothing else: with no +// packet missing the period still comes straight from the gap, so a producer +// that genuinely slows down is followed rather than second-guessed. +func TestPacketDtFollowsGapWhenNothingIsLost(t *testing.T) { + const sig = "Wave" + const n = 4 + base := time.Unix(1700000000, 0) + + h, src, rb := newPacketDtHub(t, sig) + + h.buildBinaryDataMessageForSource(src, []udpsprotocol.DataSample{ + packet(sig, base, 0, n)}) + + // Same widened gap as the test above, but reported as no loss: the + // producer really is running at half the rate. + slowDt := 2 * time.Millisecond + h.buildBinaryDataMessageForSource(src, []udpsprotocol.DataSample{ + packet(sig, base.Add(n*slowDt), 0, n)}) + + ts := ringTimes(rb) + if len(ts) != n { + t.Fatalf("ring holds %d points, want %d", len(ts), n) + } + if got := ts[1] - ts[0]; !nearSec(got, slowDt.Seconds()) { + t.Errorf("dt = %v s, want %v s: a real rate change must be followed", got, slowDt.Seconds()) + } +} diff --git a/Common/Client/go/wshub/sources.go b/Common/Client/go/wshub/sources.go index dd8c6fa..bca0f8d 100644 --- a/Common/Client/go/wshub/sources.go +++ b/Common/Client/go/wshub/sources.go @@ -341,6 +341,9 @@ func (u *UDPClient) runSession() error { } reassembler := udpsprotocol.NewReassembler(2 * time.Second) + // Per-session: the producer's counter restarts independently of ours, so + // the gate must not carry a counter over from the previous connection. + var gate udpsprotocol.SequenceGate buf := make([]byte, readBufSize) var currentSigs []udpsprotocol.SignalInfo var currentPublishMode uint8 @@ -414,11 +417,20 @@ func (u *UDPClient) runSession() error { if len(currentSigs) == 0 { continue } + fresh, lost := gate.Accept(hdr.Counter) + if !fresh { + continue + } samples, err := udpsprotocol.ParseData(complete, currentSigs, currentPublishMode, arrivalTime) if err != nil { log.Printf("[%s] udp: parse data: %v", u.sourceID, err) continue } + // The gap precedes the packet, so it belongs to its first slot only; + // the slots after it are consecutive cycles of the same batch. + if len(samples) > 0 { + samples[0].Lost = lost + } for _, s := range samples { u.hub.PushDataForSource(u.sourceID, s) } @@ -589,6 +601,9 @@ func (u *UDPClient) runMulticastSession() error { }() reassembler := udpsprotocol.NewReassembler(2 * time.Second) + // Per-session, as in runSession(): a counter from the previous connection + // would reject the whole new stream. + var gate udpsprotocol.SequenceGate buf := make([]byte, readBufSize) for { @@ -629,11 +644,18 @@ func (u *UDPClient) runMulticastSession() error { if len(currentSigs) == 0 { continue } + fresh, lost := gate.Accept(hdr.Counter) + if !fresh { + continue + } samples, parseErr := udpsprotocol.ParseData(complete, currentSigs, currentPublishMode, arrivalTime) if parseErr != nil { log.Printf("[%s] multicast: parse data: %v", u.sourceID, parseErr) continue } + if len(samples) > 0 { + samples[0].Lost = lost + } for _, s := range samples { u.hub.PushDataForSource(u.sourceID, s) } diff --git a/Docs/Protocol.md b/Docs/Protocol.md index 1497729..2c02be5 100644 --- a/Docs/Protocol.md +++ b/Docs/Protocol.md @@ -171,6 +171,39 @@ the client to reassemble them in any order. --- +## Ordering DATA (required of every receiver) + +DATA carries its own `counter` sequence, incremented once per sent packet +(CONFIG is numbered independently). Reassembly completes in arrival order, not +counter order, so a packet reordered or duplicated on the wire surfaces after a +newer one has already been consumed. Its values are well-formed but carry an +older time base: accepting it writes them over samples the consumer already +holds and leaves the span they should have filled empty — a collision on one +side and a hole on the other. + +A receiver must therefore drop any DATA packet that does not advance the +counter, and must order it by the *signed* difference: + +```c +int32_t delta = (int32_t)(counter - lastCounter); /* survives the uint32 wrap */ +if (delta <= 0) { /* stale or duplicate: drop */ } +lost = (uint32_t)delta - 1u; /* packets missing before this one */ +``` + +Comparing the values directly would call the first packet after the wrap stale +and reject the stream from then on. + +`lost` matters beyond diagnostics. A consumer that spaces batched samples from +the elapsed time since the previous packet must divide that gap by `lost + 1` +batches; dividing by one batch reports a period too long by exactly that factor +and walks the samples past their own end into the next packet's range. Reset +the sequence on (re)connect: the producer's counter restarts independently. + +Implemented in `UDPSClient::AcceptDataCounter` (C++), +`udpsprotocol.SequenceGate` (Go) and `decode_data` (C). + +--- + ## Minimal Python Client Example ```python diff --git a/Docs/UDPS-C-Client.md b/Docs/UDPS-C-Client.md index 139d7f3..3b55e44 100644 --- a/Docs/UDPS-C-Client.md +++ b/Docs/UDPS-C-Client.md @@ -175,6 +175,7 @@ replayed traffic can be decoded without a client. ```c typedef struct { uint32_t counter; /* gaps in this sequence are lost datagrams */ + uint32_t lost; /* DATA packets missing immediately before this one */ uint64_t hrt; /* producer's high-resolution timer at send */ double recv_time; /* CLOCK_REALTIME seconds at arrival */ uint8_t publish_mode; @@ -194,6 +195,13 @@ scalar signal in Accumulate mode, where the producer batches several RT cycles i and `count == num_samples` — one value per cycle. Arrays are not batched: they appear once and apply to the whole packet. `udps_frame_value(f, sig, sample, elem)` applies that rule for you. +**Ordering.** Frames reach `on_data` in counter order: a DATA packet that does not advance the +counter — reordered or duplicated on the wire — is dropped rather than delivered, because its +values carry a time base older than data you already have, and placing them would overwrite live +samples while leaving their own span empty. `lost` reports how many packets went missing just +before the frame. If you space samples yourself from the elapsed time since the previous frame, +divide by `lost + 1` batches, not one: the gap covers the missing packets' cycles too. + **Timestamps.** The protocol does not put a timestamp on every element; how to date them depends on the signal's `time_mode` (see [Protocol.md](Protocol.md#time-mode-codes)): @@ -223,6 +231,7 @@ udps_client_stats(cli, &s); | `frames_delivered` | DATA packets decoded and passed to `on_data`. | | `config_updates` | CONFIG packets applied. | | `counter_gaps` | Missing packet counters — datagrams lost on the wire or in the kernel. | +| `stale_packets` | DATA packets dropped for not advancing the counter: reordered or duplicated on the wire. | | `fragments_dropped` | Duplicate, stale or unplaceable fragments; a non-zero value with `counter_gaps` means fragmented updates are arriving incomplete. | | `reconnects` | Sessions re-established after a silence timeout. | diff --git a/Source/Applications/StreamHub/UDPSourceSession.cpp b/Source/Applications/StreamHub/UDPSourceSession.cpp index 7cca9d2..a0a5029 100644 --- a/Source/Applications/StreamHub/UDPSourceSession.cpp +++ b/Source/Applications/StreamHub/UDPSourceSession.cpp @@ -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(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 */ } diff --git a/Source/Applications/StreamHub/UDPSourceSession.h b/Source/Applications/StreamHub/UDPSourceSession.h index 9162f9d..f0f1ec3 100644 --- a/Source/Applications/StreamHub/UDPSourceSession.h +++ b/Source/Applications/StreamHub/UDPSourceSession.h @@ -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(prevN) * + (1.0 + static_cast(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 diff --git a/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp b/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp index 2436d41..42c0359 100644 --- a/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp +++ b/Source/Components/DataSources/UDPStreamer/UDPStreamer.cpp @@ -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(scratchTimestamps), - reinterpret_cast(readyTimestamps), - fill * static_cast(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(scratchTimestamps), + reinterpret_cast(readyTimestamps), + fill * static_cast(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 */ diff --git a/Source/Components/DataSources/UDPStreamer/UDPStreamer.h b/Source/Components/DataSources/UDPStreamer/UDPStreamer.h index 9606f5b..ad78fda 100644 --- a/Source/Components/DataSources/UDPStreamer/UDPStreamer.h +++ b/Source/Components/DataSources/UDPStreamer/UDPStreamer.h @@ -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 */ diff --git a/Source/Components/Interfaces/UDPStream/UDPSClient.cpp b/Source/Components/Interfaces/UDPStream/UDPSClient.cpp index 8a5598c..e8eaf18 100644 --- a/Source/Components/Interfaces/UDPStream/UDPSClient.cpp +++ b/Source/Components/Interfaces/UDPStream/UDPSClient.cpp @@ -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(sizeof(recvBuf)); - bool ok; - if (useMulticast) { - ok = mcastSocket.Read(reinterpret_cast(recvBuf), recvSize); - } - else { - ok = recvSocket.Read(reinterpret_cast(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(sizeof(recvBuf)); + bool ok; + if (useMulticast) { + ok = mcastSocket.Read(reinterpret_cast(recvBuf), recvSize); + } + else { + ok = recvSocket.Read(reinterpret_cast(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(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(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(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(fragIdx) + 1u) == + static_cast(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(fragIdx) * chunkSize; + // Flush a deferred last fragment now that the chunk size is known. + if (s.pendingTailValid) { + uint32 tailOffset = (static_cast(s.totalFragments) - 1u) * s.chunkSize; + if ((tailOffset + s.pendingTailBytes) > static_cast(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(fragIdx) * s.chunkSize; if ((offset + payloadBytes) > static_cast(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(counter - lastDataCounter); + if (delta <= 0) { + staleDataPackets++; + return false; + } + lastDataGap = static_cast(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); } } } diff --git a/Source/Components/Interfaces/UDPStream/UDPSClient.h b/Source/Components/Interfaces/UDPStream/UDPSClient.h index 6393336..d50de2f 100644 --- a/Source/Components/Interfaces/UDPStream/UDPSClient.h +++ b/Source/Components/Interfaces/UDPStream/UDPSClient.h @@ -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]; diff --git a/Test/Applications/StreamHub/AccumDtGTest.cpp b/Test/Applications/StreamHub/AccumDtGTest.cpp new file mode 100644 index 0000000..935a406 --- /dev/null +++ b/Test/Applications/StreamHub/AccumDtGTest.cpp @@ -0,0 +1,162 @@ +/** + * @file AccumDtGTest.cpp + * @brief Tests UDPSEstimateAccumDt, the per-sample period estimator used for + * accumulated scalars that carry no SamplingRate. + * + * The estimator exists because the natural formula — sender-clock gap divided + * by the previous packet's sample count — is only correct while no packet is + * lost. When one is, the gap covers cycles that count never saw and the period + * comes out too large, which spreads the packet's samples past their real end + * and into the range the next packet claims. The loss count comes from the + * producer's packet counter rather than being inferred from the gap itself, so + * these tests pin both sides: the estimate must not move when packets go + * missing, and it must still follow a genuine rate change — a cycle count + * inferred from the estimate's own period would lock onto the old one. + * + * @copyright Copyright 2015 F4E | European Joint Undertaking for ITER and + * the Development of Fusion Energy ('Fusion for Energy'). + * Licensed under the EUPL, Version 1.1 or - as soon they will be approved + * by the European Commission - subsequent versions of the EUPL (the "Licence") + * You may not use this work except in compliance with the Licence. + * You may obtain a copy of the Licence at: http://ec.europa.eu/idabc/eupl + * + * @warning Unless required by applicable law or agreed to in writing, + * software distributed under the Licence is distributed on an "AS IS" + * basis, WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express + * or implied. See the Licence permissions and limitations under the Licence. + */ + +#include + +#include "UDPSourceSession.h" + +using MARTe::float64; +using MARTe::uint32; +using StreamHub::UDPSEstimateAccumDt; + +namespace { + +/** A producer emitting batches of BATCH cycles at a period of DT seconds. */ +const float64 kDt = 1.0e-3; +const uint32 kBatch = 10u; +const float64 kGap = kDt * static_cast(kBatch); + +/** Feeds n clean packets and returns the settled estimate. */ +float64 Warmup(uint32 n, float64 &dtEMA, bool &dtValid) { + float64 dt = 0.0; + for (uint32 i = 0u; i < n; i++) { + dt = UDPSEstimateAccumDt(kGap, kBatch, 0u, dtEMA, dtValid); + } + return dt; +} + +} // namespace + +/* The first packet has nothing to go on but the previous sample count, so it + * must fall back to gap/prevN rather than to some fixed default. */ +TEST(AccumDtGTest, BootstrapsFromPreviousSampleCount) { + float64 dtEMA = 0.0; + bool dtValid = false; + + const float64 dt = UDPSEstimateAccumDt(kGap, kBatch, 0u, dtEMA, dtValid); + + EXPECT_TRUE(dtValid); + EXPECT_NEAR(kDt, dt, 1.0e-12); +} + +/* A clean stream must hold the period steady, not drift. */ +TEST(AccumDtGTest, SteadyStreamStaysOnPeriod) { + float64 dtEMA = 0.0; + bool dtValid = false; + + const float64 dt = Warmup(50u, dtEMA, dtValid); + + EXPECT_NEAR(kDt, dt, 1.0e-9); +} + +/* The regression this whole estimator is for: one packet is lost, so the gap + * doubles while prevN does not. Dividing by prevN would report 2x the true + * period — enough to walk a 10-sample batch a full batch past its own end. */ +TEST(AccumDtGTest, LostPacketDoesNotInflatePeriod) { + float64 dtEMA = 0.0; + bool dtValid = false; + (void) Warmup(50u, dtEMA, dtValid); + + const float64 dt = UDPSEstimateAccumDt(2.0 * kGap, kBatch, 1u, dtEMA, dtValid); + + /* What the naive formula would have produced. */ + const float64 naive = (2.0 * kGap) / static_cast(kBatch); + EXPECT_NEAR(2.0 * kDt, naive, 1.0e-12); + + EXPECT_NEAR(kDt, dt, 1.0e-6); +} + +/* Several consecutive losses are the same situation, just wider. */ +TEST(AccumDtGTest, MultiplePacketLossDoesNotInflatePeriod) { + float64 dtEMA = 0.0; + bool dtValid = false; + (void) Warmup(50u, dtEMA, dtValid); + + for (uint32 missing = 1u; missing <= 5u; missing++) { + const float64 span = static_cast(missing + 1u) * kGap; + const float64 dt = UDPSEstimateAccumDt(span, kBatch, missing, dtEMA, + dtValid); + EXPECT_NEAR(kDt, dt, 1.0e-6) << "after " << missing << " lost packet(s)"; + } +} + +/* Loss must not leave the estimator poisoned for the packets that follow. */ +TEST(AccumDtGTest, RecoversToCleanStreamAfterLoss) { + float64 dtEMA = 0.0; + bool dtValid = false; + (void) Warmup(50u, dtEMA, dtValid); + (void) UDPSEstimateAccumDt(3.0 * kGap, kBatch, 2u, dtEMA, dtValid); + + const float64 dt = Warmup(20u, dtEMA, dtValid); + + EXPECT_NEAR(kDt, dt, 1.0e-6); +} + +/* A real, sustained rate change must still be followed — the estimator is a + * smoother, not a latch. Half the period is exactly on the rejection boundary, + * so use a change that lands inside the accepted band. */ +TEST(AccumDtGTest, FollowsSustainedRateChange) { + float64 dtEMA = 0.0; + bool dtValid = false; + (void) Warmup(50u, dtEMA, dtValid); + + const float64 newDt = kDt * 0.75; + const float64 newGap = newDt * static_cast(kBatch); + float64 dt = 0.0; + for (uint32 i = 0u; i < 400u; i++) { + dt = UDPSEstimateAccumDt(newGap, kBatch, 0u, dtEMA, dtValid); + } + + EXPECT_NEAR(newDt, dt, 1.0e-6); +} + +/* A batch that carries fewer cycles than usual (a time-triggered flush) is not + * loss: the gap shrinks with it, so the period must not shrink too. */ +TEST(AccumDtGTest, ShortBatchDoesNotDeflatePeriod) { + float64 dtEMA = 0.0; + bool dtValid = false; + (void) Warmup(50u, dtEMA, dtValid); + + const uint32 shortBatch = 3u; + const float64 dt = UDPSEstimateAccumDt( + kDt * static_cast(shortBatch), shortBatch, 0u, dtEMA, dtValid); + + EXPECT_NEAR(kDt, dt, 1.0e-6); +} + +/* A gap shorter than one period cannot mean zero cycles; the divisor is + * clamped so the estimate can never be driven to infinity. */ +TEST(AccumDtGTest, SubPeriodGapDoesNotExplode) { + float64 dtEMA = 0.0; + bool dtValid = false; + (void) Warmup(50u, dtEMA, dtValid); + + const float64 dt = UDPSEstimateAccumDt(kDt * 1.0e-3, 1u, 0u, dtEMA, dtValid); + + EXPECT_NEAR(kDt, dt, 1.0e-6); +} diff --git a/Test/Applications/StreamHub/Makefile.inc b/Test/Applications/StreamHub/Makefile.inc index 4afea34..b840f16 100644 --- a/Test/Applications/StreamHub/Makefile.inc +++ b/Test/Applications/StreamHub/Makefile.inc @@ -22,7 +22,7 @@ # ############################################################# -OBJSX = TriggerEngineSrc.x BinaryRecorderSrc.x SignalRingBufferGTest.x TriggerEngineGTest.x LTTBGTest.x BinaryRecorderGTest.x BoundsCheckTest.x WSServerBufferTest.x +OBJSX = TriggerEngineSrc.x BinaryRecorderSrc.x SignalRingBufferGTest.x TriggerEngineGTest.x LTTBGTest.x BinaryRecorderGTest.x BoundsCheckTest.x WSServerBufferTest.x AccumDtGTest.x PACKAGE=Applications ROOT_DIR=../../.. diff --git a/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp b/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp index 38ea2ee..578e503 100644 --- a/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp +++ b/Test/Components/DataSources/UDPStreamer/UDPStreamerGTest.cpp @@ -225,3 +225,8 @@ TEST(UDPStreamerGTest, TestExecute_MulticastConnectDataDisconnect) { UDPStreamerTest test; ASSERT_TRUE(test.TestExecute_MulticastConnectDataDisconnect()); } + +TEST(UDPStreamerGTest, TestAccumulate_EveryPublishedCycleReachesTheWire) { + UDPStreamerTest test; + ASSERT_TRUE(test.TestAccumulate_EveryPublishedCycleReachesTheWire()); +} diff --git a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp index 819ff35..461e41c 100644 --- a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp +++ b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.cpp @@ -41,6 +41,7 @@ #include "RealTimeApplication.h" #include "Sleep.h" #include "StandardParser.h" +#include "UDPSClient.h" #include "UDPStreamer.h" #include "UDPStreamerTest.h" @@ -1845,3 +1846,298 @@ bool UDPStreamerTest::TestExecute_MulticastConnectDataDisconnect() { ObjectRegistryDatabase::Instance()->Purge(); return ok; } + +/*---------------------------------------------------------------------------*/ +/* Accumulate publication continuity */ +/*---------------------------------------------------------------------------*/ + +/* Four float64 scalars, no quantisation: 32 wire bytes per RT cycle. + * With MaxPayloadSize = 60 the accumulate header (8 B HRT + 4 B count) leaves + * room for exactly one cycle, so the size condition flushes on every single + * Synchronise() — the maximum number of hand-offs to the sender thread, each + * one a chance for a promoted batch to be skipped. */ +#define ACC_FUNCTIONS_BLOCK \ + " +Functions = {\n" \ + " Class = ReferenceContainer\n" \ + " +Writer = {\n" \ + " Class = UDPStreamerTestOutputGAM\n" \ + " OutputSignals = {\n" \ + " A = {\n" \ + " DataSource = Streamer\n" \ + " Type = float64\n" \ + " }\n" \ + " B = {\n" \ + " DataSource = Streamer\n" \ + " Type = float64\n" \ + " }\n" \ + " C = {\n" \ + " DataSource = Streamer\n" \ + " Type = float64\n" \ + " }\n" \ + " D = {\n" \ + " DataSource = Streamer\n" \ + " Type = float64\n" \ + " }\n" \ + " }\n" \ + " }\n" \ + " }\n" + +static const MARTe::char8 *const ACC_CFG_CONTINUITY = + "+Test = {\n" + " Class = RealTimeApplication\n" + ACC_FUNCTIONS_BLOCK + " +Data = {\n" + " Class = ReferenceContainer\n" + " +Streamer = {\n" + " Class = UDPStreamer\n" + " Port = 44680\n" + " MaxPayloadSize = 60\n" + " PublishingMode = Accumulate\n" + " MinRefreshRate = 1000.0\n" + " Signals = {\n" + " A = {\n" + " Type = float64\n" + " }\n" + " B = {\n" + " Type = float64\n" + " }\n" + " C = {\n" + " Type = float64\n" + " }\n" + " D = {\n" + " Type = float64\n" + " }\n" + " }\n" + " }\n" + HF_TAIL_BLOCK; + +namespace { + +/** Cycles driven by TestAccumulate_EveryPublishedCycleReachesTheWire. */ +static const MARTe::uint32 ACC_CONTINUITY_CYCLES = 3000u; + +/** + * @brief Records which RT cycles reached the wire, and how often. + * + * The test stamps signal A with the cycle index before every Synchronise(), + * and the config is sized so each Accumulate batch carries exactly one cycle. + * The payload is [8 B HRT][4 B numSamples][A][B][C][D], so A of the single + * slot sits at offset 12 and identifies the cycle unambiguously. + * + * Counting distinct cycles (rather than summing numSamples) is what makes this + * able to tell a lost publication from a re-sent one: a sender that never + * consumes its ready buffer emits the right *number* of packets while + * repeating a stale batch, which shows up here as duplicates plus missing + * cycles instead of a clean tally. + */ +class AccumRampRecorder: public MARTe::UDPSClientListener { +public: + AccumRampRecorder() : + packets(0u), duplicates(0u), malformed(0u) { + mux.Create(); + for (MARTe::uint32 i = 0u; i < ACC_CONTINUITY_CYCLES; i++) { + seen[i] = false; + } + } + + virtual void OnUDPSData(const MARTe::uint8 *payload, MARTe::uint32 payloadSize) { + MARTe::uint32 n = 0u; + MARTe::float64 v = 0.0; + if (payloadSize >= 20u) { + (void) MARTe::MemoryOperationsHelper::Copy(&n, &payload[8], 4u); + (void) MARTe::MemoryOperationsHelper::Copy(&v, &payload[12], 8u); + } + (void) mux.FastLock(); + packets++; + if ((payloadSize < 20u) || (n != 1u)) { + malformed++; + } + else { + MARTe::uint32 idx = static_cast(v); + if ((static_cast(idx) != v) || (idx >= ACC_CONTINUITY_CYCLES)) { + malformed++; + } + else if (seen[idx]) { + duplicates++; + } + else { + seen[idx] = true; + } + } + mux.FastUnLock(); + } + + MARTe::uint32 DistinctCycles() { + (void) mux.FastLock(); + MARTe::uint32 n = 0u; + for (MARTe::uint32 i = 0u; i < ACC_CONTINUITY_CYCLES; i++) { + if (seen[i]) { + n++; + } + } + mux.FastUnLock(); + return n; + } + + MARTe::uint32 Packets() { + (void) mux.FastLock(); + MARTe::uint32 n = packets; + mux.FastUnLock(); + return n; + } + + MARTe::uint32 Duplicates() { + (void) mux.FastLock(); + MARTe::uint32 n = duplicates; + mux.FastUnLock(); + return n; + } + + MARTe::uint32 Malformed() { + (void) mux.FastLock(); + MARTe::uint32 n = malformed; + mux.FastUnLock(); + return n; + } + +private: + MARTe::FastPollingMutexSem mux; + bool seen[ACC_CONTINUITY_CYCLES]; + MARTe::uint32 packets; + MARTe::uint32 duplicates; + MARTe::uint32 malformed; +}; + +} // namespace + +bool UDPStreamerTest::TestAccumulate_EveryPublishedCycleReachesTheWire() { + using namespace MARTe; + + /* One-cycle batches every 200 us: ~5000 small packets/s, which the sender + * thread handles comfortably. The period has to be this short because a + * wake-up can only be swallowed while the sender is mid-send; at 1 ms the + * sender is always back in its wait before the next Synchronise() and the + * defect never fires at all. */ + const uint32 CYCLES = ACC_CONTINUITY_CYCLES; + static const float64 CYCLE_SEC = 200e-6; + + /* Tolerance, as a fraction of CYCLES, for cycles that never reach the wire. + * It is not zero: this is an ordinary userspace thread on a general-purpose + * kernel, so it can occasionally be descheduled past a 200 us slot, and the + * last batch may still be in the accumulation buffer when the loop ends. + * It is small because the defect this guards against is not marginal — a + * sender that decides what to send from the semaphore edge fails to consume + * essentially every batch (~100% here), so a 1% ceiling separates the two + * regimes with three orders of magnitude to spare. */ + const uint32 MAX_LOST = CYCLES / 100u; + + ReferenceT app = LoadApplication(ACC_CFG_CONTINUITY); + bool ok = app.IsValid(); + if (ok) { + ok = (app->PrepareNextState("State1") == ErrorManagement::NoError); + } + Sleep::MSec(50u); + + AccumRampRecorder counter; + UDPSClient client; + ReferenceT ds; + if (ok) { + ConfigurationDatabase clientCfg; + ok = clientCfg.Write("ServerAddr", "127.0.0.1"); + ok = ok && clientCfg.Write("Port", 44680u); + ok = ok && clientCfg.Write("SilenceTimeout", 0.0f); + ok = ok && clientCfg.Write("KeepAliveInterval", 0u); + client.SetListener(&counter); + ok = ok && client.Initialise(clientCfg); + ok = ok && client.Start(); + } + + /* Wait for the CONNECT to register on the streamer side. */ + if (ok) { + ds = ObjectRegistryDatabase::Instance()->Find("Test.Data.Streamer"); + ok = ds.IsValid(); + } + if (ok) { + uint32 waited = 0u; + while ((waited < 3000u) && !ds->IsClientConnected()) { + Sleep::MSec(20u); + waited += 20u; + } + ok = ds->IsClientConnected(); + } + + /* Signal A carries the cycle index, so every packet identifies exactly + * which RT cycle produced it. Synchronise() snapshots the DataSource + * memory, so writing straight into it is equivalent to a GAM having + * produced the value. */ + float64 *sigA = NULL_PTR(float64 *); + if (ok) { + void *addr = NULL_PTR(void *); + ok = ds->GetSignalMemoryBuffer(0u, 0u, addr); + sigA = reinterpret_cast(addr); + ok = ok && (sigA != NULL_PTR(float64 *)); + } + + /* Drive the RT cycles. */ + if (ok) { + for (uint32 i = 0u; (i < CYCLES) && ok; i++) { + *sigA = static_cast(i); + ok = ds->Synchronise(); + Sleep::Sec(CYCLE_SEC); + } + } + + /* Let the last packets drain. */ + Sleep::MSec(300u); + + uint32 distinct = counter.DistinctCycles(); + uint32 packets = counter.Packets(); + uint32 duplicates = counter.Duplicates(); + uint32 malformed = counter.Malformed(); + uint32 dropped = (ds.IsValid()) ? ds->GetDroppedPublications() : 0u; + + if (ok) { + ok = (malformed == 0u); + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "%u of %u DATA packets did not carry exactly one " + "decodable cycle index.", malformed, packets); + } + } + if (ok) { + /* A cycle that never arrives is a hole in the consumer's time series. */ + ok = (distinct + MAX_LOST) >= CYCLES; + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "Accumulate lost cycles: %u of %u reached the wire " + "in %u packets (%u duplicates, %u publications " + "overwritten before being sent).", + distinct, CYCLES, packets, duplicates, dropped); + } + } + if (ok) { + /* A cycle that arrives twice means the sender re-sent a ready buffer it + * had already transmitted, which lands the same samples on the receiver + * under two different time bases. */ + ok = (duplicates == 0u); + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "%u of %u DATA packets repeated a cycle already sent.", + duplicates, packets); + } + } + if (ok) { + /* Same ceiling from the producer's side: it sees the overwrite directly + * and does not depend on the packet reaching the loopback socket. */ + ok = (dropped <= MAX_LOST); + if (!ok) { + REPORT_ERROR_STATIC(ErrorManagement::FatalError, + "%u of %u publications were overwritten before the " + "sender thread took them.", dropped, CYCLES); + } + } + + (void) client.Stop(); + ObjectRegistryDatabase::Instance()->Purge(); + return ok; +} diff --git a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h index 6d64f97..5ce014b 100644 --- a/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h +++ b/Test/Components/DataSources/UDPStreamer/UDPStreamerTest.h @@ -234,6 +234,16 @@ public: * @brief Tests full TCP CONNECT → CONFIG → DATA via multicast → DISCONNECT on loopback. */ bool TestExecute_MulticastConnectDataDisconnect(); + + /** + * @brief Tests that Accumulate publishes every RT cycle it batches. + * @details Drives 600 cycles at a rate the sender thread trivially keeps up + * with, and sums the numSamples field of every DATA packet that arrives. + * A batch promoted to the ready buffer but never sent — because the wake-up + * announcing it was swallowed — shows up here as missing cycles, which a + * consumer sees as a hole in the time series. + */ + bool TestAccumulate_EveryPublishedCycleReachesTheWire(); }; #endif /* UDPSTREAMERTEST_H_ */ diff --git a/Test/GTest/UDPSClientGTest.cpp b/Test/GTest/UDPSClientGTest.cpp index a865cd6..9257a29 100644 --- a/Test/GTest/UDPSClientGTest.cpp +++ b/Test/GTest/UDPSClientGTest.cpp @@ -41,6 +41,7 @@ /*---------------------------------------------------------------------------*/ #include "BasicUDPSocket.h" #include "ConfigurationDatabase.h" +#include "FastPollingMutexSem.h" #include "InternetHost.h" #include "Sleep.h" #include "UDPSClient.h" @@ -131,6 +132,147 @@ bool WaitForClient(UDPSServer &server, uint32 timeoutMs) { return false; } +/*---------------------------------------------------------------------------*/ +/* Fragment-reassembly test harness */ +/*---------------------------------------------------------------------------*/ + +/** Largest reassembled payload the recording listener keeps a copy of. */ +const uint32 kMaxRecordedBytes = 8192u; +/** How many reassembled payloads the recording listener keeps. */ +const uint32 kMaxRecorded = 16u; + +/** + * @brief Listener that records every reassembled DATA/CONFIG payload. + * + * Callbacks run on the UDPSClient receive thread; the test thread reads the + * records after a settle sleep, so both sides take the same lock. + */ +class RecordingListener: public UDPSClientListener { +public: + RecordingListener() : + dataCount(0u), configCount(0u) { + mux.Create(); + } + + virtual void OnUDPSData(const uint8 *payload, uint32 payloadSize) { + Record(dataPayloads, dataSizes, dataCount, payload, payloadSize); + } + + virtual void OnUDPSConfig(const uint8 *payload, uint32 payloadSize) { + Record(configPayloads, configSizes, configCount, payload, payloadSize); + } + + uint32 DataCount() { + (void) mux.FastLock(); + uint32 n = dataCount; + mux.FastUnLock(); + return n; + } + + uint32 ConfigCount() { + (void) mux.FastLock(); + uint32 n = configCount; + mux.FastUnLock(); + return n; + } + + /** @return true iff record @p idx matches @p expected byte for byte. */ + bool DataMatches(uint32 idx, const uint8 *expected, uint32 expectedSize) { + return Matches(dataPayloads, dataSizes, dataCount, idx, expected, + expectedSize); + } + + bool ConfigMatches(uint32 idx, const uint8 *expected, uint32 expectedSize) { + return Matches(configPayloads, configSizes, configCount, idx, expected, + expectedSize); + } + + uint32 DataSize(uint32 idx) { + (void) mux.FastLock(); + uint32 n = (idx < dataCount) ? dataSizes[idx] : 0u; + mux.FastUnLock(); + return n; + } + +private: + void Record(uint8 (&dst)[kMaxRecorded][kMaxRecordedBytes], + uint32 (&sizes)[kMaxRecorded], uint32 &count, + const uint8 *payload, uint32 payloadSize) { + (void) mux.FastLock(); + if (count < kMaxRecorded) { + sizes[count] = payloadSize; + uint32 n = (payloadSize < kMaxRecordedBytes) ? payloadSize + : kMaxRecordedBytes; + memcpy(dst[count], payload, n); + count++; + } + mux.FastUnLock(); + } + + bool Matches(uint8 (&src)[kMaxRecorded][kMaxRecordedBytes], + uint32 (&sizes)[kMaxRecorded], uint32 &count, uint32 idx, + const uint8 *expected, uint32 expectedSize) { + (void) mux.FastLock(); + bool ok = (idx < count) && (sizes[idx] == expectedSize) && + (expectedSize <= kMaxRecordedBytes) && + (memcmp(src[idx], expected, expectedSize) == 0); + mux.FastUnLock(); + return ok; + } + + FastPollingMutexSem mux; + uint8 dataPayloads[kMaxRecorded][kMaxRecordedBytes]; + uint32 dataSizes[kMaxRecorded]; + uint32 dataCount; + uint8 configPayloads[kMaxRecorded][kMaxRecordedBytes]; + uint32 configSizes[kMaxRecorded]; + uint32 configCount; +}; + +/** Fill @p buf with a position-dependent pattern so misplacement is visible. */ +void FillPattern(uint8 *buf, uint32 n, uint8 seed) { + for (uint32 i = 0u; i < n; i++) { + buf[i] = static_cast((i * 7u) + seed); + } +} + +/** Send one UDPS fragment datagram to 127.0.0.1:@p dstPort. */ +bool SendFragment(BasicUDPSocket &sock, uint16 dstPort, uint8 type, + uint32 counter, uint16 fragIdx, uint16 totalFrags, + const uint8 *payload, uint32 payloadBytes) { + uint8 buf[UDPS_HEADER_SIZE + 2048u]; + if (payloadBytes > 2048u) { + return false; + } + UDPSBuildHeader(buf, type, counter, fragIdx, totalFrags, payloadBytes); + memcpy(&buf[UDPS_HEADER_SIZE], payload, payloadBytes); + InternetHost dst(dstPort, "127.0.0.1"); + (void) sock.SetDestination(dst); + uint32 n = UDPS_HEADER_SIZE + payloadBytes; + return sock.Write(reinterpret_cast(buf), n); +} + +/** + * @brief Bring up a UDPSClient pointed at @p server and learn the ephemeral + * port it receives DATA on (the source port of its CONNECT). + * + * Silence timeout and keepalive are disabled so the session never churns + * underneath the fragments the test injects. + */ +bool StartClientAndLearnPort(UDPSClient &client, ConfigurationDatabase &cfg, + BasicUDPSocket &server, uint16 serverPort, + uint16 &clientPort) { + if (!cfg.Write("ServerAddr", "127.0.0.1")) { return false; } + if (!cfg.Write("Port", static_cast(serverPort))) { return false; } + if (!cfg.Write("SilenceTimeout", 0.0f)) { return false; } + if (!cfg.Write("KeepAliveInterval", 0u)) { return false; } + if (!client.Initialise(cfg)) { return false; } + if (!client.Start()) { return false; } + uint8 type = 0xFFu; + if (!WaitDatagram(server, 3000, type, clientPort)) { return false; } + return (type == UDPS_TYPE_CONNECT) && (clientPort != 0u); +} + } // namespace /*---------------------------------------------------------------------------*/ @@ -346,3 +488,277 @@ TEST(UDPSClientGTest, TestSilenceTimeoutSubSecondTriggersReconnect) { client.Stop(); server.Close(); } + +TEST(UDPSClientGTest, TestReorderedFragmentsAreReassembled) { + /* UDP gives no ordering guarantee: the fragments of one packet may arrive + * in any order, with nothing lost. Reassembly must not depend on fragment + * 0 arriving first — if it does, an out-of-order burst destroys a packet + * whose bytes all arrived, and leaves a slot occupied until the 2 s GC, + * which is how four slots end up permanently full. */ + BasicUDPSocket server; + ASSERT_TRUE(server.Open()); + ASSERT_TRUE(server.Listen(0u)); + uint16 serverPort = GetBoundPort(server); + ASSERT_NE(serverPort, 0u); + + RecordingListener listener; + UDPSClient client; + client.SetListener(&listener); + ConfigurationDatabase cfg; + uint16 clientPort = 0u; + ASSERT_TRUE(StartClientAndLearnPort(client, cfg, server, serverPort, + clientPort)); + + /* 20-byte payload over three 8-byte chunks: the last one is short, which + * is exactly why chunk size has to be learnt from a non-last fragment. */ + uint8 expected[20]; + FillPattern(expected, sizeof(expected), 3u); + + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 7u, 1u, 3u, + &expected[8], 8u)); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 7u, 2u, 3u, + &expected[16], 4u)); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 7u, 0u, 3u, + &expected[0], 8u)); + + Sleep::MSec(400u); + + ASSERT_EQ(listener.DataCount(), 1u) + << "no fragment was lost, yet the packet was not delivered"; + EXPECT_EQ(listener.DataSize(0u), 20u); + EXPECT_TRUE(listener.DataMatches(0u, expected, sizeof(expected))); + + client.Stop(); + server.Close(); +} + +TEST(UDPSClientGTest, TestDataAndConfigWithSameCounterDoNotCollide) { + /* DATA and CONFIG carry independent counter sequences, so the same counter + * value legitimately appears on both. A reassembly slot keyed on the + * counter alone merges the two streams: one payload is delivered under the + * wrong type and the other is silently dropped. */ + BasicUDPSocket server; + ASSERT_TRUE(server.Open()); + ASSERT_TRUE(server.Listen(0u)); + uint16 serverPort = GetBoundPort(server); + ASSERT_NE(serverPort, 0u); + + RecordingListener listener; + UDPSClient client; + client.SetListener(&listener); + ConfigurationDatabase cfg; + uint16 clientPort = 0u; + ASSERT_TRUE(StartClientAndLearnPort(client, cfg, server, serverPort, + clientPort)); + + uint8 dataPayload[16]; + uint8 cfgPayload[16]; + FillPattern(dataPayload, sizeof(dataPayload), 11u); + FillPattern(cfgPayload, sizeof(cfgPayload), 200u); + + /* Same counter (42), interleaved, two fragments each. */ + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_CONFIG, 42u, 0u, 2u, + &cfgPayload[0], 8u)); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 42u, 0u, 2u, + &dataPayload[0], 8u)); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_CONFIG, 42u, 1u, 2u, + &cfgPayload[8], 8u)); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 42u, 1u, 2u, + &dataPayload[8], 8u)); + + Sleep::MSec(400u); + + EXPECT_EQ(listener.ConfigCount(), 1u); + EXPECT_TRUE(listener.ConfigMatches(0u, cfgPayload, sizeof(cfgPayload))); + ASSERT_EQ(listener.DataCount(), 1u) + << "the DATA packet was swallowed by the CONFIG slot sharing its counter"; + EXPECT_TRUE(listener.DataMatches(0u, dataPayload, sizeof(dataPayload))); + + client.Stop(); + server.Close(); +} + +TEST(UDPSClientGTest, TestDuplicateHighIndexFragmentDoesNotFakeCompletion) { + /* Completion is decided by counting fragments, with a received-bitmask to + * reject duplicates. If the mask is narrower than the fragment count the + * client accepts, a duplicated high-index fragment is counted twice and + * the packet is delivered while a fragment is still missing — a payload + * with a hole of stale bytes, reported as valid. */ + BasicUDPSocket server; + ASSERT_TRUE(server.Open()); + ASSERT_TRUE(server.Listen(0u)); + uint16 serverPort = GetBoundPort(server); + ASSERT_NE(serverPort, 0u); + + RecordingListener listener; + UDPSClient client; + client.SetListener(&listener); + ConfigurationDatabase cfg; + uint16 clientPort = 0u; + ASSERT_TRUE(StartClientAndLearnPort(client, cfg, server, serverPort, + clientPort)); + + /* 300 fragments — past the 256 a 32-byte mask covers, but well inside the + * 512 the client's own sanity check permits. */ + const uint16 kTotalFrags = 300u; + const uint32 kChunk = 8u; + const uint32 kLastChunk = 4u; + const uint32 kTotalBytes = ((kTotalFrags - 1u) * kChunk) + kLastChunk; + uint8 expected[((kTotalFrags - 1u) * kChunk) + kLastChunk]; + FillPattern(expected, kTotalBytes, 5u); + + /* Everything except the final fragment, plus one duplicate above 255. */ + for (uint16 f = 0u; f < (kTotalFrags - 1u); f++) { + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 9u, f, + kTotalFrags, &expected[f * kChunk], kChunk)); + } + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 9u, 260u, + kTotalFrags, &expected[260u * kChunk], kChunk)); + + Sleep::MSec(500u); + + ASSERT_EQ(listener.DataCount(), 0u) + << "delivered with a fragment still missing (a duplicate was counted " + "as a new fragment)"; + + /* The genuinely missing fragment completes it, with the right bytes. */ + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 9u, + kTotalFrags - 1u, kTotalFrags, + &expected[(kTotalFrags - 1u) * kChunk], + kLastChunk)); + Sleep::MSec(400u); + + ASSERT_EQ(listener.DataCount(), 1u); + EXPECT_EQ(listener.DataSize(0u), kTotalBytes); + EXPECT_TRUE(listener.DataMatches(0u, expected, kTotalBytes)); + + client.Stop(); + server.Close(); +} + +TEST(UDPSClientGTest, TestStaleDataPacketIsNotDelivered) { + /* A DATA packet that arrives after a newer one has already been delivered + * carries an older time base. Delivering it makes the consumer place its + * samples behind the ones it has: they collide with what is already + * plotted, and the range they should have occupied stays empty. The + * counter is the only thing that tells the two apart, so the client must + * drop anything that does not advance it. */ + BasicUDPSocket server; + ASSERT_TRUE(server.Open()); + ASSERT_TRUE(server.Listen(0u)); + uint16 serverPort = GetBoundPort(server); + ASSERT_NE(serverPort, 0u); + + RecordingListener listener; + UDPSClient client; + client.SetListener(&listener); + ConfigurationDatabase cfg; + uint16 clientPort = 0u; + ASSERT_TRUE(StartClientAndLearnPort(client, cfg, server, serverPort, + clientPort)); + + uint8 pkt[8]; + FillPattern(pkt, sizeof(pkt), 1u); + + /* 10 and 11 advance the counter; 9 and the repeat of 11 do not. */ + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 10u, 0u, 1u, + pkt, sizeof(pkt))); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 11u, 0u, 1u, + pkt, sizeof(pkt))); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 9u, 0u, 1u, + pkt, sizeof(pkt))); + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 11u, 0u, 1u, + pkt, sizeof(pkt))); + + Sleep::MSec(400u); + + EXPECT_EQ(listener.DataCount(), 2u) + << "a packet older than one already delivered reached the listener"; + EXPECT_EQ(client.GetStaleDataPackets(), 2u); + + client.Stop(); + server.Close(); +} + +TEST(UDPSClientGTest, TestCounterGapIsReported) { + /* Consumers that infer a sample period from the sender-clock gap need to + * know how many packets that gap spans; without it a single loss reads as + * a halved rate. The gap comes from the counter, and must exclude the + * packet being delivered. */ + BasicUDPSocket server; + ASSERT_TRUE(server.Open()); + ASSERT_TRUE(server.Listen(0u)); + uint16 serverPort = GetBoundPort(server); + ASSERT_NE(serverPort, 0u); + + RecordingListener listener; + UDPSClient client; + client.SetListener(&listener); + ConfigurationDatabase cfg; + uint16 clientPort = 0u; + ASSERT_TRUE(StartClientAndLearnPort(client, cfg, server, serverPort, + clientPort)); + + uint8 pkt[8]; + FillPattern(pkt, sizeof(pkt), 2u); + + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 100u, 0u, 1u, + pkt, sizeof(pkt))); + Sleep::MSec(200u); + EXPECT_EQ(client.GetLastDataGap(), 0u) << "the first packet lost nothing"; + + /* 101, 102 and 103 never arrive. */ + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 104u, 0u, 1u, + pkt, sizeof(pkt))); + Sleep::MSec(200u); + EXPECT_EQ(client.GetLastDataGap(), 3u); + + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, 105u, 0u, 1u, + pkt, sizeof(pkt))); + Sleep::MSec(200u); + EXPECT_EQ(client.GetLastDataGap(), 0u) << "the gap must not persist"; + + EXPECT_EQ(listener.DataCount(), 3u); + EXPECT_EQ(client.GetStaleDataPackets(), 0u); + + client.Stop(); + server.Close(); +} + +TEST(UDPSClientGTest, TestCounterWraparoundDoesNotRejectStream) { + /* The counter is a uint32 that wraps. Ordering it by plain comparison + * would call every packet after the wrap older than 0xFFFFFFFF and reject + * the stream permanently, so the ordering has to be done on the signed + * difference. */ + BasicUDPSocket server; + ASSERT_TRUE(server.Open()); + ASSERT_TRUE(server.Listen(0u)); + uint16 serverPort = GetBoundPort(server); + ASSERT_NE(serverPort, 0u); + + RecordingListener listener; + UDPSClient client; + client.SetListener(&listener); + ConfigurationDatabase cfg; + uint16 clientPort = 0u; + ASSERT_TRUE(StartClientAndLearnPort(client, cfg, server, serverPort, + clientPort)); + + uint8 pkt[8]; + FillPattern(pkt, sizeof(pkt), 4u); + + const uint32 counters[4] = { 0xFFFFFFFEu, 0xFFFFFFFFu, 0u, 1u }; + for (uint32 i = 0u; i < 4u; i++) { + ASSERT_TRUE(SendFragment(server, clientPort, UDPS_TYPE_DATA, + counters[i], 0u, 1u, pkt, sizeof(pkt))); + Sleep::MSec(150u); + } + + EXPECT_EQ(listener.DataCount(), 4u) + << "the stream was rejected across the counter wrap"; + EXPECT_EQ(client.GetStaleDataPackets(), 0u); + EXPECT_EQ(client.GetLastDataGap(), 0u); + + client.Stop(); + server.Close(); +}