martino/udpscope
1
0 Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: hold a packet that runs ahead of the sequence, and deliver a late one flagged

Browse Source
This commit is contained in:
Martino Ferrari committed 2026-10-08 17:34:26 +02:00
1 parent 57e2f6aa8b
commit 6e79a67252
2 files changed
+276 -29
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

deps/udpsclient/udps_client.c
Vendored
+250 -29
View File
@@ -33,6 +33,20 @@
/* Tunable limits */
/*---------------------------------------------------------------------------*/
/** Datagrams held waiting for an earlier counter. */
#define UDPS_REORDER_SLOTS 64
/** Total bytes the reorder ring may hold, so a fast stream cannot grow it without bound. */
#define UDPS_REORDER_BYTES (4u * 1024u * 1024u)
typedef struct {
uint8_t *buf; /**< owned; grown on demand to the payload's length */
size_t cap;
size_t len;
uint32_t counter;
double recv_time;
int used;
} udps_held_t;
/** Concurrent in-flight reassemblies. The server interleaves at most a couple. */
#define UDPS_MAX_SLOTS 4
@@ -181,7 +195,12 @@ struct udps_client {
uint8_t *rxbuf;
uint32_t last_counter;
udps_held_t held[UDPS_REORDER_SLOTS];
size_t held_bytes; /**< payload bytes currently held */
int held_count;
uint32_t next_counter; /**< the counter the sequence is waiting for */
double gap_deadline; /**< wall time the current gap gives up at; 0 = no gap */
int have_counter;
udps_stats_t stats;
@@ -475,43 +494,23 @@ static int decode_config(udps_client_t *c, const uint8_t *pl, size_t len) {
return 0;
}
static int decode_data(udps_client_t *c, const uint8_t *pl, size_t len,
uint32_t counter, double recv_time) {
/* Decodes and delivers one DATA payload whose ordering has already been resolved
* by the caller. `lost` counts how many counters the sequence skipped over
* before this packet, and `late` marks it as having lost the race past the
* reorder window (so the consumer already has a hole where it belongs). */
static int deliver_data(udps_client_t *c, const uint8_t *pl, size_t len,
uint32_t counter, double recv_time, uint32_t lost, uint8_t late) {
uint32_t nsamples = 1u;
size_t off = 8u;
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);
}
@@ -554,10 +553,12 @@ static int decode_data(udps_client_t *c, const uint8_t *pl, size_t len,
}
c->stats.frames_delivered++;
c->stats.counter_gaps += lost;
if (c->on_data != NULL) {
frame.counter = counter;
frame.lost = lost;
frame.lost = late ? 0u : lost;
frame.late = late;
frame.hrt = rd_u64(pl);
frame.recv_time = recv_time;
frame.publish_mode = c->publish_mode;
@@ -570,6 +571,201 @@ static int decode_data(udps_client_t *c, const uint8_t *pl, size_t len,
return 0;
}
/* Finds a free slot in the reorder ring and stores the payload there.
*
* A duplicate counter (same datagram arriving twice on the wire) replaces the
* existing slot so one counter never consumes two slots and both get delivered.
* Returns non-zero and holds nothing when there is no room (ring full or byte
* budget exhausted). */
static int hold_packet(udps_client_t *c, const uint8_t *pl, size_t len,
uint32_t counter, double recv_time) {
int i;
int slot = -1;
if (c->held_count >= UDPS_REORDER_SLOTS) {
return -1;
}
if (c->held_bytes + len > UDPS_REORDER_BYTES) {
return -1;
}
/* Duplicate counter: reuse the existing slot to avoid double delivery. */
for (i = 0; i < UDPS_REORDER_SLOTS; i++) {
if (c->held[i].used && c->held[i].counter == counter) {
slot = i;
break;
}
}
if (slot < 0) {
for (i = 0; i < UDPS_REORDER_SLOTS; i++) {
if (!c->held[i].used) {
slot = i;
break;
}
}
}
if (slot < 0) {
return -1;
}
if (ensure_cap((void **)&c->held[slot].buf, &c->held[slot].cap, len) != 0) {
return -1;
}
/* If this is a duplicate replacement, subtract the old length first. */
if (c->held[slot].used) {
c->held_bytes -= c->held[slot].len;
c->held_count--;
}
memcpy(c->held[slot].buf, pl, len);
c->held[slot].len = len;
c->held[slot].counter = counter;
c->held[slot].recv_time = recv_time;
c->held[slot].used = 1;
c->held_bytes += len;
c->held_count++;
return 0;
}
/* Walks the held ring and delivers every entry whose counter matches the
* expected sequence, in order. The first delivery carries `first_lost` (so
* a caller that committed a gap can report the correct count); every
* subsequent delivery in the same drain pass carries lost = 0.
*
* Clears the gap deadline once the ring empties. */
static int drain_held(udps_client_t *c, uint32_t first_lost) {
int found;
int i;
int rc;
uint32_t lost;
lost = first_lost;
do {
found = 0;
for (i = 0; i < UDPS_REORDER_SLOTS; i++) {
if (c->held[i].used &&
(int32_t)(c->held[i].counter - c->next_counter) == 0) {
uint32_t cnt = c->held[i].counter;
double rt = c->held[i].recv_time;
const uint8_t *pl = c->held[i].buf;
size_t len = c->held[i].len;
/* Release the slot before delivering so deliver_data can call
* back into the client without hitting a stale used entry. */
c->held_bytes -= len;
c->held_count--;
c->held[i].used = 0;
c->next_counter = cnt + 1u;
c->stats.late_resequenced++;
rc = deliver_data(c, pl, len, cnt, rt, lost, 0);
lost = 0u;
if (rc != 0) {
return rc;
}
found = 1;
break;
}
}
} while (found);
if (c->held_count == 0) {
c->gap_deadline = 0.0;
}
return 0;
}
/* Gives up on the current gap: finds the lowest held counter (by signed delta,
* not raw value, so a wrap mid-gap is handled correctly), advances
* next_counter to it, then drains. The first delivery out of that drain
* carries the gap's lost count; the rest carry 0.
*
* With nothing held it just clears the deadline and returns. */
static int commit_gap(udps_client_t *c) {
int i;
int best = -1;
uint32_t lost;
if (c->held_count == 0) {
c->gap_deadline = 0.0;
return 0;
}
for (i = 0; i < UDPS_REORDER_SLOTS; i++) {
if (!c->held[i].used) {
continue;
}
if (best < 0 ||
(int32_t)(c->held[i].counter - c->held[best].counter) < 0) {
best = i;
}
}
lost = (uint32_t)(int32_t)(c->held[best].counter - c->next_counter);
c->next_counter = c->held[best].counter;
return drain_held(c, lost);
}
/* Orders the sequence before spending anything on the payload.
*
* 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.
*
* Three cases. A packet that *continues* the sequence is decoded now and costs no
* latency at all -- that is the common case and it must stay free. A packet that
* runs ahead of the sequence means an earlier one is missing, so it is held and the
* gap is given `reorder_window_s` to resolve; a reorder on the wire resolves in
* milliseconds, and holding is what turns it back into order. A packet behind the
* sequence lost that race: the consumer already has a hole where it belongs, so it
* is delivered flagged rather than discarded, which is the whole point -- a dropped
* packet cannot fill the hole it left. */
static int order_data(udps_client_t *c, const uint8_t *pl, size_t len,
uint32_t counter, double recv_time) {
int32_t delta;
if (c->cfg.reorder_window_s <= 0.0) {
/* Resequencing off: byte-for-byte the behaviour before this task. `delta`
* is measured against the *next* counter rather than the last, which is the
* same test shifted by one -- `delta < 0` here is `delta <= 0` there. */
uint32_t lost = 0u;
if (c->have_counter) {
delta = (int32_t)(counter - c->next_counter);
if (delta < 0) {
c->stats.stale_packets++;
return 0;
}
lost = (uint32_t)delta;
}
c->have_counter = 1;
c->next_counter = counter + 1u;
return deliver_data(c, pl, len, counter, recv_time, lost, 0);
}
if (!c->have_counter) {
c->have_counter = 1;
c->next_counter = counter + 1u;
return deliver_data(c, pl, len, counter, recv_time, 0u, 0);
}
delta = (int32_t)(counter - c->next_counter);
if (delta == 0) {
int rc = deliver_data(c, pl, len, counter, recv_time, 0u, 0);
c->next_counter = counter + 1u;
if (rc == 0) rc = drain_held(c, 0u);
return rc;
}
if (delta > 0) {
if (hold_packet(c, pl, len, counter, recv_time) != 0) {
/* No room: commit the gap now rather than growing. The counter is
* visible, so an undersized window is a reading and not a mystery. */
return commit_gap(c);
}
if (c->gap_deadline == 0.0) {
c->gap_deadline = recv_time + c->cfg.reorder_window_s;
}
return 0;
}
/* delta < 0: behind the sequence. */
c->stats.late_delivered++;
return deliver_data(c, pl, len, counter, recv_time, 0u, 1);
}
/*---------------------------------------------------------------------------*/
/* Fragment reassembly */
/*---------------------------------------------------------------------------*/
@@ -579,6 +775,14 @@ static void slot_reset_all(udps_client_t *c) {
for (i = 0; i < UDPS_MAX_SLOTS; i++) {
c->slots[i].active = 0;
}
/* A reconnect starts a new sequence; payloads held from the previous one
* must not be delivered into it. */
for (i = 0; i < UDPS_REORDER_SLOTS; i++) {
c->held[i].used = 0;
}
c->held_count = 0;
c->held_bytes = 0;
c->gap_deadline = 0.0;
}
static void gc_slots(udps_client_t *c, double now) {
@@ -596,7 +800,7 @@ static void deliver_payload(udps_client_t *c, uint8_t type, const uint8_t *pl,
if (type == UDPS_PKT_CONFIG) {
(void)decode_config(c, pl, len);
} else {
(void)decode_data(c, pl, len, counter, recv_time);
(void)order_data(c, pl, len, counter, recv_time);
}
}
@@ -906,6 +1110,7 @@ static int do_connect(udps_client_t *c) {
c->last_data = now_mono();
c->last_keepalive = c->last_data;
c->have_counter = 0;
c->next_counter = 0u;
slot_reset_all(c);
if (c->ever_connected) {
c->stats.reconnects++;
@@ -981,6 +1186,7 @@ void udps_client_config_init(udps_client_config_t *cfg) {
cfg->keepalive_interval_s = 15.0; /* server evicts silent clients at 30 s */
cfg->recv_buffer_bytes = 4u * 1024u * 1024u;
cfg->max_packet_bytes = 1u * 1024u * 1024u;
cfg->reorder_window_s = 0.020;
}
static void copy_str(char *dst, size_t cap, const char *src) {
@@ -1048,6 +1254,9 @@ void udps_client_destroy(udps_client_t *c) {
for (i = 0; i < UDPS_MAX_SLOTS; i++) {
free(c->slots[i].payload);
}
for (i = 0; i < UDPS_REORDER_SLOTS; i++) {
free(c->held[i].buf);
}
free(c->sigs);
free(c->valbuf);
free(c->vals);
@@ -1151,6 +1360,14 @@ int udps_client_poll(udps_client_t *c, int timeout_ms) {
}
budget = clamp_deadline(budget, c->cfg.silence_timeout_s, now - c->last_data);
/* A held packet must flush on time even when nothing further arrives: at 10 pkt/s
* the next arrival is 100 ms away, which is five times the whole budget. */
if (c->gap_deadline > 0.0) {
double left = c->gap_deadline - now_wall();
if (left < 0.0) left = 0.0;
if (left < budget) budget = left;
}
FD_ZERO(&rset);
maxfd = -1;
if (c->udp_fd >= 0 && c->udp_fd < FD_SETSIZE) {
@@ -1221,6 +1438,10 @@ int udps_client_poll(udps_client_t *c, int timeout_ms) {
}
gc_slots(c, now_wall());
if (c->gap_deadline > 0.0 && now_wall() >= c->gap_deadline) {
(void)commit_gap(c);
}
if (c->cfg.silence_timeout_s > 0.0 &&
(now - c->last_data) >= c->cfg.silence_timeout_s) {
do_disconnect(c, "server went silent");