From 6e79a67252081f263848a4f8be47eb8a27db55a8 Mon Sep 17 00:00:00 2001 From: Martino Ferrari Date: Thu, 8 Oct 2026 17:34:26 +0200 Subject: [PATCH] feat: hold a packet that runs ahead of the sequence, and deliver a late one flagged --- deps/udpsclient/udps_client.c | 279 ++++++++++++++++++++++++++++++---- deps/udpsclient/udps_client.h | 26 ++++ 2 files changed, 276 insertions(+), 29 deletions(-) diff --git a/deps/udpsclient/udps_client.c b/deps/udpsclient/udps_client.c index 64925c4..cc72595 100644 --- a/deps/udpsclient/udps_client.c +++ b/deps/udpsclient/udps_client.c @@ -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"); diff --git a/deps/udpsclient/udps_client.h b/deps/udpsclient/udps_client.h index 0d3fe8b..92ff0ef 100644 --- a/deps/udpsclient/udps_client.h +++ b/deps/udpsclient/udps_client.h @@ -150,6 +150,13 @@ typedef struct { * range the next frame claims. */ uint32_t lost; + /** + * 1 when this frame's counter is behind the sequence already delivered: it lost + * the race past the reorder window and the consumer has a hole where it belongs. + * @c lost is 0 for such a frame -- it is not missing anything, it *is* what was + * missing -- so a consumer sizes its backfill from @c counter, not from @c lost. + */ + uint8_t late; 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_*. */ @@ -180,6 +187,17 @@ typedef struct { * time base older than data already handed over. */ uint64_t stale_packets; + /** + * DATA packets that arrived out of order and were put *back* in order inside + * the reorder window, so the consumer never saw a gap for them. + */ + uint64_t late_resequenced; + /** + * DATA packets delivered behind the sequence because the reorder window had + * already closed on them. The consumer has a hole for these and may fill it; + * before the window existed they were counted in @c stale_packets and dropped. + */ + uint64_t late_delivered; uint64_t reconnects; } udps_stats_t; @@ -226,6 +244,14 @@ typedef struct { double keepalive_interval_s;/**< Unicast ACK period. 0 disables. */ uint32_t recv_buffer_bytes; /**< SO_RCVBUF; large bursts need a large value. */ uint32_t max_packet_bytes; /**< Ceiling on one reassembled payload. */ + /** + * How long a packet may be held waiting for an earlier counter, in seconds. + * + * A *contiguous* arrival is never held, so this is the latency a gap costs and + * not a latency the stream pays. 0 disables resequencing entirely and restores + * the drop-on-late behaviour. Default 0.020. + */ + double reorder_window_s; } udps_client_config_t; /** Fills @p cfg with the defaults documented in Docs/UDPS-C-Client.md. */