Dispatcher class, tests to come

libuavcan/include/uavcan/internal/transport/dispatcher.hpp

@@ -0,0 +1,93 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <stdint.h>
+#include <uavcan/internal/transport/transfer_listener.hpp>
+#include <uavcan/internal/transport/outgoing_transfer_registry.hpp>
+#include <uavcan/internal/transport/can_io.hpp>
+#include <uavcan/internal/linked_list.hpp>
+
+namespace uavcan
+{
+
+class Dispatcher : Noncopyable
+{
+    CanIOManager canio_;
+    ISystemClock* const sysclock_;
+    IOutgoingTransferRegistry* const outgoing_transfer_reg_;
+
+    class ListenerRegister
+    {
+        LinkedListRoot<TransferListenerBase> list_;
+
+        class DataTypeIDInsertionComparator
+        {
+            const uint16_t id_;
+        public:
+            DataTypeIDInsertionComparator(uint16_t id) : id_(id) { }
+            bool operator()(const TransferListenerBase* listener) const
+            {
+                assert(listener);
+                return id_ > listener->getDataTypeDescriptor()->id;
+            }
+        };
+
+    public:
+        enum Mode { UNIQUE_LISTENER, MANY_LISTENERS };
+
+        bool add(TransferListenerBase* listener, Mode mode);
+        void remove(TransferListenerBase* listener);
+        void cleanup(uint64_t ts_monotonic);
+        void handleFrame(const RxFrame& frame);
+    };
+
+    ListenerRegister lmsg_;
+    ListenerRegister lsrv_req_;
+    ListenerRegister lsrv_resp_;
+
+    const uint8_t self_node_id_;
+
+    void handleFrame(const CanRxFrame& can_frame);
+
+public:
+    Dispatcher(ICanDriver* driver, IAllocator* allocator, ISystemClock* sysclock, IOutgoingTransferRegistry* otr,
+               uint8_t self_node_id)
+    : canio_(driver, allocator, sysclock)
+    , sysclock_(sysclock)
+    , outgoing_transfer_reg_(otr)
+    , self_node_id_(self_node_id)
+    {
+        assert(driver);
+        assert(allocator);
+        assert(sysclock);
+        assert(otr);
+    }
+
+    int spin(uint64_t monotonic_deadline);
+
+    /**
+     * Refer to CanIOManager::send() for the parameter description
+     */
+    int send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
+             CanTxQueue::Qos qos);
+
+    void cleanup(uint64_t ts_monotonic);
+
+    bool registerMessageListener(TransferListenerBase* listener);
+    bool registerServiceRequestListener(TransferListenerBase* listener);
+    bool registerServiceResponseListener(TransferListenerBase* listener);
+
+    void unregisterMessageListener(TransferListenerBase* listener);
+    void unregisterServiceRequestListener(TransferListenerBase* listener);
+    void unregisterServiceResponseListener(TransferListenerBase* listener);
+
+    IOutgoingTransferRegistry* getOutgoingTransferRegistry() { return outgoing_transfer_reg_; }
+
+    uint8_t getSelfNodeID() const { return self_node_id_; }
+};
+
+}

libuavcan/src/transport/dispatcher.cpp

@@ -0,0 +1,211 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <uavcan/internal/transport/dispatcher.hpp>
+#include <uavcan/internal/debug.hpp>
+
+namespace uavcan
+{
+/*
+ * Dispatcher::ListenerRegister
+ */
+bool Dispatcher::ListenerRegister::add(TransferListenerBase* listener, Mode mode)
+{
+    if (mode == UNIQUE_LISTENER)
+    {
+        TransferListenerBase* p = list_.get();
+        while (p)
+        {
+            if (p->getDataTypeDescriptor()->id == listener->getDataTypeDescriptor()->id)
+                return false;
+            p = p->getNextListNode();
+        }
+    }
+    // Objective is to arrange entries by Data Type ID in ascending order from root.
+    list_.insertBefore(listener, DataTypeIDInsertionComparator(listener->getDataTypeDescriptor()->id));
+    return true;
+}
+
+void Dispatcher::ListenerRegister::remove(TransferListenerBase* listener)
+{
+    list_.remove(listener);
+}
+
+void Dispatcher::ListenerRegister::cleanup(uint64_t ts_monotonic)
+{
+    TransferListenerBase* p = list_.get();
+    while (p)
+    {
+        p->cleanup(ts_monotonic);
+        p = p->getNextListNode();
+    }
+}
+
+void Dispatcher::ListenerRegister::handleFrame(const RxFrame& frame)
+{
+    TransferListenerBase* p = list_.get();
+    while (p)
+    {
+        if (p->getDataTypeDescriptor()->id == frame.data_type_id)
+            p->handleFrame(frame);
+        else if (p->getDataTypeDescriptor()->id < frame.data_type_id)  // Listeners are ordered by data type id!
+            break;
+        p = p->getNextListNode();
+    }
+}
+
+/*
+ * Dispatcher
+ */
+void Dispatcher::handleFrame(const CanRxFrame& can_frame)
+{
+    RxFrame frame;
+    if (!frame.parse(can_frame))
+    {
+        UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
+        return;
+    }
+
+    /*
+     * Target Node ID checking
+     */
+    switch (frame.transfer_type)
+    {
+    case TRANSFER_TYPE_MESSAGE_BROADCAST:
+        // Always accepted
+        break;
+
+    case TRANSFER_TYPE_SERVICE_RESPONSE:
+    case TRANSFER_TYPE_SERVICE_REQUEST:
+    case TRANSFER_TYPE_MESSAGE_UNICAST:
+        /* Here we drop transfer headers that aren't addressed to us. Non-first frames must be accepted in
+         * any case, because they lack address information in order to reduce payload overhead.
+         */
+        if (frame.frame_index == 0)
+        {
+            const uint8_t target_node_id = frame.payload[0];
+            if (target_node_id != getSelfNodeID())
+                return;
+        }
+        break;
+
+    default:
+        // This means that RxFrame::parse() accepted an invalid frame. Too bad!
+        assert(0);
+        return;
+    }
+
+    /*
+     * Target register selection
+     */
+    switch (frame.transfer_type)
+    {
+    case TRANSFER_TYPE_MESSAGE_BROADCAST:
+    case TRANSFER_TYPE_MESSAGE_UNICAST:
+        lmsg_.handleFrame(frame);
+        break;
+
+    case TRANSFER_TYPE_SERVICE_REQUEST:
+        lsrv_req_.handleFrame(frame);
+        break;
+
+    case TRANSFER_TYPE_SERVICE_RESPONSE:
+        lsrv_resp_.handleFrame(frame);
+        break;
+
+    default:
+        assert(0);
+    }
+}
+
+int Dispatcher::spin(uint64_t monotonic_deadline)
+{
+    int num_frames_processed = 0;
+    do
+    {
+        CanRxFrame frame;
+        const int res = canio_.receive(frame, monotonic_deadline);
+        if (res < 0)
+            return res;
+        if (res > 0)
+        {
+            num_frames_processed++;
+            handleFrame(frame);
+        }
+    }
+    while (sysclock_->getMonotonicMicroseconds() < monotonic_deadline);
+
+    return num_frames_processed;
+}
+
+int Dispatcher::send(const Frame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
+                     CanTxQueue::Qos qos)
+{
+    if (frame.source_node_id != getSelfNodeID())
+    {
+        assert(0);
+        return -1;
+    }
+
+    const CanFrame can_frame = frame.compile();
+    const int iface_mask = (1 << canio_.getNumIfaces()) - 1;
+
+    return canio_.send(can_frame, monotonic_tx_deadline, monotonic_blocking_deadline, iface_mask, qos);
+}
+
+void Dispatcher::cleanup(uint64_t ts_monotonic)
+{
+    outgoing_transfer_reg_->cleanup(ts_monotonic);
+    lmsg_.cleanup(ts_monotonic);
+    lsrv_req_.cleanup(ts_monotonic);
+    lsrv_resp_.cleanup(ts_monotonic);
+}
+
+bool Dispatcher::registerMessageListener(TransferListenerBase* listener)
+{
+    if (listener->getDataTypeDescriptor()->kind != DATA_TYPE_KIND_MESSAGE)
+    {
+        assert(0);
+        return false;
+    }
+    return lmsg_.add(listener, ListenerRegister::MANY_LISTENERS);       // Multiple subscribers are OK
+}
+
+bool Dispatcher::registerServiceRequestListener(TransferListenerBase* listener)
+{
+    if (listener->getDataTypeDescriptor()->kind != DATA_TYPE_KIND_SERVICE)
+    {
+        assert(0);
+        return false;
+    }
+    return lsrv_req_.add(listener, ListenerRegister::UNIQUE_LISTENER);  // Only one server per data type
+}
+
+bool Dispatcher::registerServiceResponseListener(TransferListenerBase* listener)
+{
+    if (listener->getDataTypeDescriptor()->kind != DATA_TYPE_KIND_SERVICE)
+    {
+        assert(0);
+        return false;
+    }
+    return lsrv_resp_.add(listener, ListenerRegister::MANY_LISTENERS);  // Multiple callers may call same srv
+}
+
+void Dispatcher::unregisterMessageListener(TransferListenerBase* listener)
+{
+    lmsg_.remove(listener);
+}
+
+void Dispatcher::unregisterServiceRequestListener(TransferListenerBase* listener)
+{
+    lsrv_req_.remove(listener);
+}
+
+void Dispatcher::unregisterServiceResponseListener(TransferListenerBase* listener)
+{
+    lsrv_resp_.remove(listener);
+}
+
+}