From 20c828912f40f49c784767dbfb9e7f3002271122 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 14 Feb 2014 19:19:23 +0400 Subject: [PATCH] Dispatcher class, tests to come --- .../uavcan/internal/transport/dispatcher.hpp | 93 ++++++++ libuavcan/src/transport/dispatcher.cpp | 211 ++++++++++++++++++ 2 files changed, 304 insertions(+) create mode 100644 libuavcan/include/uavcan/internal/transport/dispatcher.hpp create mode 100644 libuavcan/src/transport/dispatcher.cpp diff --git a/libuavcan/include/uavcan/internal/transport/dispatcher.hpp b/libuavcan/include/uavcan/internal/transport/dispatcher.hpp new file mode 100644 index 0000000000..ed9a89d433 --- /dev/null +++ b/libuavcan/include/uavcan/internal/transport/dispatcher.hpp @@ -0,0 +1,93 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class Dispatcher : Noncopyable +{ + CanIOManager canio_; + ISystemClock* const sysclock_; + IOutgoingTransferRegistry* const outgoing_transfer_reg_; + + class ListenerRegister + { + LinkedListRoot 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_; } +}; + +} diff --git a/libuavcan/src/transport/dispatcher.cpp b/libuavcan/src/transport/dispatcher.cpp new file mode 100644 index 0000000000..34ab0768e2 --- /dev/null +++ b/libuavcan/src/transport/dispatcher.cpp @@ -0,0 +1,211 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +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); +} + +}