Update but not tested yet

Author: pavel-kirienko

libcanard/canard.c

@@ -75,6 +75,8 @@ typedef unsigned char byte_t;
 
 #define FOREACH_IFACE(i) for (size_t i = 0; i < CANARD_IFACE_COUNT; i++)
 
+#define TREE_NULL (canard_tree_t){ NULL, { NULL, NULL }, 0 }
+
 typedef enum transfer_kind_t
 {
     transfer_kind_message     = 0,
@@ -521,7 +523,8 @@ struct canard_txfer_t
     /// Memory-efficient but not very fast indexes optimized for a low number of pending transfers.
     canard_listed_t list_pending[CANARD_IFACE_COUNT];
     canard_listed_t list_delayed;
-    canard_listed_t list_oldest;
+    canard_listed_t list_agewise;
+    canard_tree_t   index_reliable; ///< Ordered by (topic hash, transfer-ID), contains ALL pending reliable transfers.
 
     /// Mutable transmission state. All other fields, except for the index handles, are immutable.
     ///
@@ -576,8 +579,9 @@ static canard_txfer_t* txfer_new(const canard_mem_t          mem,
         FOREACH_IFACE (i) {
             tr->list_pending[i] = LIST_NULL;
         }
-        tr->list_delayed = LIST_NULL;
-        tr->list_oldest  = LIST_NULL;
+        tr->list_delayed   = LIST_NULL;
+        tr->list_agewise   = LIST_NULL;
+        tr->index_reliable = TREE_NULL;
         //
         FOREACH_IFACE (i) {
             tr->head[i] = tr->cursor[i] = NULL;
@@ -686,8 +690,8 @@ static void txfer_retire(canard_t* const self, canard_txfer_t* const tr, const b
     const bool                  backlogged   = txfer_is_backlogged(tr);
 
     // Delist everywhere. Remember that delisting a non-listed entity is a safe no-op.
-    if (self->tx.iterator[reliable] == tr) {
-        self->tx.iterator[reliable] = LIST_NEXT(tr, canard_txfer_t, list_oldest); // May be NULL, is OK.
+    if (self->tx.iter == tr) {
+        self->tx.iter = LIST_NEXT(tr, canard_txfer_t, list_agewise); // May be NULL, is OK.
     }
     FOREACH_IFACE (i) {
         if ((tr->iface_bitmap & (1U << i)) != 0) {
@@ -698,8 +702,12 @@ static void txfer_retire(canard_t* const self, canard_txfer_t* const tr, const b
             }
         }
     }
-    delist(&self->tx.oldest[reliable], &tr->list_oldest);
     delist(&self->tx.delayed[shard], &tr->list_delayed);
+    delist(&self->tx.agewise, &tr->list_agewise);
+    if (reliable) {
+        CANARD_ASSERT(cavl2_is_inserted(self->tx.reliable, &tr->index_reliable));
+        cavl2_remove(&self->tx.reliable, &tr->index_reliable);
+    }
 
     // If the transfer was not backlogged, it may have been blocking other transfers, but only if it is reliable.
     // Best-effort transfers do not block anything because in the presence of a stalled interface they may take a
@@ -894,20 +902,32 @@ static tx_frame_t* tx_spool_v0(canard_t* const            self,
     return head;
 }
 
+/// To check all transfers under a specific topic hash, use lower bound lookup with transfer_id=0 and
+/// then iterate until the topic hash changes.
+/// This is O(log n) because the number of distinct transfer-ID in Cyphal/CAN is bounded at a low value.
+typedef struct
+{
+    uint64_t topic_hash;
+    byte_t   transfer_id;
+} txfer_key_t;
+
+static int32_t tx_cavl_compare_reliable(const void* const user, const canard_tree_t* const node)
+{
+    const txfer_key_t* const key = (const txfer_key_t*)user;
+    const canard_txfer_t* const tr = CAVL2_TO_OWNER(node, canard_txfer_t, index_reliable); // clang-format off
+    if (key->topic_hash < tr->topic_hash) { return -1; }
+    if (key->topic_hash > tr->topic_hash) { return +1; } // clang-format on
+    return (int32_t)key->transfer_id - (int32_t)tr->remote_transfer_id;
+}
+
 /// When the queue is exhausted, finds a transfer to sacrifice using simple heuristics and returns it.
 /// Will return NULL if there are no transfers worth sacrificing (no queue space can be reclaimed).
 /// We cannot simply stop accepting new transfers when the queue is full, because it may be caused by a single
 /// stalled interface holding back progress for all transfers.
 /// The heuristics are subject to review and improvement.
 static canard_txfer_t* tx_sacrifice(const canard_t* const self)
 {
-    // A best-effort transfer can be stuck for a long time if there is a stalled redundant interface.
-    // A single stalled interface does not affect reliable transfers because the first ack will free them.
-    canard_txfer_t* tr = LIST_HEAD(self->tx.oldest[0], canard_txfer_t, list_oldest); // best-effort first
-    if (tr == NULL) {
-        tr = LIST_HEAD(self->tx.oldest[1], canard_txfer_t, list_oldest);
-    }
-    return tr;
+    return LIST_HEAD(self->tx.agewise, canard_txfer_t, list_agewise);
 }
 
 /// True on success, false if not possible to reclaim enough space.
@@ -978,6 +998,21 @@ static bool tx_push(canard_t* const            self,
     // if they have the same arbitration priority as the new transfer, they should get a chance to go first.
     tx_promote_delayed(self, now);
 
+    // Ensure there are no duplicate reliable transfers with the same (topic hash, transfer-ID).
+    // Insert at the same time to avoid double tree walk.
+    if (tr->reliable) {
+        const txfer_key_t     key = { .topic_hash = tr->topic_hash, .transfer_id = tr->transfer_id };
+        canard_txfer_t* const rel = CAVL2_TO_OWNER(
+          cavl2_find_or_insert(&self->tx.reliable, &key, tx_cavl_compare_reliable, tr, cavl2_trivial_factory),
+          canard_txfer_t,
+          index_reliable);
+        if (rel != tr) { // Duplicate found.
+            mem_free(self->mem.tx_transfer, sizeof(canard_txfer_t), tr);
+            self->err.tx_duplicate++;
+            return false;
+        }
+    }
+
     // Ensure the queue has enough space. v0 transfers always use Classic CAN regardless of tr->fd.
     const size_t mtu      = tr->fd ? CANARD_MTU_CAN_FD : CANARD_MTU_CAN_CLASSIC;
     const size_t size     = bytes_chain_size(payload); // TODO: pass the precomputed size into spool functions
@@ -986,6 +1021,10 @@ static bool tx_push(canard_t* const            self,
     if (!tx_ensure_queue_space(self, n_frames)) {
         mem_free(self->mem.tx_transfer, sizeof(canard_txfer_t), tr);
         self->err.tx_capacity++;
+        if (tr->reliable) {
+            CANARD_ASSERT(cavl2_is_inserted(self->tx.reliable, &tr->index_reliable));
+            cavl2_remove(&self->tx.reliable, &tr->index_reliable);
+        }
         return false;
     }
 
@@ -996,6 +1035,10 @@ static bool tx_push(canard_t* const            self,
     if (spool == NULL) {
         mem_free(self->mem.tx_transfer, sizeof(canard_txfer_t), tr);
         self->err.oom++;
+        if (tr->reliable) {
+            CANARD_ASSERT(cavl2_is_inserted(self->tx.reliable, &tr->index_reliable));
+            cavl2_remove(&self->tx.reliable, &tr->index_reliable);
+        }
         return false;
     }
     CANARD_ASSERT((self->tx.queue_size - queue_size_before) == n_frames);
@@ -1011,8 +1054,8 @@ static bool tx_push(canard_t* const            self,
         }
     }
 
-    // Insert into the oldest list.
-    enlist_tail(&self->tx.oldest[tr->reliable], &tr->list_oldest);
+    // Register the transfer.
+    enlist_tail(&self->tx.agewise, &tr->list_agewise);
 
     // We need to ensure that transfers emitted at the same priority level are send strictly in push order.
     // For best-effort transfers this is trivial as all we need to do is to enqueue them as-is.
@@ -1028,29 +1071,23 @@ static bool tx_push(canard_t* const            self,
     // If there are no such transfers, it can be enqueued immediately; observe that there may still be some reliable
     // transfers waiting for ack, but if such transfers are neither pending nor delayed, it means that they have
     // completed the last transmission attempt, no further attempts will be made, and thus no reordering can occur.
-    const byte_t shard                  = txfer_shard(tr);
-    bool         has_preceding_reliable = false;
-    FOREACH_IFACE (i) {
-        LIST_FIND_FIRST(self->tx.pending[shard][i],
-                        canard_txfer_t,
-                        list_pending[i], // check if this is standard-compliant
-                        match,
-                        (match->topic_hash == tr->topic_hash) && match->reliable);
-        if (match != NULL) {
-            has_preceding_reliable = true;
-            break;
+    bool has_preceding_reliable = false;
+    {
+        const txfer_key_t key = { .topic_hash = tr->topic_hash, .transfer_id = 0 };
+        canard_txfer_t*   rel = CAVL2_TO_OWNER(
+          cavl2_lower_bound(self->tx.reliable, &key, tx_cavl_compare_reliable), canard_txfer_t, index_reliable);
+        while ((rel != NULL) && (rel->topic_hash == tr->topic_hash)) {
+            // Ignore ourselves and transfers that will no longer be retransmitted.
+            if ((rel != tr) && (rel->delayed_until > BIG_BANG)) {
+                has_preceding_reliable = true;
+                break;
+            }
+            rel = CAVL2_TO_OWNER(cavl2_next_greater(&rel->index_reliable), canard_txfer_t, index_reliable);
         }
     }
-    if (!has_preceding_reliable) {
-        LIST_FIND_FIRST(self->tx.delayed[shard],
-                        canard_txfer_t,
-                        list_delayed,
-                        match,
-                        (match->topic_hash == tr->topic_hash) && match->reliable);
-        has_preceding_reliable = (match != NULL);
-    }
 
     // Schedule for transmission or backlog depending on the findings above.
+    const byte_t shard = txfer_shard(tr);
     if (has_preceding_reliable) { // into the backlog you go, buddy
         tr->delayed_until = HEAT_DEATH;
         enlist_tail(&self->tx.delayed[shard], &tr->list_delayed); // FIFO, newest at the tail.
@@ -1069,20 +1106,22 @@ static bool tx_push(canard_t* const            self,
 }
 
 /// Handle an ACK received from a remote node.
-static void tx_receive_ack(canard_t* const self, const uint64_t topic_hash, const byte_t transfer_id)
+static void tx_receive_ack(canard_t* const self, const uint64_t topic_hash_lower_bound, const byte_t transfer_id)
 {
-    // Scan from oldest because they are the most likely to match.
-    // We are not expected to hold a large number of pending reliable transfers, so a linear search is acceptable --
-    // up to a couple dozen transfers should be at least on par with a BST lookup.
-    // If this ever becomes a problem, we can add a separate index for pending reliable transfers keyed by topic hash.
-    LIST_FIND_FIRST(self->tx.oldest[1], // Search reliable only; best-effort transfers are not in this list.
-                    canard_txfer_t,
-                    list_oldest,
-                    tr, // Backlogged transfers may possibly have conflicting transfer-ID.
-                    (tr->topic_hash == topic_hash) && (tr->transfer_id == transfer_id) && !txfer_is_backlogged(tr));
-    if (tr != NULL) {
-        CANARD_ASSERT(tr->reliable && (tr->topic_hash == topic_hash) && (tr->transfer_id == transfer_id));
-        txfer_retire(self, tr, true);
+    const txfer_key_t key = { .topic_hash = topic_hash_lower_bound, .transfer_id = 0 };
+    canard_txfer_t*   tr  = CAVL2_TO_OWNER(cavl2_lower_bound(self->tx.reliable, &key, tx_cavl_compare_reliable), //
+                                        canard_txfer_t,
+                                        index_reliable);
+    if ((tr == NULL) || ((tr->topic_hash & CANARD_P2P_TOPIC_HASH_LOWER_BOUND_MASK) != topic_hash_lower_bound)) {
+        return;
+    }
+    // Linear scan to find the matching transfer ID. This is bounded by the max transfer ID count (32).
+    while ((tr != NULL) && ((tr->topic_hash & CANARD_P2P_TOPIC_HASH_LOWER_BOUND_MASK) == topic_hash_lower_bound)) {
+        if (tr->remote_transfer_id == transfer_id) { // Found!
+            txfer_retire(self, tr, true);
+            break;
+        }
+        tr = CAVL2_TO_OWNER(cavl2_next_greater(&tr->index_reliable), canard_txfer_t, index_reliable);
     }
 }
 

libcanard/canard.h

@@ -106,6 +106,10 @@ extern "C"
 
 #define CANARD_P2P_TOPIC_HASH_MSb_COUNT 36U
 
+#define CANARD_P2P_TOPIC_HASH_LOWER_BOUND_MASK                                                        \
+    (((((1ULL << CANARD_P2P_TOPIC_HASH_MSb_COUNT) - 1U) << (64U - CANARD_P2P_TOPIC_HASH_MSb_COUNT)) | \
+      CANARD_SUBJECT_ID_MAX_1v0))
+
 /// See canard_t::ack_baseline_timeout.
 /// This default value might be a good starting point for many applications.
 /// The baseline timeout should be greater than the expected round-trip time (RTT) for a message at the highest
@@ -292,8 +296,8 @@ typedef struct canard_vtable_t
     /// A new P2P message is received.
     ///
     /// The topic hash bound is less than or equal the true topic hash, which enables easy lookup using lower bounds.
-    /// The CANARD_P2P_TOPIC_HASH_MSb_COUNT MSb and 13 LSb of the topic hash are provided in the lower bound,
-    /// with the intermediate bits zeroed.
+    /// Use CANARD_P2P_TOPIC_HASH_LOWER_BOUND_MASK when comparing the lower bound to extract the relevant bits;
+    /// the irrelevant bits are guaranteed to be zero (hence why it is a lower bound).
     /// For pinned topics, the lower bound does not exceed CANARD_SUBJECT_ID_MAX_1v0 and exactly equals the subject-ID.
     ///
     /// The handler takes ownership of the payload; it must free it after use using the corresponding memory resource.
@@ -404,6 +408,11 @@ struct canard_t
         /// by having multiple queues, one per TX shard. This does not follow the full CAN ID arbitration order,
         /// but it is sufficient because the leading sharding bits of the CAN ID provide sufficient selectivity.
         ///
+        /// The linear lookup complexity is not considered an issue because real-time applications always have a bound
+        /// on the peak number of simultaneously pending transfers, which is typically low due to the limited bus
+        /// capacity; given the fixed bound, the asymptotic complexity is essentially constant time. If performance
+        /// becomes an issue in larger applications, it can be addressed by sharding the lists further.
+        ///
         /// The pending list contains transfers that are ready to be transmitted immediately.
         /// The delayed list contains transfers that will become eligible for transmission in the future;
         /// these are either reliable transfers waiting for their next retransmission attempt unless acknowledged,
@@ -414,17 +423,18 @@ struct canard_t
         /// The shards are based on the most significant bits of the CAN ID, meaning that the lowest index shard
         /// has the highest arbitration priority and should be chosen for transmission first. There is no ordering
         /// preservation guarantee between priority levels (obviously) so they all are treated as independent lists.
-        /// The pending shards bitmap is used for O(1) fetching of the highes-priority non-empty shard during poll()
+        /// The pending shards bitmap is used for O(1) fetching of the highest-priority non-empty shard during poll()
         /// -- no scanning needed.
         ///
         /// Unlike Cyphal/UDP, we don't have a dedicated deadline index, again to conserve memory. Instead, we do
         /// iterative scanning during poll() by storing the iteration cursors here. They are invalidated as needed.
         uint32_t      pending_shards_bitmap[CANARD_IFACE_COUNT]; ///< Bit index corresponds to non-empty pending shards.
         canard_list_t pending[CANARD_TX_SHARDS][CANARD_IFACE_COUNT]; ///< Next to transmit at the head.
-        canard_list_t delayed[CANARD_TX_SHARDS]; ///< Soonest retry time at the head. HEAT_DEATH if backlogged, at tail.
-        canard_list_t oldest[2];                 ///< ALL transfers, oldest at head, sharded by QoS (1=reliable).
+        canard_list_t delayed[CANARD_TX_SHARDS]; ///< Soonest retry time at head. HEAT_DEATH if backlogged, at tail.
+        canard_list_t agewise;                   ///< ALL transfers, oldest at the head.
+        canard_tree_t* reliable;                 ///< All reliable transfers, ordered by (topic hash, transfer-ID).
 
-        canard_txfer_t* iterator[2]; ///< For iterative poll() scanning, sharded by QoS like oldest; NULL to restart.
+        canard_txfer_t* iter; ///< For iterative poll() scanning; NULL to restart.
     } tx;
 
     struct
@@ -445,6 +455,7 @@ struct canard_t
         uint64_t tx_capacity;   ///< A transfer could not be enqueued due to queue capacity limit.
         uint64_t tx_sacrifice;  ///< A transfer had to be sacrificed to make room for a new transfer.
         uint64_t tx_expiration; ///< A transfer had to be dequeued due to deadline expiration.
+        uint64_t tx_duplicate;  ///< A transfer with the same topic hash and transfer-ID is already pending.
         uint64_t rx_frame;      ///< A received frame was malformed and thus dropped.
         uint64_t rx_transfer;   ///< A transfer could not be reassembled correctly.
     } err;