fix(udps): stop packets being dated from an earlier time base
Reported as samples sporadically carrying a previous packet's timestamp: holes on one side of the stream and collisions on the other, in both the Go and the MARTe2 receiver. That it appeared in both is what located it -- the shared cause is upstream of either client. Four independent defects, all of which end in a packet's values being placed at a time that is not theirs. Reassembly slot exhaustion (the "Reassembly slots full; evicting oldest" flood). Chunk size was learnt only from fragment 0, so an out-of-order burst destroyed a packet whose bytes had all arrived and left the slot occupied until the 2 s GC. Slots were keyed on the counter alone, but DATA and CONFIG number independently, so equal counters merged the two streams. The 32-byte received-mask covered 256 of the 512 fragments the client accepts, so a duplicate above 255 was counted as new and the packet was delivered with a hole of stale bytes in it. And one datagram was read per Execute(), which cannot drain a fast producer. Fixed with a pendingTail deferral, (counter, type) keying, a 64-byte mask, a 256-datagram drain, counter-age slot reclamation, and a 1 Hz aggregated warning in place of the per-eviction flood. UDPStreamer dropping whole Accumulate batches. EventSem::ResetWait is Reset-then-Wait, so a Post() landing while the sender thread was inside ServiceClients()/SendData() was destroyed by the next Reset. The batch was then skipped with dataReady false, readyFill was never cleared, and the following flush overwrote it: an entire run of RT cycles never reached the wire. The record of pending work now lives in the buffers rather than in the semaphore edge, which also removes up to UDPS_DATA_WAIT_MS of latency; genuine backpressure overwrites are counted and reported. Against the unfixed code the new test sees 2999/3000 batches never consumed. Period inflation after loss. Accumulated scalars carry no SamplingRate, so the receiver derives dt from the sender-clock gap -- but dividing it by the previous packet's sample count is only right while nothing is lost. One loss doubles the reported period, which spreads a batch a full batch past its own end and into the range the next packet claims. That is the hole and the collision, exactly. Inferring the cycle count from the estimate's own period is not a way out: it has a stable fixed point wherever gap/dt is an integer, so a real rate change locks it at the old one for good (AccumDtGTest.FollowsSustainedRateChange). The packet counter removes the ambiguity, so all three receivers now order on it: a DATA packet that does not advance the counter is dropped rather than delivered, because its values are older than data already handed over. Ordering is on the signed difference so it survives the uint32 wrap, and the sequence resets on reconnect, where the producer's counter restarts independently of ours. The loss count that falls out of the same delta feeds the period estimate as cycles = prevN * (1 + lost), which reduces exactly to gap/prevN when nothing is lost and therefore still tracks a genuine rate change. UDPSClient::AcceptDataCounter (C++), udpsprotocol.SequenceGate (Go), decode_data (C). The C client's existing gap counter was wrap-unsafe and let a stale packet rewind last_counter, which made every subsequent gap wrong; it uses the same code now. Docs/Protocol.md gains an Ordering DATA section stating the requirement for any receiver, including ones outside this repository. Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
Martino Ferrari
co-authored by
Claude Opus 4.6
Claude Opus 4.6
parent
13fac79400
commit
deabd257e5
@@ -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;
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -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
|
||||
}
|
||||
@@ -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}
|
||||
}
|
||||
|
||||
@@ -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
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
}
|
||||
@@ -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;
|
||||
|
||||
@@ -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())
|
||||
}
|
||||
}
|
||||
@@ -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)
|
||||
}
|
||||
|
||||
@@ -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
|
||||
|
||||
@@ -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. |
|
||||
|
||||
|
||||
@@ -99,6 +99,8 @@ void UDPSourceSession::ResetCalibration() {
|
||||
lastPktWallValid_[i] = false;
|
||||
lastPktWallS_[i] = 0.0;
|
||||
accScalarPrevN_[i] = 0u;
|
||||
accScalarDtValid_[i] = false;
|
||||
accScalarDtEMA_[i] = 0.0;
|
||||
}
|
||||
}
|
||||
|
||||
@@ -197,7 +199,8 @@ void UDPSourceSession::OnUDPSConfig(const uint8 *payload, uint32 payloadSize) {
|
||||
}
|
||||
|
||||
void UDPSourceSession::OnUDPSData(const uint8 *payload, uint32 payloadSize) {
|
||||
ParseDataPayload(payload, payloadSize);
|
||||
/* Valid only for the duration of this callback. */
|
||||
ParseDataPayload(payload, payloadSize, client_.GetLastDataGap());
|
||||
}
|
||||
|
||||
/*---------------------------------------------------------------------------*/
|
||||
@@ -376,7 +379,8 @@ float64 UDPSourceSession::ProducerNewestTime() const {
|
||||
/* DATA parsing */
|
||||
/*---------------------------------------------------------------------------*/
|
||||
|
||||
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size,
|
||||
uint32 lostPackets) {
|
||||
if (size < 8u) { return; }
|
||||
|
||||
/* Copy metadata under lock */
|
||||
@@ -581,16 +585,25 @@ void UDPSourceSession::ParseDataPayload(const uint8 *payload, uint32 size) {
|
||||
}
|
||||
|
||||
/* Per-sample dt: samplingRate if present, else derive it from
|
||||
* the sender-HRT gap to the previous packet divided by that
|
||||
* packet's sample count (the flushes carry contiguous RT
|
||||
* cycles, so this is exactly one cycle period). */
|
||||
* the sender-HRT gap to the previous packet.
|
||||
*
|
||||
* The gap is divided by the number of RT cycles it actually
|
||||
* spans, not by the previous packet's sample count. Those two
|
||||
* agree only while nothing is lost; once a packet goes missing
|
||||
* the gap covers cycles the previous count never saw, and
|
||||
* dividing by that count inflates dt until this packet's
|
||||
* samples overrun into the next packet's range. lostPackets
|
||||
* comes from the producer's packet counter, so the divisor
|
||||
* widens with the gap and dt is unchanged. */
|
||||
float64 dt;
|
||||
if (desc.samplingRate > 0.0) {
|
||||
dt = 1.0 / desc.samplingRate;
|
||||
} else if (lastPktWallValid_[s] && (accScalarPrevN_[s] > 0u) &&
|
||||
(hrt0Sec > lastPktWallS_[s])) {
|
||||
dt = (hrt0Sec - lastPktWallS_[s]) /
|
||||
static_cast<float64>(accScalarPrevN_[s]);
|
||||
dt = UDPSEstimateAccumDt(hrt0Sec - lastPktWallS_[s],
|
||||
accScalarPrevN_[s], lostPackets,
|
||||
accScalarDtEMA_[s],
|
||||
accScalarDtValid_[s]);
|
||||
} else {
|
||||
dt = 1.0e-3; /* 1 kHz default until the gap is known */
|
||||
}
|
||||
|
||||
@@ -39,6 +39,69 @@ using MARTe::ConfigurationDatabase;
|
||||
/** Maximum number of signals per source session. */
|
||||
static const uint32 UDPSS_MAX_SIGNALS = 256u;
|
||||
|
||||
/* Accumulated-scalar dt estimator tuning. */
|
||||
/** Weight of a new observation; slow enough that one bad gap barely moves it. */
|
||||
static const float64 UDPSS_DT_EMA_ALPHA = 0.05;
|
||||
/** Observations outside [lo, hi] x the current estimate are treated as a
|
||||
* mis-counted gap and discarded rather than smoothed in. */
|
||||
static const float64 UDPSS_DT_ACCEPT_LO = 0.5;
|
||||
static const float64 UDPSS_DT_ACCEPT_HI = 2.0;
|
||||
|
||||
/**
|
||||
* @brief Per-sample period of an accumulated scalar packet, robust to loss.
|
||||
*
|
||||
* An Accumulate producer batches consecutive RT cycles, so the sender-clock
|
||||
* gap between two packets' first samples covers exactly as many cycles as the
|
||||
* earlier packet carried — but only while nothing is lost in between. Over UDP
|
||||
* (and with a producer that can overwrite a batch the sender never took) that
|
||||
* assumption fails, and dividing the gap by the previous packet's sample count
|
||||
* then inflates the period. The packet's own samples are laid out as
|
||||
* base + e*dt, so an inflated dt walks them past their real end and into the
|
||||
* span the next packet will claim: samples collide there and leave a hole
|
||||
* behind them.
|
||||
*
|
||||
* The number of packets that went missing is not guessed from the gap — that
|
||||
* is circular, and an estimator that infers the cycle count from its own
|
||||
* period has a stable fixed point wherever gap/dt is an integer, so a genuine
|
||||
* rate change locks it at the old period forever. It comes instead from the
|
||||
* UDPS packet counter, which the producer increments once per sent packet. The
|
||||
* gap then spans (1 + lost) batches, each assumed to be prevN cycles, and with
|
||||
* nothing lost the formula reduces exactly to gap/prevN.
|
||||
*
|
||||
* @param gap Sender-clock seconds since the previous packet's first sample.
|
||||
* Must be > 0.
|
||||
* @param prevN Samples in the previous packet. Must be > 0.
|
||||
* @param lost Packets missing between the previous packet and this one,
|
||||
* from the producer's counter.
|
||||
* @param[in,out] dtEMA Smoothed period. Seeded on the first call.
|
||||
* @param[in,out] dtValid False until dtEMA holds an estimate.
|
||||
* @return The period to space this packet's samples by.
|
||||
*/
|
||||
inline float64 UDPSEstimateAccumDt(const float64 gap, const uint32 prevN,
|
||||
const uint32 lost, float64 &dtEMA,
|
||||
bool &dtValid) {
|
||||
float64 cycles = static_cast<float64>(prevN) *
|
||||
(1.0 + static_cast<float64>(lost));
|
||||
if (cycles < 1.0) {
|
||||
cycles = 1.0;
|
||||
}
|
||||
const float64 dtObs = gap / cycles;
|
||||
|
||||
if (!dtValid) {
|
||||
dtEMA = dtObs;
|
||||
dtValid = true;
|
||||
} else if ((dtObs > (dtEMA * UDPSS_DT_ACCEPT_LO)) &&
|
||||
(dtObs < (dtEMA * UDPSS_DT_ACCEPT_HI))) {
|
||||
/* Track slow drift, but ignore observations far outside the current
|
||||
* estimate: those are the signature of a mis-counted gap, and folding
|
||||
* one in would drag the estimate towards the very error it exists to
|
||||
* absorb. */
|
||||
dtEMA = ((1.0 - UDPSS_DT_EMA_ALPHA) * dtEMA) +
|
||||
(UDPSS_DT_EMA_ALPHA * dtObs);
|
||||
}
|
||||
return dtEMA;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief One connected UDPStreamer source.
|
||||
*
|
||||
@@ -229,7 +292,13 @@ private:
|
||||
|
||||
/* DATA payload parsing */
|
||||
void ParseConfigPayload(const uint8 *payload, uint32 size);
|
||||
void ParseDataPayload(const uint8 *payload, uint32 size);
|
||||
/**
|
||||
* @param lostPackets DATA packets missing immediately before this one, from
|
||||
* the producer's counter; the accumulated-scalar period estimate
|
||||
* needs it to know how many cycles the sender-clock gap spans.
|
||||
*/
|
||||
void ParseDataPayload(const uint8 *payload, uint32 size,
|
||||
uint32 lostPackets);
|
||||
void AllocateRingBuffers();
|
||||
|
||||
/** @brief Invalidate all wall-clock calibration state (receive thread only). */
|
||||
@@ -401,6 +470,12 @@ private:
|
||||
float64 hrtFreq_;
|
||||
uint32 accScalarPrevN_[UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Per-signal state of UDPSEstimateAccumDt (see above): the smoothed
|
||||
* per-sample period for accumulated scalars whose descriptor carries no
|
||||
* SamplingRate. */
|
||||
float64 accScalarDtEMA_[UDPSS_MAX_SIGNALS];
|
||||
bool accScalarDtValid_[UDPSS_MAX_SIGNALS];
|
||||
|
||||
/* Scratch buffers for decoding arrays (receive thread only). */
|
||||
float64 *timeScratch_; ///< Time values scratch
|
||||
float64 *valScratch_; ///< Data values scratch
|
||||
|
||||
@@ -107,6 +107,9 @@ UDPStreamer::UDPStreamer()
|
||||
readyTimestamps = NULL_PTR(uint64 *);
|
||||
scratchTimestamps = NULL_PTR(uint64 *);
|
||||
readyFill = 0u;
|
||||
readySnapshotPending = false;
|
||||
droppedPublications = 0u;
|
||||
lastDropReportTicks = 0u;
|
||||
decimateRatio = 1u;
|
||||
decimateCounter = 0u;
|
||||
|
||||
@@ -871,6 +874,11 @@ bool UDPStreamer::Synchronise() {
|
||||
/* HI-3: if accumFill reached maxBatchCount, force-flush before writing */
|
||||
if (accumFill >= maxBatchCount) {
|
||||
uint32 filled = accumFill;
|
||||
if (readyFill > 0u) {
|
||||
/* The sender has not taken the previous batch: it is about to be
|
||||
* overwritten and its cycles will never reach any receiver. */
|
||||
droppedPublications++;
|
||||
}
|
||||
(void)MemoryOperationsHelper::Copy(readyBuffer, accumBuffer,
|
||||
filled * totalSrcBytes);
|
||||
(void)MemoryOperationsHelper::Copy(
|
||||
@@ -901,6 +909,9 @@ bool UDPStreamer::Synchronise() {
|
||||
|
||||
if (sizeCondition || timeCondition) {
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
if (readyFill > 0u) {
|
||||
droppedPublications++;
|
||||
}
|
||||
(void)MemoryOperationsHelper::Copy(readyBuffer, accumBuffer,
|
||||
filled * totalSrcBytes);
|
||||
(void)MemoryOperationsHelper::Copy(
|
||||
@@ -922,16 +933,24 @@ bool UDPStreamer::Synchronise() {
|
||||
if (decimateCounter >= decimateRatio) {
|
||||
decimateCounter = 0u;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
if (readySnapshotPending) {
|
||||
droppedPublications++;
|
||||
}
|
||||
(void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
|
||||
syncTimestamp = ts;
|
||||
readySnapshotPending = true;
|
||||
bufMutex.FastUnLock();
|
||||
(void)dataSem.Post();
|
||||
}
|
||||
} else {
|
||||
/* --- Strict path: post every call --- */
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
if (readySnapshotPending) {
|
||||
droppedPublications++;
|
||||
}
|
||||
(void)MemoryOperationsHelper::Copy(readyBuffer, memory, totalSrcBytes);
|
||||
syncTimestamp = ts;
|
||||
readySnapshotPending = true;
|
||||
bufMutex.FastUnLock();
|
||||
(void)dataSem.Post();
|
||||
}
|
||||
@@ -955,60 +974,71 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::MainStage) {
|
||||
/* --- Wait for RT thread to post new data ---
|
||||
* ResetWait sleeps the background thread until the RT thread calls
|
||||
* Synchronise() and posts dataSem, or until the timeout expires.
|
||||
* Doing this FIRST means the thread spends nearly all its time here
|
||||
* instead of spinning on the non-blocking select() below.
|
||||
* Command latency is bounded by UDPS_DATA_WAIT_MS (acceptable for
|
||||
* CONNECT / DISCONNECT). */
|
||||
ErrorManagement::ErrorType waitErr =
|
||||
dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||||
bool dataReady = (waitErr == ErrorManagement::NoError);
|
||||
/* --- Wait for the RT thread to publish new data ---
|
||||
* dataSem is only a wake-up hint, never the record of pending work:
|
||||
* EventSem::ResetWait resets the semaphore before waiting, so a Post that
|
||||
* landed while this thread was inside ServiceClients()/SendData() is
|
||||
* destroyed by the next Reset. Deciding what to send from the wait result
|
||||
* would then skip that publication entirely, and the next flush would
|
||||
* overwrite it — the receiver sees the batch's whole time span missing.
|
||||
* The buffers therefore carry the state, and are only waited on when they
|
||||
* are empty (which also avoids paying the wait when work is already
|
||||
* queued). */
|
||||
if (!HasPendingPublication()) {
|
||||
(void)dataSem.ResetWait(TimeoutType(UDPS_DATA_WAIT_MS));
|
||||
}
|
||||
|
||||
/* --- Poll for incoming control commands (CONNECT / DISCONNECT / ACK) ---
|
||||
*/
|
||||
server.ServiceClients();
|
||||
|
||||
if (dataReady && server.HasClients()) {
|
||||
/* Synchronise() already gates posting dataSem to the correct rate
|
||||
* (size/time for Accumulate, every-Nth for Decimate, every call for
|
||||
* Strict). Execute() just sends whatever is in the ready buffers. */
|
||||
if (publishMode == UDPStreamerPublishAccumulate) {
|
||||
/* --- Accumulate batch send --- */
|
||||
uint32 fill = 0u;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
fill = readyFill;
|
||||
if (fill > 0u) {
|
||||
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
|
||||
fill * totalSrcBytes);
|
||||
(void)MemoryOperationsHelper::Copy(
|
||||
reinterpret_cast<uint8 *>(scratchTimestamps),
|
||||
reinterpret_cast<const uint8 *>(readyTimestamps),
|
||||
fill * static_cast<uint32>(sizeof(uint64)));
|
||||
}
|
||||
bufMutex.FastUnLock();
|
||||
/* Synchronise() already gates publication to the correct rate (size/time
|
||||
* for Accumulate, every-Nth for Decimate, every call for Strict). The
|
||||
* pending publication is consumed whether or not anyone is listening, so
|
||||
* that a client-less streamer neither spins here nor delivers a stale
|
||||
* snapshot to the next client that connects. */
|
||||
if (publishMode == UDPStreamerPublishAccumulate) {
|
||||
/* --- Accumulate batch send --- */
|
||||
uint32 fill = 0u;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
fill = readyFill;
|
||||
if (fill > 0u) {
|
||||
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
|
||||
fill * totalSrcBytes);
|
||||
(void)MemoryOperationsHelper::Copy(
|
||||
reinterpret_cast<uint8 *>(scratchTimestamps),
|
||||
reinterpret_cast<const uint8 *>(readyTimestamps),
|
||||
fill * static_cast<uint32>(sizeof(uint64)));
|
||||
readyFill = 0u;
|
||||
}
|
||||
bufMutex.FastUnLock();
|
||||
|
||||
if (fill > 0u) {
|
||||
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
|
||||
uint32 sendBytes =
|
||||
UDPS_TIMESTAMP_BYTES + 4u + fill * singleCycleWireBytes;
|
||||
packetCounter++;
|
||||
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send Accumulate DATA packet (counter=%u).",
|
||||
packetCounter);
|
||||
}
|
||||
if ((fill > 0u) && server.HasClients()) {
|
||||
SerializeAccumulated(scratchBuffer, scratchTimestamps, fill);
|
||||
uint32 sendBytes =
|
||||
UDPS_TIMESTAMP_BYTES + 4u + fill * singleCycleWireBytes;
|
||||
packetCounter++;
|
||||
if (!server.SendData(packetCounter, wireBuffer, sendBytes)) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Failed to send Accumulate DATA packet (counter=%u).",
|
||||
packetCounter);
|
||||
}
|
||||
} else {
|
||||
/* --- Single-snapshot send (Strict or Decimate) --- */
|
||||
uint64 ts = 0u;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
}
|
||||
} else {
|
||||
/* --- Single-snapshot send (Strict or Decimate) --- */
|
||||
uint64 ts = 0u;
|
||||
bool pending = false;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
pending = readySnapshotPending;
|
||||
if (pending) {
|
||||
(void)MemoryOperationsHelper::Copy(scratchBuffer, readyBuffer,
|
||||
totalSrcBytes);
|
||||
ts = syncTimestamp;
|
||||
bufMutex.FastUnLock();
|
||||
readySnapshotPending = false;
|
||||
}
|
||||
bufMutex.FastUnLock();
|
||||
|
||||
if (pending && server.HasClients()) {
|
||||
QuantizeAndSerialize(scratchBuffer, ts);
|
||||
|
||||
packetCounter++;
|
||||
@@ -1019,6 +1049,8 @@ ErrorManagement::ErrorType UDPStreamer::Execute(ExecutionInfo &info) {
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
ReportDroppedPublications();
|
||||
}
|
||||
|
||||
if (info.GetStage() == ExecutionInfo::TerminationStage) {
|
||||
@@ -1327,6 +1359,36 @@ bool UDPStreamer::IsClientConnected() const { return server.HasClients(); }
|
||||
|
||||
bool UDPStreamer::IsMulticast() const { return server.IsMulticast(); }
|
||||
|
||||
uint32 UDPStreamer::GetDroppedPublications() const { return droppedPublications; }
|
||||
|
||||
bool UDPStreamer::HasPendingPublication() {
|
||||
bool pending = false;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
pending = (readyFill > 0u) || readySnapshotPending;
|
||||
bufMutex.FastUnLock();
|
||||
return pending;
|
||||
}
|
||||
|
||||
void UDPStreamer::ReportDroppedPublications() {
|
||||
uint64 now = HighResolutionTimer::Counter();
|
||||
if ((now - lastDropReportTicks) < HighResolutionTimer::Frequency()) {
|
||||
return;
|
||||
}
|
||||
lastDropReportTicks = now;
|
||||
|
||||
uint32 dropped = 0u;
|
||||
bufMutex.FastLock(TTInfiniteWait);
|
||||
dropped = droppedPublications;
|
||||
bufMutex.FastUnLock();
|
||||
|
||||
if (dropped > 0u) {
|
||||
REPORT_ERROR(ErrorManagement::Warning,
|
||||
"Dropped %u unsent publication(s) so far: the sender thread is "
|
||||
"not keeping up with the RT cycle.",
|
||||
dropped);
|
||||
}
|
||||
}
|
||||
|
||||
CLASS_REGISTER(UDPStreamer, "1.0")
|
||||
|
||||
} /* namespace MARTe */
|
||||
|
||||
@@ -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 */
|
||||
|
||||
@@ -36,7 +36,13 @@ UDPSClient::UDPSClient()
|
||||
disconnectTick(0u),
|
||||
lastKeepAliveTicks(0u),
|
||||
localPort(0u),
|
||||
lastGcTicks(0u) {
|
||||
lastGcTicks(0u),
|
||||
lastDropWarnTicks(0u),
|
||||
droppedSinceWarn(0u),
|
||||
lastDataCounter(0u),
|
||||
lastDataCounterValid(false),
|
||||
lastDataGap(0u),
|
||||
staleDataPackets(0u) {
|
||||
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
reassemblySlots[i].counter = 0u;
|
||||
@@ -46,7 +52,11 @@ UDPSClient::UDPSClient()
|
||||
reassemblySlots[i].active = false;
|
||||
reassemblySlots[i].firstSeenTicks = 0u;
|
||||
reassemblySlots[i].chunkSize = 0u;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0, 32u);
|
||||
reassemblySlots[i].assembledBytes = 0u;
|
||||
reassemblySlots[i].pendingTailBytes = 0u;
|
||||
reassemblySlots[i].pendingTailValid = false;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[i].recvMask, 0,
|
||||
UDPS_CLIENT_RECV_MASK_BYTES);
|
||||
}
|
||||
}
|
||||
|
||||
@@ -228,6 +238,12 @@ bool UDPSClient::Connect() {
|
||||
connected = true;
|
||||
lastDataTicks = HighResolutionTimer::Counter();
|
||||
lastKeepAliveTicks = lastDataTicks;
|
||||
/* The producer's packetCounter restarts independently of ours, so a
|
||||
* counter carried over from the previous connection would make the
|
||||
* sequence gate reject the whole new stream as stale. */
|
||||
lastDataCounterValid = false;
|
||||
lastDataCounter = 0u;
|
||||
lastDataGap = 0u;
|
||||
if (listener != NULL_PTR(UDPSClientListener *)) {
|
||||
listener->OnUDPSConnected();
|
||||
}
|
||||
@@ -498,25 +514,45 @@ bool UDPSClient::ReceiveAndProcess() {
|
||||
return true; // only the TCP socket was readable
|
||||
}
|
||||
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(recvBuf));
|
||||
bool ok;
|
||||
if (useMulticast) {
|
||||
ok = mcastSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
|
||||
}
|
||||
else {
|
||||
ok = recvSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
|
||||
}
|
||||
/* Drain the socket rather than taking one datagram per Execute() iteration:
|
||||
* a fragmented high-rate source delivers datagrams far faster than the
|
||||
* select/read round trip retires them, and the resulting kernel-buffer
|
||||
* overflow shows up as lost fragments — i.e. as packets that can never be
|
||||
* reassembled. Bounded so the silence and keepalive checks in Execute()
|
||||
* still run under a sustained flood. */
|
||||
uint32 drained = 0u;
|
||||
while (drained < UDPS_CLIENT_MAX_DATAGRAMS_PER_CYCLE) {
|
||||
uint32 recvSize = static_cast<uint32>(sizeof(recvBuf));
|
||||
bool ok;
|
||||
if (useMulticast) {
|
||||
ok = mcastSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
|
||||
}
|
||||
else {
|
||||
ok = recvSocket.Read(reinterpret_cast<char8 *>(recvBuf), recvSize);
|
||||
}
|
||||
|
||||
if (!ok || (recvSize < UDPS_HEADER_SIZE)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: ReceiveAndProcess: Read() failed or short packet "
|
||||
"(ok=%s, recvSize=%u, HEADER_SIZE=%u).",
|
||||
ok ? "true" : "false", recvSize, UDPS_HEADER_SIZE);
|
||||
return false;
|
||||
}
|
||||
if (!ok || (recvSize < UDPS_HEADER_SIZE)) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: ReceiveAndProcess: Read() failed or short packet "
|
||||
"(ok=%s, recvSize=%u, HEADER_SIZE=%u).",
|
||||
ok ? "true" : "false", recvSize, UDPS_HEADER_SIZE);
|
||||
return false;
|
||||
}
|
||||
|
||||
lastDataTicks = HighResolutionTimer::Counter();
|
||||
ProcessDatagram(recvBuf, recvSize);
|
||||
lastDataTicks = HighResolutionTimer::Counter();
|
||||
ProcessDatagram(recvBuf, recvSize);
|
||||
drained++;
|
||||
|
||||
/* Stop as soon as the socket runs dry: Read() would otherwise block. */
|
||||
fd_set dset;
|
||||
FD_ZERO(&dset);
|
||||
FD_SET(fd, &dset);
|
||||
struct timeval zero;
|
||||
zero.tv_sec = 0; zero.tv_usec = 0;
|
||||
if (select(fd + 1, &dset, NULL, NULL, &zero) <= 0) {
|
||||
break;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
@@ -599,7 +635,7 @@ void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
|
||||
if (hdr->type == UDPS_TYPE_CONFIG) {
|
||||
listener->OnUDPSConfig(pl, payloadBytes);
|
||||
}
|
||||
else {
|
||||
else if (AcceptDataCounter(hdr->counter)) {
|
||||
listener->OnUDPSData(pl, payloadBytes);
|
||||
}
|
||||
}
|
||||
@@ -616,21 +652,83 @@ void UDPSClient::ProcessDatagram(const uint8 *buf, uint32 size) {
|
||||
// Private: PlaceFragment
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
uint32 UDPSClient::AcquireReassemblySlot(uint32 counter, uint8 type) {
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (!reassemblySlots[i].active) {
|
||||
return i;
|
||||
}
|
||||
}
|
||||
|
||||
/* All slots busy. The producer emits packets sequentially, so a slot
|
||||
* holding an OLDER counter of the SAME stream is provably dead: its
|
||||
* missing fragments were sent before the ones arriving now and will never
|
||||
* turn up. Reclaiming it immediately — instead of waiting out the 2 s GC —
|
||||
* is what keeps a handful of lost fragments from wedging the whole table.
|
||||
* The counter is a wrapping uint32, so compare via the signed difference. */
|
||||
uint32 victim = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
|
||||
int32 bestDist = 0;
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (reassemblySlots[i].type != type) {
|
||||
continue;
|
||||
}
|
||||
int32 dist = static_cast<int32>(counter - reassemblySlots[i].counter);
|
||||
if ((dist > 0) && (dist > bestDist)) {
|
||||
bestDist = dist;
|
||||
victim = i;
|
||||
}
|
||||
}
|
||||
|
||||
if (victim >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
|
||||
/* Nothing is provably dead (e.g. the other stream owns every slot):
|
||||
* fall back to the least recently started. */
|
||||
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
|
||||
victim = 0u;
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (reassemblySlots[i].firstSeenTicks < oldestTick) {
|
||||
oldestTick = reassemblySlots[i].firstSeenTicks;
|
||||
victim = i;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
NoteDroppedIncomplete(reassemblySlots[victim].counter);
|
||||
return victim;
|
||||
}
|
||||
|
||||
void UDPSClient::NoteDroppedIncomplete(uint32 counter) {
|
||||
droppedSinceWarn++;
|
||||
uint64 now = HighResolutionTimer::Counter();
|
||||
if ((now - lastDropWarnTicks) >= HighResolutionTimer::Frequency()) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: dropped %u incomplete packet(s) in the last "
|
||||
"second (latest counter %u); fragments are being lost.",
|
||||
droppedSinceWarn, counter);
|
||||
droppedSinceWarn = 0u;
|
||||
lastDropWarnTicks = now;
|
||||
}
|
||||
}
|
||||
|
||||
bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
const uint8 *payload,
|
||||
uint32 payloadBytes) {
|
||||
uint32 counter = hdr->counter;
|
||||
uint8 type = hdr->type;
|
||||
uint16 fragIdx = hdr->fragmentIdx;
|
||||
uint16 totalFrags = hdr->totalFragments;
|
||||
|
||||
if ((fragIdx >= totalFrags) || (totalFrags > 512u)) {
|
||||
if ((fragIdx >= totalFrags) ||
|
||||
(static_cast<uint32>(totalFrags) > UDPS_CLIENT_MAX_FRAGMENTS)) {
|
||||
return false; // sanity check
|
||||
}
|
||||
|
||||
// Find existing slot for this counter
|
||||
/* Slots are keyed on (counter, type): DATA and CONFIG carry independent
|
||||
* counter sequences, so the same counter value legitimately appears on
|
||||
* both, and matching on the counter alone merges the two streams into one
|
||||
* slot — one payload is delivered under the wrong type, the other is lost. */
|
||||
uint32 slot = UDPS_CLIENT_MAX_REASSEMBLY_SLOTS;
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter)) {
|
||||
if (reassemblySlots[i].active && (reassemblySlots[i].counter == counter) &&
|
||||
(reassemblySlots[i].type == type)) {
|
||||
slot = i;
|
||||
break;
|
||||
}
|
||||
@@ -638,34 +736,20 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
|
||||
// Allocate new slot if not found
|
||||
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (!reassemblySlots[i].active) {
|
||||
slot = i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
if (slot >= UDPS_CLIENT_MAX_REASSEMBLY_SLOTS) {
|
||||
// All slots occupied — evict the oldest
|
||||
uint64 oldestTick = 0xFFFFFFFFFFFFFFFFuLL;
|
||||
for (uint32 i = 0u; i < UDPS_CLIENT_MAX_REASSEMBLY_SLOTS; i++) {
|
||||
if (reassemblySlots[i].firstSeenTicks < oldestTick) {
|
||||
oldestTick = reassemblySlots[i].firstSeenTicks;
|
||||
slot = i;
|
||||
}
|
||||
}
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Reassembly slots full; evicting oldest.");
|
||||
}
|
||||
slot = AcquireReassemblySlot(counter, type);
|
||||
|
||||
reassemblySlots[slot].counter = counter;
|
||||
reassemblySlots[slot].type = hdr->type;
|
||||
reassemblySlots[slot].type = type;
|
||||
reassemblySlots[slot].totalFragments = totalFrags;
|
||||
reassemblySlots[slot].receivedFragments = 0u;
|
||||
reassemblySlots[slot].active = true;
|
||||
reassemblySlots[slot].firstSeenTicks = HighResolutionTimer::Counter();
|
||||
reassemblySlots[slot].chunkSize = 0u;
|
||||
reassemblySlots[slot].assembledBytes = 0u;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0, 32u);
|
||||
reassemblySlots[slot].pendingTailBytes = 0u;
|
||||
reassemblySlots[slot].pendingTailValid = false;
|
||||
(void) MemoryOperationsHelper::Set(reassemblySlots[slot].recvMask, 0,
|
||||
UDPS_CLIENT_RECV_MASK_BYTES);
|
||||
}
|
||||
|
||||
UDPSReassemblySlot &s = reassemblySlots[slot];
|
||||
@@ -673,27 +757,56 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
// Skip duplicate
|
||||
uint32 byteIdx = fragIdx / 8u;
|
||||
uint8 bitMask = static_cast<uint8>(1u << (fragIdx % 8u));
|
||||
if (byteIdx < 32u) {
|
||||
if ((s.recvMask[byteIdx] & bitMask) != 0u) {
|
||||
return false; // already have this fragment
|
||||
}
|
||||
if ((s.recvMask[byteIdx] & bitMask) != 0u) {
|
||||
return false; // already have this fragment
|
||||
}
|
||||
|
||||
// Compute placement offset
|
||||
uint32 chunkSize = s.chunkSize;
|
||||
if (chunkSize == 0u) {
|
||||
// Learn chunk size from first non-last fragment
|
||||
if (fragIdx == 0u) {
|
||||
chunkSize = payloadBytes;
|
||||
s.chunkSize = chunkSize;
|
||||
}
|
||||
else {
|
||||
// Can't place yet without knowing chunk size — drop (rare edge case)
|
||||
const bool isLastFragment = ((static_cast<uint32>(fragIdx) + 1u) ==
|
||||
static_cast<uint32>(totalFrags));
|
||||
|
||||
/* Every fragment but the last carries a full chunk, so any of them reveals
|
||||
* the chunk size — waiting specifically for fragment 0 means a merely
|
||||
* reordered burst, with nothing lost, destroys the packet. */
|
||||
if ((s.chunkSize == 0u) && !isLastFragment) {
|
||||
s.chunkSize = payloadBytes;
|
||||
}
|
||||
|
||||
if (s.chunkSize == 0u) {
|
||||
/* The last fragment arrived before any full-size one: its offset is
|
||||
* not computable yet, so hold it until the chunk size is known. */
|
||||
if (payloadBytes > UDPS_CLIENT_PENDING_TAIL_BYTES) {
|
||||
return false;
|
||||
}
|
||||
if (payloadBytes > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(s.pendingTail, payload, payloadBytes);
|
||||
}
|
||||
s.pendingTailBytes = payloadBytes;
|
||||
s.pendingTailValid = true;
|
||||
s.recvMask[byteIdx] |= bitMask;
|
||||
s.receivedFragments++;
|
||||
return false; // totalFrags > 1 here, so this can never complete a packet
|
||||
}
|
||||
|
||||
uint32 offset = static_cast<uint32>(fragIdx) * chunkSize;
|
||||
// Flush a deferred last fragment now that the chunk size is known.
|
||||
if (s.pendingTailValid) {
|
||||
uint32 tailOffset = (static_cast<uint32>(s.totalFragments) - 1u) * s.chunkSize;
|
||||
if ((tailOffset + s.pendingTailBytes) > static_cast<uint32>(sizeof(s.payload))) {
|
||||
s.active = false;
|
||||
NoteDroppedIncomplete(s.counter);
|
||||
return false; // overflow guard
|
||||
}
|
||||
if (s.pendingTailBytes > 0u) {
|
||||
(void) MemoryOperationsHelper::Copy(s.payload + tailOffset,
|
||||
s.pendingTail, s.pendingTailBytes);
|
||||
}
|
||||
if ((tailOffset + s.pendingTailBytes) > s.assembledBytes) {
|
||||
s.assembledBytes = tailOffset + s.pendingTailBytes;
|
||||
}
|
||||
s.pendingTailValid = false;
|
||||
s.pendingTailBytes = 0u;
|
||||
}
|
||||
|
||||
uint32 offset = static_cast<uint32>(fragIdx) * s.chunkSize;
|
||||
if ((offset + payloadBytes) > static_cast<uint32>(sizeof(s.payload))) {
|
||||
return false; // overflow guard
|
||||
}
|
||||
@@ -709,9 +822,7 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
s.assembledBytes = offset + payloadBytes;
|
||||
}
|
||||
|
||||
if (byteIdx < 32u) {
|
||||
s.recvMask[byteIdx] |= bitMask;
|
||||
}
|
||||
s.recvMask[byteIdx] |= bitMask;
|
||||
s.receivedFragments++;
|
||||
|
||||
// Check if complete
|
||||
@@ -727,6 +838,30 @@ bool UDPSClient::PlaceFragment(const UDPSPacketHeader *hdr,
|
||||
// Private: DeliverAssembled
|
||||
// ---------------------------------------------------------------------------
|
||||
|
||||
bool UDPSClient::AcceptDataCounter(uint32 counter) {
|
||||
if (!lastDataCounterValid) {
|
||||
lastDataCounterValid = true;
|
||||
lastDataCounter = counter;
|
||||
lastDataGap = 0u;
|
||||
return true;
|
||||
}
|
||||
// The counter is a wrapping uint32, so order it by the signed difference:
|
||||
// that stays correct across the wrap, where a plain comparison would call
|
||||
// the first packet after it stale and reject the stream from then on.
|
||||
int32 delta = static_cast<int32>(counter - lastDataCounter);
|
||||
if (delta <= 0) {
|
||||
staleDataPackets++;
|
||||
return false;
|
||||
}
|
||||
lastDataGap = static_cast<uint32>(delta) - 1u;
|
||||
lastDataCounter = counter;
|
||||
return true;
|
||||
}
|
||||
|
||||
uint32 UDPSClient::GetLastDataGap() const { return lastDataGap; }
|
||||
|
||||
uint32 UDPSClient::GetStaleDataPackets() const { return staleDataPackets; }
|
||||
|
||||
void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
|
||||
if (listener == NULL_PTR(UDPSClientListener *)) {
|
||||
return;
|
||||
@@ -739,7 +874,7 @@ void UDPSClient::DeliverAssembled(UDPSReassemblySlot &s) {
|
||||
if (s.type == UDPS_TYPE_CONFIG) {
|
||||
listener->OnUDPSConfig(s.payload, totalSize);
|
||||
}
|
||||
else {
|
||||
else if (AcceptDataCounter(s.counter)) {
|
||||
listener->OnUDPSData(s.payload, totalSize);
|
||||
}
|
||||
}
|
||||
@@ -757,10 +892,8 @@ void UDPSClient::GcReassemblySlots() {
|
||||
continue;
|
||||
}
|
||||
if ((now - reassemblySlots[i].firstSeenTicks) > staleThreshold) {
|
||||
REPORT_ERROR_STATIC(ErrorManagement::Warning,
|
||||
"UDPSClient: Discarding stale reassembly slot (counter %u).",
|
||||
reassemblySlots[i].counter);
|
||||
reassemblySlots[i].active = false;
|
||||
NoteDroppedIncomplete(reassemblySlots[i].counter);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -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];
|
||||
|
||||
@@ -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 <gtest/gtest.h>
|
||||
|
||||
#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<float64>(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<float64>(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<float64>(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<float64>(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<float64>(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);
|
||||
}
|
||||
@@ -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=../../..
|
||||
|
||||
@@ -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());
|
||||
}
|
||||
|
||||
@@ -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<MARTe::uint32>(v);
|
||||
if ((static_cast<MARTe::float64>(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<RealTimeApplication> 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<UDPStreamer> 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<float64 *>(addr);
|
||||
ok = ok && (sigA != NULL_PTR(float64 *));
|
||||
}
|
||||
|
||||
/* Drive the RT cycles. */
|
||||
if (ok) {
|
||||
for (uint32 i = 0u; (i < CYCLES) && ok; i++) {
|
||||
*sigA = static_cast<float64>(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;
|
||||
}
|
||||
|
||||
@@ -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_ */
|
||||
|
||||
@@ -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<uint8>((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<const char8 *>(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<uint32>(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();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user