fix(uavcan): correct TX queue block accounting and report queue pressure (#28395)

* fix(uavcan): count a TX queue block only once the pool provides it

LimitedPoolAllocator::allocate() incremented used_blocks_ before asking the
underlying allocator, so an allocation the pool could not satisfy still spent
quota. Nothing ever hands that block back, because deallocate() only runs for a
pointer that was returned, so every queue that asked for memory while the pool
was empty stays permanently smaller than its configured limit.

The pool is shared between the RX side and one TX queue per interface, so it
does empty transiently under load -- a DroneCAN node firmware update is enough.
The damage accumulates and is only cleared by restarting the node.

* feat(uavcan): report TX queue depth and why frames were dropped

`uavcan status` gains, per interface:

	TX queue peak: 44/84 blocks
	TX rejected:   80 frames (78 expired, 2 no memory)

CanTxQueue already counted rejected frames but tx_queues_ is private and
CanIOManager exposed no accessor, so the count was unreachable. It also conflated
three causes: a frame handed over with its deadline already passed, a queued frame
evicted as expired to make room, and a frame dropped because the queue was at its
block limit. Only the last two are memory pressure, and they call for opposite
responses, so a single count sends you the wrong way.

The peak is what makes the count actionable. The limit is
pool_soft / (num_ifaces + 1) + 1, derived from the soft capacity in
allocator.hpp, so a peak sitting at the limit says raising that capacity will
help and a peak well under it says the frames are ageing out for some other
reason and a larger pool will not.

Measured on an FMU-v6XRT serving a DroneCAN node firmware update: one node holds
44 of 84 blocks and drops nothing, another pins the limit and drops hundreds. The
counts alone did not distinguish those.

src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/dynamic_memory.hpp

@@ -114,12 +114,14 @@ class LimitedPoolAllocator : public IPoolAllocator
     IPoolAllocator& allocator_;
     const uint16_t max_blocks_;
     uint16_t used_blocks_;
+    uint16_t peak_used_blocks_;
 
 public:
     LimitedPoolAllocator(IPoolAllocator& allocator, std::size_t max_blocks)
         : allocator_(allocator)
         , max_blocks_(static_cast<uint16_t>(min<std::size_t>(max_blocks, 0xFFFFU)))
         , used_blocks_(0)
+        , peak_used_blocks_(0)
     {
         UAVCAN_ASSERT(max_blocks_ > 0);
     }
@@ -128,6 +130,13 @@ public:
     virtual void deallocate(const void* ptr) override;
 
     virtual uint16_t getBlockCapacity() const override;
+
+    uint16_t getNumUsedBlocks() const { return used_blocks_; }
+
+    /// High water mark. Reaching getBlockLimit() is what turns into a rejected frame.
+    uint16_t getPeakNumUsedBlocks() const { return peak_used_blocks_; }
+
+    uint16_t getBlockLimit() const { return max_blocks_; }
 };
 
 // ----------------------------------------------------------------------------

src/drivers/uavcan/libdronecan/libuavcan/include/uavcan/transport/can_io.hpp

@@ -90,14 +90,20 @@ private:
     LimitedPoolAllocator allocator_;
     ISystemClock& sysclock_;
     uint32_t rejected_frames_cnt_;
+    uint32_t rejected_expired_cnt_;
+    uint32_t rejected_oom_cnt_;
 
     void registerRejectedFrame();
+    void registerExpiredFrame();
+    void registerOutOfMemoryFrame();
 
 public:
     CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota)
         : allocator_(allocator, allocator_quota)
         , sysclock_(sysclock)
         , rejected_frames_cnt_(0)
+        , rejected_expired_cnt_(0)
+        , rejected_oom_cnt_(0)
     { }
 
     ~CanTxQueue();
@@ -113,6 +119,15 @@ public:
 
     uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }
 
+    /// Frames dropped because their transmit deadline had already passed. Not a memory shortage.
+    uint32_t getExpiredFrameCount() const { return rejected_expired_cnt_; }
+
+    /// Frames dropped with the queue out of blocks, whether this frame lost QoS arbitration or evicted one.
+    uint32_t getOutOfMemoryFrameCount() const { return rejected_oom_cnt_; }
+
+    uint16_t getPeakNumUsedBlocks() const { return allocator_.getPeakNumUsedBlocks(); }
+    uint16_t getBlockLimit() const { return allocator_.getBlockLimit(); }
+
     bool isEmpty() const { return queue_.isEmpty(); }
 };
 
@@ -131,6 +146,31 @@ struct UAVCAN_EXPORT CanIfacePerfCounters
 };
 
 
+/**
+ * State of one interface's TX queue. A rejected frame is one the queue could not hold: its deadline
+ * had already passed, or the queue was at its block limit and QoS arbitration found nothing to evict.
+ * Dropping a single frame loses the whole multi-frame transfer it belonged to, which the peer sees as
+ * a timeout. peak_used_blocks against block_limit says whether the limit is what is doing the
+ * rejecting - the limit is derived from the pool's soft capacity, so it is raised by raising that.
+ */
+struct UAVCAN_EXPORT CanTxQueuePerfCounters
+{
+    uint32_t rejected_frames;
+    uint32_t expired_frames;
+    uint32_t out_of_memory_frames;
+    uint16_t peak_used_blocks;
+    uint16_t block_limit;
+
+    CanTxQueuePerfCounters()
+        : rejected_frames(0)
+        , expired_frames(0)
+        , out_of_memory_frames(0)
+        , peak_used_blocks(0)
+        , block_limit(0)
+    { }
+};
+
+
 class UAVCAN_EXPORT CanIOManager : Noncopyable
 {
     struct IfaceFrameCounters
@@ -165,6 +205,8 @@ public:
 
     CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const;
 
+    CanTxQueuePerfCounters getTxQueuePerfCounters(uint8_t iface_index) const;
+
     const ICanDriver& getCanDriver() const { return driver_; }
     ICanDriver& getCanDriver()             { return driver_; }
 

src/drivers/uavcan/libdronecan/libuavcan/src/transport/uc_can_io.cpp

@@ -105,6 +105,26 @@ void CanTxQueue::registerRejectedFrame()
     }
 }
 
+void CanTxQueue::registerExpiredFrame()
+{
+    registerRejectedFrame();
+
+    if (rejected_expired_cnt_ < NumericTraits<uint32_t>::max())
+    {
+        rejected_expired_cnt_++;
+    }
+}
+
+void CanTxQueue::registerOutOfMemoryFrame()
+{
+    registerRejectedFrame();
+
+    if (rejected_oom_cnt_ < NumericTraits<uint32_t>::max())
+    {
+        rejected_oom_cnt_++;
+    }
+}
+
 void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags)
 {
     const MonotonicTime timestamp = sysclock_.getMonotonic();
@@ -112,7 +132,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
     if (timestamp >= tx_deadline)
     {
         UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
-        registerRejectedFrame();
+        registerExpiredFrame();
         return;
     }
 
@@ -128,7 +148,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
             if (p->isExpired(timestamp))
             {
                 UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str());
-                registerRejectedFrame();
+                registerExpiredFrame();
                 remove(p);
             }
             p = next;
@@ -139,7 +159,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos,
     if (praw == UAVCAN_NULLPTR)
     {
         UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration");
-        registerRejectedFrame();
+        registerOutOfMemoryFrame();
 
         // Find a frame with lowest QoS
         Entry* p = queue_.get();
@@ -335,6 +355,22 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con
     return cnt;
 }
 
+CanTxQueuePerfCounters CanIOManager::getTxQueuePerfCounters(uint8_t iface_index) const
+{
+    if (iface_index >= num_ifaces_ || iface_index >= MaxCanIfaces || !tx_queues_[iface_index].isConstructed())
+    {
+        UAVCAN_ASSERT(0);
+        return CanTxQueuePerfCounters();
+    }
+    CanTxQueuePerfCounters cnt;
+    cnt.rejected_frames = tx_queues_[iface_index]->getRejectedFrameCount();
+    cnt.expired_frames = tx_queues_[iface_index]->getExpiredFrameCount();
+    cnt.out_of_memory_frames = tx_queues_[iface_index]->getOutOfMemoryFrameCount();
+    cnt.peak_used_blocks = tx_queues_[iface_index]->getPeakNumUsedBlocks();
+    cnt.block_limit = tx_queues_[iface_index]->getBlockLimit();
+    return cnt;
+}
+
 int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
                        uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
 {

src/drivers/uavcan/libdronecan/libuavcan/src/uc_dynamic_memory.cpp

@@ -11,15 +11,29 @@ namespace uavcan
  */
 void* LimitedPoolAllocator::allocate(std::size_t size)
 {
-    if (used_blocks_ < max_blocks_)
-    {
-        used_blocks_++;
-        return allocator_.allocate(size);
-    }
-    else
+    if (used_blocks_ >= max_blocks_)
     {
         return UAVCAN_NULLPTR;
     }
+
+    /*
+     * Counting the block only once the underlying allocator has actually produced one. Counting the
+     * attempt instead spends quota on a block that deallocate() will never return, so a pool that is
+     * momentarily empty permanently shrinks every queue that asked it for memory while it was.
+     */
+    void* const praw = allocator_.allocate(size);
+
+    if (praw != UAVCAN_NULLPTR)
+    {
+        used_blocks_++;
+
+        if (used_blocks_ > peak_used_blocks_)
+        {
+            peak_used_blocks_ = used_blocks_;
+        }
+    }
+
+    return praw;
 }
 
 void LimitedPoolAllocator::deallocate(const void* ptr)

src/drivers/uavcan/uavcan_main.cpp

@@ -1295,6 +1295,11 @@ UavcanNode::print_info()
 			printf("\tIO errors: %" PRIu64 "\n", iface_perf_cnt.errors);
 			printf("\tRX frames: %" PRIu64 "\n", iface_perf_cnt.frames_rx);
 			printf("\tTX frames: %" PRIu64 "\n", iface_perf_cnt.frames_tx);
+
+			auto txq = _node.getDispatcher().getCanIOManager().getTxQueuePerfCounters(i);
+			printf("\tTX queue peak: %" PRIu16 "/%" PRIu16 " blocks\n", txq.peak_used_blocks, txq.block_limit);
+			printf("\tTX rejected:   %" PRIu32 " frames (%" PRIu32 " expired, %" PRIu32 " no memory)\n",
+			       txq.rejected_frames, txq.expired_frames, txq.out_of_memory_frames);
 		}
 	}