Virtual CAN driver interface

libuavcan_drivers/linux/apps/test_multithreading.cpp

@@ -4,11 +4,222 @@
 
 #include <iostream>
 #include <thread>
+#include <condition_variable>
 #include <uavcan_linux/uavcan_linux.hpp>
 #include <uavcan/node/sub_node.hpp>
 #include <uavcan/protocol/node_status_monitor.hpp>
 #include "debug.hpp"
 
+/**
+ * Objects of this class are owned by the sub-node thread.
+ */
+class VirtualCanIface : public uavcan::ICanIface, uavcan::Noncopyable
+{
+    struct RxItem
+    {
+        const uavcan::CanRxFrame frame;
+        const uavcan::CanIOFlags flags;
+
+        RxItem(const uavcan::CanRxFrame& arg_frame, uavcan::CanIOFlags arg_flags) :
+            frame(arg_frame),
+            flags(arg_flags)
+        { }
+    };
+
+    std::mutex& mutex_;
+    uavcan::CanTxQueue tx_queue_;
+    std::queue<RxItem> rx_queue_;
+
+    int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime tx_deadline, uavcan::CanIOFlags flags) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        tx_queue_.push(frame, tx_deadline, uavcan::CanTxQueue::Volatile, flags);
+        return 1;
+    }
+
+    int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
+                    uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags) override
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+
+        if (rx_queue_.empty())
+        {
+            return 0;
+        }
+
+        const auto item = rx_queue_.front();
+        rx_queue_.pop();
+
+        out_frame = item.frame;
+        out_ts_monotonic = item.frame.ts_mono;
+        out_ts_utc = item.frame.ts_utc;
+        out_flags = item.flags;
+
+        return 1;
+    }
+
+    int16_t configureFilters(const uavcan::CanFilterConfig*, std::uint16_t) override { return -uavcan::ErrDriver; }
+    uint16_t getNumFilters() const override { return 0; }
+    uint64_t getErrorCount() const override { return 0; }
+
+    static unsigned computeTxQueuePoolQuota(uavcan::INode& node)
+    {
+        return node.getAllocator().getNumBlocks() / 4;
+    }
+
+public:
+    VirtualCanIface(uavcan::INode& node, std::mutex& arg_mutex) :
+        mutex_(arg_mutex),
+        tx_queue_(node.getAllocator(), node.getSystemClock(), computeTxQueuePoolQuota(node))
+    { }
+
+    /**
+     * Call this from the main thread only.
+     * No additional locking is required.
+     */
+    void addRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        rx_queue_.emplace(frame, flags);
+    }
+
+    /**
+     * Call this from the main thread only.
+     * No additional locking is required.
+     */
+    void flushTxQueueTo(uavcan::INode& main_node, std::uint8_t iface_index)
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+
+        const std::uint8_t iface_mask = static_cast<std::uint8_t>(1U << iface_index);
+
+        while (auto e = tx_queue_.peek())
+        {
+            const int res = main_node.injectTxFrame(e->frame, e->deadline, iface_mask,
+                                                    uavcan::CanTxQueue::Qos(e->qos), e->flags);
+            if (res <= 0)
+            {
+                break;
+            }
+            tx_queue_.remove(e);
+        }
+    }
+
+    /**
+     * Call this from the sub-node thread only.
+     * No additional locking is required.
+     */
+    bool hasDataInRxQueue() const
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return !rx_queue_.empty();
+    }
+};
+
+/**
+ * Objects of this class are owned by the sub-node thread.
+ */
+class VirtualCanDriver : public uavcan::ICanDriver, public uavcan::IRxFrameListener
+{
+    class Event
+    {
+        std::mutex m_;
+        std::condition_variable cv_;
+
+    public:
+        /**
+         * Note that this method may return spuriously.
+         */
+        void waitFor(uavcan::MonotonicDuration duration)
+        {
+            std::unique_lock<std::mutex> lk(m_);
+            (void)cv_.wait_for(lk, std::chrono::microseconds(duration.toUSec()));
+        }
+
+        void signal() { cv_.notify_all(); }
+    };
+
+    Event event_;
+    std::mutex mutex_;
+    uavcan::INode& sub_node_;
+    std::vector<std::shared_ptr<VirtualCanIface>> ifaces_;
+
+    uavcan::ICanIface* getIface(uint8_t iface_index) override
+    {
+        return ifaces_.at(iface_index).get();
+    }
+
+    uint8_t getNumIfaces() const override { return ifaces_.size(); }
+
+    /**
+     * This and other methods of ICanDriver will be invoked by the sub-node thread.
+     */
+    int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline) override
+    {
+        bool need_block = (inout_masks.write == 0);    // Write queue is infinite
+        for (unsigned i = 0; need_block && (i < ifaces_.size()); i++)
+        {
+            const bool need_read = inout_masks.read & (1U << i);
+            if (need_read && ifaces_[i]->hasDataInRxQueue())
+            {
+                need_block = false;
+            }
+        }
+
+        if (need_block)
+        {
+            event_.waitFor(blocking_deadline - sub_node_.getMonotonicTime());
+        }
+
+        inout_masks = uavcan::CanSelectMasks();
+        for (unsigned i = 0; i < ifaces_.size(); i++)
+        {
+            const std::uint8_t iface_mask = 1U << i;
+            inout_masks.write |= iface_mask;           // Always ready to write
+            if (ifaces_[i]->hasDataInRxQueue())
+            {
+                inout_masks.read |= iface_mask;
+            }
+        }
+
+        return ifaces_.size();       // We're always ready for write, hence > 0.
+    }
+
+    /**
+     * This handler will be invoked by the main node thread.
+     */
+    void handleRxFrame(const uavcan::CanRxFrame& frame, uavcan::CanIOFlags flags) override
+    {
+        ifaces_.at(frame.iface_index)->addRxFrame(frame, flags);
+        event_.signal();
+    }
+
+public:
+    VirtualCanDriver(uavcan::INode& arg_sub_node, unsigned num_ifaces) :
+        sub_node_(arg_sub_node)
+    {
+        for (unsigned i = 0; i < num_ifaces; i++)
+        {
+            ifaces_.emplace_back(new VirtualCanIface(arg_sub_node, mutex_));
+        }
+    }
+
+    /**
+     * This method should be invoked from the main node thread periodically in order to move contents of the
+     * sub-node's TX queues into the TX queues of the main node. A good practice is to call this method
+     * immediately after executing spinOnce() on the main node.
+     * No additional locking is required.
+     */
+    void flushTxQueuesTo(uavcan::INode& main_node)
+    {
+        for (unsigned i = 0; i < ifaces_.size(); i++)
+        {
+            ifaces_.at(i)->flushTxQueueTo(main_node, i);
+        }
+        event_.signal();
+    }
+};
+
 static uavcan_linux::NodePtr initMainNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid,
                                           const std::string& name)
 {