wip

Author: pavel-kirienko

.idea/dictionaries/project.xml

@@ -109,6 +109,8 @@ const uint_least8_t canard_len_to_dlc[65] = {
 
 static size_t      smaller(const size_t a, const size_t b) { return (a < b) ? a : b; }
 static size_t      larger(const size_t a, const size_t b) { return (a > b) ? a : b; }
+static uint64_t    min_u64(const uint64_t a, const uint64_t b) { return (a < b) ? a : b; }
+static uint64_t    max_u64(const uint64_t a, const uint64_t b) { return (a > b) ? a : b; }
 static int64_t     min_i64(const int64_t a, const int64_t b) { return (a < b) ? a : b; }
 static int64_t     max_i64(const int64_t a, const int64_t b) { return (a > b) ? a : b; }
 static canard_us_t sooner(const canard_us_t a, const canard_us_t b) { return min_i64(a, b); }
@@ -1171,17 +1173,6 @@ static byte_t rx_parse(const uint32_t       can_id,
     return (is_v0 ? 1U : 0U) | (is_v1 ? 2U : 0U);
 }
 
-// f(2, 3)==31, f(2, 2)==0, f(2, 1)==1
-static byte_t rx_transfer_id_forward_difference(const byte_t a, const byte_t b)
-{
-    CANARD_ASSERT((a <= CANARD_TRANSFER_ID_MAX) && (b <= CANARD_TRANSFER_ID_MAX));
-    int16_t diff = (int16_t)(((int16_t)a) - ((int16_t)b));
-    if (diff < 0) {
-        diff = (int16_t)(diff + (int16_t)(1U << CANARD_TRANSFER_ID_BIT_LENGTH));
-    }
-    return (byte_t)diff;
-}
-
 // Idle sessions are removed after this timeout even if reassembly is not finished.
 // This is not related to the transfer-ID timeout and does not affect the correctness;
 // it is only needed to improve the memory footprint when remotes cease sending messages.
@@ -1193,19 +1184,92 @@ static byte_t rx_transfer_id_forward_difference(const byte_t a, const byte_t b)
 // by a transfer at the priority one higher, and so on up to the maximum.
 #define FOREACH_SLOT(i) for (size_t i = 0; (i) < CANARD_PRIO_COUNT; (i)++)
 
+// The seqno is the transfer-ID unrolled at the session layer. It is used to handle transfer-ID wraparound across
+// priority levels. The minimum sufficient bit width is approx. 44, which given 10000 transfers per second will take
+// approx. 55 years to overflow. We use bit fields for aggressive dynamic memory footprint optimization.
+#define RX_SEQNO_BITS (48 - IFACE_INDEX_BIT_LENGTH - 1)
+static_assert(RX_SEQNO_BITS >= 44, "not enough bits for seqno");
+#define RX_SEQNO_MASK ((UINT64_C(1) << RX_SEQNO_BITS) - 1U)
+
 // Reassembly state at a specific priority level.
 // Maintaining separate state per priority level allows preemption of higher-priority transfers without loss.
 typedef struct
 {
     canard_us_t start_ts;
-    size_t      total_size; // The raw payload size before the implicit truncation and CRC removal.
-    uint16_t    crc;
-    byte_t      transfer_id : CANARD_TRANSFER_ID_BIT_LENGTH;
-    byte_t      expected_toggle : 1;
-    byte_t      iface_index : IFACE_INDEX_BIT_LENGTH;
-    byte_t      payload[]; // Extent-sized.
+    uint64_t    seqno : RX_SEQNO_BITS; // Unrolled transfer-ID.
+    uint64_t    iface_index : IFACE_INDEX_BIT_LENGTH;
+    uint64_t    expected_toggle : 1;
+    uint64_t    crc             : 16;
+    uint32_t    total_size; // The raw payload size seen before the implicit truncation and CRC removal.
+    byte_t      payload[];  // Extent-sized.
 } rx_slot_t;
 #define RX_SLOT_OVERHEAD (offsetof(rx_slot_t, payload))
+static_assert(RX_SLOT_OVERHEAD == 20, "unexpected layout");
+
+static rx_slot_t* rx_slot_new(const canard_subscription_t* const sub,
+                              const canard_us_t                  start_ts,
+                              const uint64_t                     seqno,
+                              const byte_t                       iface_index)
+{
+    rx_slot_t* const slot = mem_alloc(sub->owner->mem.rx_payload, RX_SLOT_OVERHEAD + sub->extent);
+    if (slot != NULL) {
+        memset(slot, 0, RX_SLOT_OVERHEAD);
+        slot->start_ts        = start_ts;
+        slot->crc             = sub->crc_seed;
+        slot->seqno           = seqno & RX_SEQNO_MASK;
+        slot->expected_toggle = kind_is_v1(sub->kind) ? 1 : 0;
+        slot->iface_index     = iface_index & ((1U << IFACE_INDEX_BIT_LENGTH) - 1U);
+    }
+    return slot;
+}
+
+static void rx_slot_destroy(const canard_subscription_t* const sub, rx_slot_t* const slot)
+{
+    mem_free(sub->owner->mem.rx_payload, RX_SLOT_OVERHEAD + sub->extent, slot);
+}
+
+static void rx_slot_store(rx_slot_t* const slot, const size_t extent, const canard_bytes_t payload)
+{
+    if (slot->total_size < extent) {
+        const size_t copy_size = smaller(payload.size, (size_t)(extent - slot->total_size));
+        (void)memcpy(&slot->payload[slot->total_size], payload.data, copy_size);
+    }
+    slot->total_size = (uint32_t)(slot->total_size + payload.size); // Before truncation.
+}
+
+// A compact representation is needed because we need to store an array of these in dynamic memory.
+typedef struct
+{
+    uint16_t limbs[3];
+} rx_seqno_packed_t;
+
+static uint64_t rx_seqno_unpack(const rx_seqno_packed_t v)
+{
+    return ((uint64_t)v.limbs[0]) | (((uint64_t)v.limbs[1]) << 16U) | (((uint64_t)v.limbs[2]) << 32U);
+}
+static rx_seqno_packed_t rx_seqno_pack(const uint64_t v)
+{
+    return (rx_seqno_packed_t){
+        { (uint16_t)(v & 0xFFFFU), (uint16_t)((v >> 16U) & 0xFFFFU), (uint16_t)((v >> 32U) & 0xFFFFU) }
+    };
+}
+static uint64_t rx_seqno_linearize(const uint64_t ref_seqno, const byte_t transfer_id)
+{
+    CANARD_ASSERT(transfer_id <= CANARD_TRANSFER_ID_MAX);
+    const byte_t ref_tid = (byte_t)(ref_seqno & CANARD_TRANSFER_ID_MAX);
+    int16_t      delta   = ((int16_t)transfer_id) - ((int16_t)ref_tid); // NOLINT(*-narrowing-conversions)
+    // Select the nearest congruent seqno in the half-open interval [-16, +16) around the reference.
+    if (delta > (int16_t)(CANARD_TRANSFER_ID_MAX / 2U)) {
+        delta -= (int16_t)(CANARD_TRANSFER_ID_MAX + 1U);
+    } else if (delta < -((int16_t)(CANARD_TRANSFER_ID_MAX / 2U) + 1)) {
+        delta += (int16_t)(CANARD_TRANSFER_ID_MAX + 1U);
+    }
+    // Near the origin the backward representative may underflow, so use the next forward one instead.
+    if ((delta < 0) && (ref_seqno < (uint64_t)(-delta))) {
+        delta += (int16_t)(CANARD_TRANSFER_ID_MAX + 1U);
+    }
+    return (uint64_t)(((int64_t)ref_seqno) + ((int64_t)delta));
+}
 
 // Up to libcanard v4 we used a fixed-capacity array of pointers for per-remote sessions for constant-time lookup,
 // but it was too costly on MCUs: with a 32-bit pointer it took 512 bytes for the array plus overheads,
@@ -1217,41 +1281,20 @@ typedef struct
 //  - Non-start frames never create state.
 //  - Session dedup state is updated on admitted start-of-transfer, not on transfer completion.
 //  - Timeout is consulted only for start-of-transfer admission.
-//  - Slot matching for continuation uses exact match: priority, transfer-ID, toggle, and iface.
+//  - Slot matching for continuation uses exact match: priority, transfer-ID/seqno, toggle, and iface.
 typedef struct
 {
     canard_tree_t          index;
     canard_listed_t        list_animation; // On update, session moved to the tail; oldest pushed to the head.
     canard_us_t            last_admitted_start_ts;
     rx_slot_t*             slots[CANARD_PRIO_COUNT]; // Indexed by priority level to allow preemption.
     canard_subscription_t* owner;
-    byte_t                 last_admitted_transfer_id;
+    rx_seqno_packed_t      seqno_frontier[CANARD_PRIO_COUNT];
+    byte_t                 iface_index;
     byte_t                 node_id;
 } rx_session_t;
 static_assert((sizeof(void*) > 4) || (sizeof(rx_session_t) <= 120), "too large");
 
-static rx_slot_t* rx_slot_new(const canard_subscription_t* const sub,
-                              const canard_us_t                  start_ts,
-                              const byte_t                       transfer_id,
-                              const byte_t                       iface_index)
-{
-    rx_slot_t* const slot = mem_alloc(sub->owner->mem.rx_payload, RX_SLOT_OVERHEAD + sub->extent);
-    if (slot != NULL) {
-        memset(slot, 0, RX_SLOT_OVERHEAD);
-        slot->start_ts        = start_ts;
-        slot->crc             = sub->crc_seed;
-        slot->transfer_id     = transfer_id & CANARD_TRANSFER_ID_MAX;
-        slot->expected_toggle = kind_is_v1(sub->kind) ? 1 : 0;
-        slot->iface_index     = iface_index & ((1U << IFACE_INDEX_BIT_LENGTH) - 1U);
-    }
-    return slot;
-}
-
-static void rx_slot_destroy(const canard_subscription_t* const sub, rx_slot_t* const slot)
-{
-    mem_free(sub->owner->mem.rx_payload, RX_SLOT_OVERHEAD + sub->extent, slot);
-}
-
 static int32_t rx_session_cavl_compare(const void* const user, const canard_tree_t* const node)
 {
     return ((int32_t)(*(byte_t*)user)) - ((int32_t)CAVL2_TO_OWNER(node, rx_session_t, index)->node_id);
@@ -1315,13 +1358,61 @@ static size_t rx_session_scan(rx_session_t* const ses, const canard_us_t now)
     return n_slots;
 }
 
-static void rx_slot_write_payload(const rx_session_t* const ses, rx_slot_t* const slot, const canard_bytes_t payload)
+static void rx_session_record_admission(rx_session_t* const ses,
+                                        const canard_prio_t priority,
+                                        const uint64_t      seqno,
+                                        const canard_us_t   ts,
+                                        const byte_t        iface_index)
 {
-    if (slot->total_size < ses->owner->extent) {
-        const size_t copy_size = smaller(payload.size, ses->owner->extent - slot->total_size);
-        (void)memcpy(&slot->payload[slot->total_size], payload.data, copy_size);
+    ses->seqno_frontier[priority] = rx_seqno_pack(seqno); // nothing older than this at this priority from now on
+    ses->last_admitted_start_ts   = ts;
+    ses->iface_index              = iface_index;
+}
+
+// Maximum seqno seen from the given highest priority level (numerically lowest, inclusive) and down.
+static uint64_t rx_session_seqno_frontier(const rx_session_t* const ses, const canard_prio_t highest_priority)
+{
+    uint64_t seqno = 0;
+    for (size_t i = (size_t)highest_priority; i < CANARD_PRIO_COUNT; i++) {
+        seqno = max_u64(seqno, rx_seqno_unpack(ses->seqno_frontier[i]));
+    }
+    return seqno;
+}
+
+// Frame admittance state machine update. A complex piece, redesigned after v4 to support priority preemption.
+// Key ideas: 1. Separate reassembly state per priority level. 2. TID is linearized into seqno.
+// Once we admit a transfer at some priority with a certain seqno, we know that any older seqno at this or higher
+// priority would be stale, since only higher priority transfers can preempt lower priority ones.
+static bool rx_session_should_admit(const rx_session_t* const ses,
+                                    const canard_us_t         ts,
+                                    const canard_prio_t       priority,
+                                    const bool                start,
+                                    const bool                toggle,
+                                    const uint64_t            seqno,
+                                    const byte_t              iface_index)
+{
+    const rx_slot_t* const slot = ses->slots[priority];
+    if (!start) {
+        // Continuation not resumed if seqno match is prevented by transfer-ID wraparound, even if the LSb would match.
+        // This is to mitigate the risk of frankentransfers, where distinct transfers are stitched together.
+        // Such old slots would eventually either time out or be replaced on next start frame.
+        return (slot != NULL) && (slot->seqno == seqno) && (slot->iface_index == iface_index) &&
+               (slot->expected_toggle == toggle);
     }
-    slot->total_size += payload.size; // Before truncation.
+    // In the case of a single priority level, the seqno would be considered new if it is greater than last admitted.
+    // Priority preemption makes this simple condition insufficient, because a newer higher-priority transfer with
+    // a greater seqno may push older lower-priority transfers aside, causing the receiver to observe seqno going
+    // backward for new transfers. Our solution is to keep a dedicated seqno frontier per priority level.
+    //
+    // The lowest bound of a genuinely new seqno would then be located at the lowest priority level, since that level
+    // cannot preempt others. To decide if a given seqno is new, one needs to scan all priority levels from the
+    // current one (inclusive) down to the lowest level (numerically greater).
+    //
+    // Higher priority levels (numerically lesser) may have greater seqno frontiers which bear no relevance.
+    const bool seqno_new   = rx_session_seqno_frontier(ses, priority) >= seqno;
+    const bool iface_match = ses->iface_index == iface_index;
+    const bool timed_out   = ts > (ses->last_admitted_start_ts + ses->owner->transfer_id_timeout);
+    return (seqno_new && iface_match) || (iface_match && timed_out) || (timed_out && seqno_new);
 }
 
 // Returns false on OOM, no other failure modes.
@@ -1333,6 +1424,7 @@ static bool rx_session_update(canard_subscription_t* const sub,
     CANARD_ASSERT((sub != NULL) && (frame != NULL) && (frame->payload.data != NULL) && (ts >= 0));
     CANARD_ASSERT(frame->end || (frame->payload.size >= 7));
 
+    // Only start frames may create new states.
     rx_session_factory_context_t factory_context = { .owner = sub, .node_id = frame->src };
     rx_session_t* const          ses =
       CAVL2_TO_OWNER(frame->start ? cavl2_find_or_insert(&sub->sessions, //
@@ -1348,20 +1440,11 @@ static bool rx_session_update(canard_subscription_t* const sub,
         return !frame->start;
     }
 
-    // Frame admittance state machine. A highly complex piece, redesigned after v4 to support priority preemption.
-    // TID forward difference illustration: f(2,3)==31, f(2,2)==0, f(2,1)==1
-    const bool tid_new   = rx_transfer_id_forward_difference(ses->last_admitted_transfer_id, frame->transfer_id) > 1;
-    const bool timed_out = ts > (ses->last_admitted_start_ts + ses->owner->transfer_id_timeout);
-    bool       accept    = false;
-    const rx_slot_t* const slot = ses->slots[frame->priority];
-    if (!frame->start) {
-        accept = (slot != NULL) && (slot->transfer_id == frame->transfer_id) && (slot->iface_index == iface_index) &&
-                 (slot->expected_toggle == frame->toggle);
-    } else {
-        accept = ((slot == NULL) || (slot->transfer_id != frame->transfer_id)) && (tid_new || timed_out);
-    }
-    if (!accept) {
-        return true; // Frame not needed; not a failure to accept.
+    // Decide admit or drop.
+    const uint64_t seqno =
+      rx_seqno_linearize(rx_session_seqno_frontier(ses, canard_prio_exceptional), frame->transfer_id);
+    if (!rx_session_should_admit(ses, ts, frame->priority, frame->start, frame->toggle, seqno, iface_index)) {
+        return true; // Rejection is not a failure.
     }
 
     // The frame must be accepted. If this is the start of a new transfer, we must update state.
@@ -1370,17 +1453,16 @@ static bool rx_session_update(canard_subscription_t* const sub,
         rx_slot_destroy(sub, ses->slots[frame->priority]);
         ses->slots[frame->priority] = NULL;
         if (!frame->end) { // more frames to follow, must store in-progress state
-            ses->slots[frame->priority] = rx_slot_new(sub, ts, frame->transfer_id, iface_index);
+            ses->slots[frame->priority] = rx_slot_new(sub, ts, seqno, iface_index);
             if (ses->slots[frame->priority] == NULL) {
                 sub->owner->err.oom++;
                 return false;
             }
         }
-        ses->last_admitted_start_ts    = ts;
-        ses->last_admitted_transfer_id = frame->transfer_id;
+        rx_session_record_admission(ses, frame->priority, seqno, ts, iface_index);
     }
 
-    // TODO acceptance
+    // TODO acceptance is not yet implemented.
 
     return false;
 }
@@ -1396,10 +1478,10 @@ bool canard_new(canard_t* const              self,
                 const size_t                 filter_count,
                 canard_filter_t* const       filter_storage)
 {
-    bool ok = (self != NULL) && (vtable != NULL) && (vtable->now != NULL) && (vtable->tx != NULL) &&
-              (vtable->filter != NULL) && mem_valid(memory.tx_transfer) && mem_valid(memory.tx_frame) &&
-              mem_valid(memory.rx_session) && mem_valid(memory.rx_payload) &&
-              ((filter_count == 0U) || (filter_storage != NULL)) && (node_id <= CANARD_NODE_ID_MAX);
+    const bool ok = (self != NULL) && (vtable != NULL) && (vtable->now != NULL) && (vtable->tx != NULL) &&
+                    (vtable->filter != NULL) && mem_valid(memory.tx_transfer) && mem_valid(memory.tx_frame) &&
+                    mem_valid(memory.rx_session) && mem_valid(memory.rx_payload) &&
+                    ((filter_count == 0U) || (filter_storage != NULL)) && (node_id <= CANARD_NODE_ID_MAX);
     if (ok) {
         (void)memset(self, 0, sizeof(*self));
         self->node_id                   = (node_id <= CANARD_NODE_ID_MAX) ? node_id : CANARD_NODE_ID_ANONYMOUS;