Merge pull request #198 from Zarkopafilis/txq

[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

@@ -104,15 +104,12 @@ class UAVCAN_EXPORT INode
      *
      * Optional parameters:
      *
-     * @param qos               Quality of service. Please refer to the CAN IO manager for details.
-     *
      * @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,
                       CanIOFlags flags = 0)
     {
-        return getDispatcher().getCanIOManager().send(frame, tx_deadline, MonotonicTime(), iface_mask, qos, flags);
+        return getDispatcher().getCanIOManager().send(frame, tx_deadline, MonotonicTime(), iface_mask, flags);
     }
 
 #if !UAVCAN_TINY

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);
 
     MonotonicTime getTxDeadline() const;
 
@@ -93,12 +93,6 @@ class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase
                             ZeroTransferBuffer,
                             StaticTransferBuffer<BitLenToByteLen<DataStruct::MaxBitLen>::Result> >::Result Buffer;
 
-    enum
-    {
-        Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ?
-              CanTxQueue::Volatile : CanTxQueue::Persistent
-    };
-
     int checkInit();
 
     int doEncode(const DataStruct& message, ITransferBuffer& buffer) const;
@@ -157,7 +151,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());
 }
 
 template <typename DataSpec, typename DataStruct>

libuavcan/include/uavcan/transport/can_io.hpp

@@ -1,6 +1,7 @@
 /*
  * 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
@@ -10,6 +11,7 @@
 #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>
@@ -20,7 +22,6 @@
 
 namespace uavcan
 {
-
 struct UAVCAN_EXPORT CanRxFrame : public CanFrame
 {
     MonotonicTime ts_mono;
@@ -36,87 +37,81 @@ struct UAVCAN_EXPORT CanRxFrame : public CanFrame
 #endif
 };
 
-
-class UAVCAN_EXPORT CanTxQueue : Noncopyable
+struct UAVCAN_EXPORT CanTxQueueEntry  // Not required to be packed - fits the block in any case
 {
-public:
-    enum Qos { Volatile, Persistent };
+    MonotonicTime deadline;
+    const CanFrame frame;
+    CanIOFlags flags;
+
+    CanTxQueueEntry(const CanFrame& arg_frame, const MonotonicTime arg_deadline, CanIOFlags arg_flags)
+        : deadline(arg_deadline)
+        , frame(arg_frame)
+        , flags(arg_flags)
+    {
+        IsDynamicallyAllocatable<CanTxQueueEntry>::check();
+    }
+
+    static void destroy(CanTxQueueEntry*& obj, IPoolAllocator& allocator);
+
+    bool isExpired(const MonotonicTime timestamp) const { return timestamp > deadline; }
+
+    bool operator<(const CanTxQueueEntry& other) const
+    {
+        return this->frame.priorityLowerThan(other.frame);
+    }
+
+    bool operator>(const CanTxQueueEntry& other) const
+    {
+        return this->frame.priorityHigherThan(other.frame);
+    }
 
-    struct Entry : public LinkedListNode<Entry>  // Not required to be packed - fits the block in any case
+    bool operator==(const CanTxQueueEntry& other) const
     {
-        MonotonicTime deadline;
-        CanFrame frame;
-        uint8_t qos;
-        CanIOFlags flags;
-
-        Entry(const CanFrame& arg_frame, MonotonicTime arg_deadline, Qos arg_qos, CanIOFlags arg_flags)
-            : deadline(arg_deadline)
-            , frame(arg_frame)
-            , qos(uint8_t(arg_qos))
-            , flags(arg_flags)
-        {
-            UAVCAN_ASSERT((qos == Volatile) || (qos == Persistent));
-            IsDynamicallyAllocatable<Entry>::check();
-        }
-
-        static void destroy(Entry*& obj, IPoolAllocator& allocator);
-
-        bool isExpired(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)); }
+        return this->frame == other.frame;
+    }
 
 #if UAVCAN_TOSTRING
         std::string toString() const;
 #endif
-    };
+};
 
+class UAVCAN_EXPORT CanTxQueue : public AvlTree<CanTxQueueEntry>
+{
 private:
-    class PriorityInsertionComparator
-    {
-        const CanFrame& frm_;
-    public:
-        explicit PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { }
-        bool operator()(const Entry* entry)
-        {
-            UAVCAN_ASSERT(entry);
-            return frm_.priorityHigherThan(entry->frame);
-        }
-    };
+    void postOrderTraverseEntryCleanup(Node* n);
 
-    LinkedListRoot<Entry> queue_;
-    LimitedPoolAllocator allocator_;
+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)
-        , sysclock_(sysclock)
-        , rejected_frames_cnt_(0)
-    { }
+    CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t 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, 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 +125,6 @@ struct UAVCAN_EXPORT CanIfacePerfCounters
     { }
 };
 
-
 class UAVCAN_EXPORT CanIOManager : Noncopyable
 {
     struct IfaceFrameCounters
@@ -177,7 +171,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, 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,
              CanIOFlags flags, uint8_t iface_mask);
 
     void cleanup(MonotonicTime ts);

libuavcan/include/uavcan/transport/transfer_sender.hpp

@@ -24,7 +24,6 @@ class UAVCAN_EXPORT TransferSender
     Dispatcher& dispatcher_;
 
     TransferPriority priority_;
-    CanTxQueue::Qos qos_;
     TransferCRC crc_base_;
     DataTypeID data_type_id_;
     CanIOFlags flags_;
@@ -41,30 +40,28 @@ 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,
                    MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
         : max_transfer_interval_(max_transfer_interval)
         , dispatcher_(dispatcher)
         , priority_(TransferPriority::Default)
-        , qos_(CanTxQueue::Qos())
         , flags_(CanIOFlags(0))
         , iface_mask_(AllIfacesMask)
         , allow_anonymous_transfers_(false)
     {
-        init(data_type, qos);
+        init(data_type);
     }
 
     TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval())
         : max_transfer_interval_(max_transfer_interval)
         , dispatcher_(dispatcher)
         , priority_(TransferPriority::Default)
-        , qos_(CanTxQueue::Qos())
         , flags_(CanIOFlags(0))
         , iface_mask_(AllIfacesMask)
         , allow_anonymous_transfers_(false)
     { }
 
-    void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos);
+    void init(const DataTypeDescriptor& dtid);
 
     bool isInitialized() const { return data_type_id_ != DataTypeID(); }