[Review & Discuss] Update TxQueue to have a backing AVL Tree (Issues #189 #195)

libuavcan/include/uavcan/driver/can.hpp

@@ -57,6 +57,16 @@ struct UAVCAN_EXPORT CanFrame
         return (id == rhs.id) && (dlc == rhs.dlc) && equal(data, data + dlc, rhs.data);
     }
 
+    bool operator<(const CanFrame & other) const
+    {
+        return this->priorityLowerThan(other);
+    }
+
+    bool operator>(const CanFrame & other) const
+    {
+        return this->priorityHigherThan(other);
+    }
+
     bool isExtended()                  const { return id & FlagEFF; }
     bool isRemoteTransmissionRequest() const { return id & FlagRTR; }
     bool isErrorFrame()                const { return id & FlagERR; }

libuavcan/include/uavcan/node/abstract_node.hpp

@@ -109,7 +109,7 @@ class UAVCAN_EXPORT INode
      * @param flags             CAN IO flags. Please refer to the CAN driver API for details.
      */
     int injectTxFrame(const CanFrame& frame, MonotonicTime tx_deadline, uint8_t iface_mask,
-                      CanTxQueue::Qos qos = CanTxQueue::Volatile,
+                      Qos qos = Qos::Volatile,
                       CanIOFlags flags = 0)
     {
         return getDispatcher().getCanIOManager().send(frame, tx_deadline, MonotonicTime(), iface_mask, qos, flags);

libuavcan/include/uavcan/node/generic_publisher.hpp

@@ -41,7 +41,7 @@ class GenericPublisherBase : Noncopyable
 
     bool isInited() const;
 
-    int doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos);
+    int doInit(DataTypeKind dtkind, const char* dtname, Qos qos);
 
     MonotonicTime getTxDeadline() const;
 
@@ -93,10 +93,11 @@ class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase
                             ZeroTransferBuffer,
                             StaticTransferBuffer<BitLenToByteLen<DataStruct::MaxBitLen>::Result> >::Result Buffer;
 
+
     enum
     {
-        Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
-              CanTxQueue::Volatile : CanTxQueue::Persistent
+        ExtractQosFromDataKind = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
+              Qos::Volatile : Qos::Persistent
     };
 
     int checkInit();
@@ -157,7 +158,8 @@ int GenericPublisher<DataSpec, DataStruct>::checkInit()
     {
         return 0;
     }
-    return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), CanTxQueue::Qos(Qos));
+
+    return doInit(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName(), Qos(ExtractQosFromDataKind));
 }
 
 template <typename DataSpec, typename DataStruct>

libuavcan/include/uavcan/transport/can_io.hpp

@@ -1,15 +1,18 @@
 /*
  * CAN bus IO logic.
  * Copyright (C) 2014 Pavel Kirienko <[email protected]>
+ * Copyright (C) 2019 Theodoros Ntakouris <[email protected]>
  */
 
 #ifndef UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED
 #define UAVCAN_TRANSPORT_CAN_IO_HPP_INCLUDED
 
 #include <cassert>
+#include <utility>
 #include <uavcan/error.hpp>
 #include <uavcan/std.hpp>
 #include <uavcan/util/linked_list.hpp>
+#include <uavcan/util/avl_tree.hpp>
 #include <uavcan/dynamic_memory.hpp>
 #include <uavcan/build_config.hpp>
 #include <uavcan/util/templates.hpp>
@@ -36,87 +39,83 @@ struct UAVCAN_EXPORT CanRxFrame : public CanFrame
 #endif
 };
 
+enum Qos { Volatile, Persistent };
 
-class UAVCAN_EXPORT CanTxQueue : Noncopyable
+struct CanTxQueueEntry  // Not required to be packed - fits the block in any case
 {
-public:
-    enum Qos { Volatile, Persistent };
-
-    struct Entry : public LinkedListNode<Entry>  // Not required to be packed - fits the block in any case
-    {
-        MonotonicTime deadline;
-        CanFrame frame;
-        uint8_t qos;
-        CanIOFlags flags;
+    MonotonicTime deadline;
+    const CanFrame frame;
+    uint8_t qos;
+    CanIOFlags flags;
 
-        Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags)
-            : deadline(arg_deadline)
+    CanTxQueueEntry(const CanFrame& arg_frame, const MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags)
+            : deadline(std::move(arg_deadline))
             , frame(arg_frame)
             , qos(uint8_t(arg_qos))
             , flags(arg_flags)
-        {
-            UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent));
-            IsDynamicallyAllocatable<Entry>::check();
-        }
+    {
+        UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent));
+        IsDynamicallyAllocatable<CanTxQueueEntry>::check();
+    }
 
-        static void destroy(Entry*& obj, IPoolAllocator& allocator);
+    static void destroy(CanTxQueueEntry*& obj, IPoolAllocator& allocator);
 
-        bool isExpired(MonotonicTime timestamp) const { return timestamp > deadline; }
+    bool isExpired(const MonotonicTime timestamp) const { return timestamp > deadline; }
 
-        bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
-        bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
-        bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); }
-        bool qosLowerThan(const Entry& rhs)  const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); }
+    bool operator<(const CanTxQueueEntry & other) const
+    {
+        return this->frame.priorityLowerThan(other.frame);
+    }
 
-#if UAVCAN_TOSTRING
-        std::string toString() const;
-#endif
-    };
+    bool operator>(const CanTxQueueEntry & other) const
+    {
+        return this->frame.priorityHigherThan(other.frame);
+    }
 
-private:
-    class PriorityInsertionComparator
+    bool operator==(const CanTxQueueEntry & other) const
     {
-        const CanFrame& frm_;
-    public:
-        explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { }
-        bool operator()(const Entry* entry)
-        {
-            UAVCAN_ASSERT(entry);
-            return frm_.priorityHigherThan(entry->frame);
-        }
-    };
+        return this->frame == other.frame;
+    }
 
-    LinkedListRoot<Entry> queue_;
-    LimitedPoolAllocator allocator_;
+#if UAVCAN_TOSTRING
+    std::string toString() const;
+#endif
+};
+
+class UAVCAN_EXPORT CanTxQueue : public AvlTree<CanTxQueueEntry>
+{
+protected:
     ISystemClock& sysclock_;
     uint32_t rejected_frames_cnt_;
 
-    void registerRejectedFrame();
+    bool linkedListContains(Node *head, const CanFrame &frame) const;
+    void safeIncrementRejectedFrames();
+    AvlTree::Node *searchForNonExpiredMax(Node *n);
 
 public:
     CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota)
-        : allocator_(allocator, allocator_quota)
+        : AvlTree(allocator, allocator_quota)
         , sysclock_(sysclock)
         , rejected_frames_cnt_(0)
-    { }
+    {}
 
-    ~CanTxQueue();
+    ~CanTxQueue() override;
 
-    void push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags);
+    /* Avl Tree allocates the AvlTree::Node, while this(CanTxQueue) allocates the CanTxQueueEntry
+     * Same logic for removal. */
+    void push(const CanFrame &frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags);
+    void remove(CanTxQueueEntry *entry);
 
-    Entry* peek();               // Modifier
-    void remove(Entry*& entry);
-    const CanFrame* getTopPriorityPendingFrame() const;
+    uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }
 
-    /// The 'or equal' condition is necessary to avoid frame reordering.
-    bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const;
+    bool contains(const CanFrame &frame) const;
 
-    uint32_t getRejectedFrameCount() const { return rejected_frames_cnt_; }
+    /* Tries to look up rightmost Node. If the frame is expired, removes it and continues traversing */
+    CanTxQueueEntry *peek();
 
-    bool isEmpty() const { return queue_.isEmpty(); }
+    bool topPriorityHigherOrEqual(const CanFrame &rhs_frame);
 };
 
-
 struct UAVCAN_EXPORT CanIfacePerfCounters
 {
     uint64_t frames_tx;
@@ -130,7 +129,6 @@ struct UAVCAN_EXPORT CanIfacePerfCounters
     { }
 };
 
-
 class UAVCAN_EXPORT CanIOManager : Noncopyable
 {
     struct IfaceFrameCounters
@@ -177,7 +175,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
      *  negative - failure
      */
     int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
-             uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
+             uint8_t iface_mask, Qos qos, CanIOFlags flags);
     int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags);
 };
 

libuavcan/include/uavcan/transport/dispatcher.hpp

@@ -157,7 +157,7 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable
     /**
      * Refer to CanIOManager::send() for the parameter description
      */
-    int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos,
+    int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, Qos qos,
              CanIOFlags flags, uint8_t iface_mask);
 
     void cleanup(MonotonicTime ts);

libuavcan/include/uavcan/transport/transfer_sender.hpp

@@ -24,7 +24,7 @@ class UAVCAN_EXPORT TransferSender
     Dispatcher& dispatcher_;
 
     TransferPriority priority_;
-    CanTxQueue::Qos qos_;
+    Qos qos_;
     TransferCRC crc_base_;
     DataTypeID data_type_id_;
     CanIOFlags flags_;
@@ -41,12 +41,12 @@ class UAVCAN_EXPORT TransferSender
         return MonotonicDuration::fromMSec(60 * 1000);
     }
 
-    TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos,
+    TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, Qos qos,
                    MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
         : max_transfer_interval_(max_transfer_interval)
         , dispatcher_(dispatcher)
         , priority_(TransferPriority::Default)
-        , qos_(CanTxQueue::Qos())
+        , qos_(Qos())
         , flags_(CanIOFlags(0))
         , iface_mask_(AllIfacesMask)
         , allow_anonymous_transfers_(false)
@@ -58,13 +58,13 @@ class UAVCAN_EXPORT TransferSender
         : max_transfer_interval_(max_transfer_interval)
         , dispatcher_(dispatcher)
         , priority_(TransferPriority::Default)
-        , qos_(CanTxQueue::Qos())
+        , qos_(Qos())
         , flags_(CanIOFlags(0))
         , iface_mask_(AllIfacesMask)
         , allow_anonymous_transfers_(false)
     { }
 
-    void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos);
+    void init(const DataTypeDescriptor& dtid, Qos qos);
 
     bool isInitialized() const { return data_type_id_ != DataTypeID(); }