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

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,
+/* Decodes and delivers one DATA payload whose ordering has already been resolved
-                       uint32_t counter, double recv_time) {
+ * 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");

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. */