From 54921a3738d543754fc248b300879f34adc88ceb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 21 Mar 2014 17:30:25 +0400 Subject: [PATCH] Loopback frame listener API implemented; TransferSender supports iface masks and CanIOFlags. All of that is necessary to implement network-wide time synchronization. --- .../include/uavcan/transport/dispatcher.hpp | 45 ++++++++++- .../uavcan/transport/transfer_sender.hpp | 11 +++ libuavcan/src/transport/can_io.cpp | 2 - libuavcan/src/transport/dispatcher.cpp | 79 +++++++++++++++++-- libuavcan/src/transport/transfer_sender.cpp | 4 +- libuavcan/test/transport/dispatcher.cpp | 64 ++++++++++++++- libuavcan/test/transport/transfer_sender.cpp | 57 +++++++++++++ 7 files changed, 250 insertions(+), 12 deletions(-) diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index ebab7bb8bf..709ad3ff9f 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -14,6 +14,44 @@ namespace uavcan { +class Dispatcher; + +class LoopbackFrameListenerBase : public LinkedListNode, Noncopyable +{ + Dispatcher& dispatcher_; + +protected: + LoopbackFrameListenerBase(Dispatcher& dispatcher) + : dispatcher_(dispatcher) + { } + + virtual ~LoopbackFrameListenerBase() { stopListening(); } + + void startListening(); + void stopListening(); + bool isListening() const; + + Dispatcher& getDispatcher() { return dispatcher_; } + +public: + virtual void handleLoopbackFrame(const RxFrame& frame) = 0; +}; + + +class LoopbackFrameListenerRegistry : Noncopyable +{ + LinkedListRoot listeners_; + +public: + void add(LoopbackFrameListenerBase* listener); + void remove(LoopbackFrameListenerBase* listener); + bool doesExist(const LoopbackFrameListenerBase* listener) const; + unsigned int getNumListeners() const { return listeners_.getLength(); } + + void invokeListeners(RxFrame& frame); +}; + + class Dispatcher : Noncopyable { CanIOManager canio_; @@ -52,9 +90,12 @@ class Dispatcher : Noncopyable ListenerRegister lsrv_req_; ListenerRegister lsrv_resp_; + LoopbackFrameListenerRegistry loopback_listeners_; + NodeID self_node_id_; void handleFrame(const CanRxFrame& can_frame); + void handleLoopbackFrame(const CanRxFrame& can_frame); public: Dispatcher(ICanDriver& driver, IAllocator& allocator, ISystemClock& sysclock, IOutgoingTransferRegistry& otr) @@ -69,7 +110,7 @@ public: * Refer to CanIOManager::send() for the parameter description */ int send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, CanTxQueue::Qos qos, - CanIOFlags flags); + CanIOFlags flags, uint8_t iface_mask); void cleanup(MonotonicTime ts); @@ -91,6 +132,8 @@ public: IOutgoingTransferRegistry& getOutgoingTransferRegistry() { return outgoing_transfer_reg_; } + LoopbackFrameListenerRegistry& getLoopbackFrameListenerRegistry() { return loopback_listeners_; } + NodeID getNodeID() const { return self_node_id_; } bool setNodeID(NodeID nid); diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 3c7ad86863..baddb65462 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -21,10 +21,13 @@ class TransferSender const CanTxQueue::Qos qos_; const TransferCRC crc_base_; CanIOFlags flags_; + uint8_t iface_mask_; Dispatcher& dispatcher_; public: + enum { AllIfacesMask = 0xFF }; + static MonotonicDuration getDefaultMaxTransferInterval() { return MonotonicDuration::fromMSec(60 * 1000); @@ -37,12 +40,20 @@ public: , qos_(qos) , crc_base_(data_type.getSignature().toTransferCRC()) , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) , dispatcher_(dispatcher) { } CanIOFlags getCanIOFlags() const { return flags_; } void setCanIOFlags(CanIOFlags flags) { flags_ = flags; } + uint8_t getIfaceMask() const { return iface_mask_; } + void setIfaceMask(uint8_t iface_mask) + { + assert(iface_mask); + iface_mask_ = iface_mask; + } + /** * Send with explicit Transfer ID. * Should be used only for service responses, where response TID should match request TID. diff --git a/libuavcan/src/transport/can_io.cpp b/libuavcan/src/transport/can_io.cpp index eba1998c48..e4500c1ca8 100644 --- a/libuavcan/src/transport/can_io.cpp +++ b/libuavcan/src/transport/can_io.cpp @@ -258,8 +258,6 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton { const int num_ifaces = getNumIfaces(); const int all_ifaces_mask = (1 << num_ifaces) - 1; - - assert((iface_mask & ~all_ifaces_mask) == 0); iface_mask &= all_ifaces_mask; if (blocking_deadline > tx_deadline) diff --git a/libuavcan/src/transport/dispatcher.cpp b/libuavcan/src/transport/dispatcher.cpp index 0e09f25e50..7e807ca2af 100644 --- a/libuavcan/src/transport/dispatcher.cpp +++ b/libuavcan/src/transport/dispatcher.cpp @@ -8,6 +8,63 @@ namespace uavcan { +/* + * LoopbackFrameListenerBase + */ +void LoopbackFrameListenerBase::startListening() +{ + dispatcher_.getLoopbackFrameListenerRegistry().add(this); +} + +void LoopbackFrameListenerBase::stopListening() +{ + dispatcher_.getLoopbackFrameListenerRegistry().remove(this); +} + +bool LoopbackFrameListenerBase::isListening() const +{ + return dispatcher_.getLoopbackFrameListenerRegistry().doesExist(this); +} + +/* + * LoopbackFrameListenerRegistry + */ +void LoopbackFrameListenerRegistry::add(LoopbackFrameListenerBase* listener) +{ + assert(listener); + listeners_.insert(listener); +} + +void LoopbackFrameListenerRegistry::remove(LoopbackFrameListenerBase* listener) +{ + assert(listener); + listeners_.remove(listener); +} + +bool LoopbackFrameListenerRegistry::doesExist(const LoopbackFrameListenerBase* listener) const +{ + assert(listener); + const LoopbackFrameListenerBase* p = listeners_.get(); + while (p) + { + if (p == listener) + return true; + p = p->getNextListNode(); + } + return false; +} + +void LoopbackFrameListenerRegistry::invokeListeners(RxFrame& frame) +{ + LoopbackFrameListenerBase* p = listeners_.get(); + while (p) + { + LoopbackFrameListenerBase* const next = p->getNextListNode(); + p->handleLoopbackFrame(frame); + p = next; + } +} + /* * Dispatcher::ListenerRegister */ @@ -106,12 +163,25 @@ void Dispatcher::handleFrame(const CanRxFrame& can_frame) } } +void Dispatcher::handleLoopbackFrame(const CanRxFrame& can_frame) +{ + RxFrame frame; + if (!frame.parse(can_frame)) + { + UAVCAN_TRACE("Dispatcher", "Invalid loopback CAN frame: %s", can_frame.toString().c_str()); + assert(0); // No way! + return; + } + assert(frame.getSrcNodeID() == getNodeID()); + loopback_listeners_.invokeListeners(frame); +} + int Dispatcher::spin(MonotonicTime deadline) { int num_frames_processed = 0; do { - CanIOFlags flags = CanIOFlags(); + CanIOFlags flags = 0; CanRxFrame frame; const int res = canio_.receive(frame, deadline, flags); if (res < 0) @@ -120,8 +190,7 @@ int Dispatcher::spin(MonotonicTime deadline) { if (flags & CanIOFlagLoopback) { - // TODO: Loopback handling! - assert(0); // Not implemented + handleLoopbackFrame(frame); } else { @@ -136,7 +205,7 @@ int Dispatcher::spin(MonotonicTime deadline) } int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, - CanTxQueue::Qos qos, CanIOFlags flags) + CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask) { if (frame.getSrcNodeID() != getNodeID()) { @@ -151,8 +220,6 @@ int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTim assert(0); return -1; } - const int iface_mask = (1 << canio_.getNumIfaces()) - 1; - return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags); } diff --git a/libuavcan/src/transport/transfer_sender.cpp b/libuavcan/src/transport/transfer_sender.cpp index 3e3bc03344..7f4d32dae1 100644 --- a/libuavcan/src/transport/transfer_sender.cpp +++ b/libuavcan/src/transport/transfer_sender.cpp @@ -27,7 +27,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime } frame.makeLast(); assert(frame.isLast() && frame.isFirst()); - return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_); + return dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_); } else // Multi Frame Transfer { @@ -57,7 +57,7 @@ int TransferSender::send(const uint8_t* payload, int payload_len, MonotonicTime while (true) { - const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_); + const int send_res = dispatcher_.send(frame, tx_deadline, blocking_deadline, qos_, flags_, iface_mask_); if (send_res < 0) return send_res; diff --git a/libuavcan/test/transport/dispatcher.cpp b/libuavcan/test/transport/dispatcher.cpp index 30c02ef19d..c51fe241bb 100644 --- a/libuavcan/test/transport/dispatcher.cpp +++ b/libuavcan/test/transport/dispatcher.cpp @@ -237,7 +237,7 @@ TEST(Dispatcher, Transmission) ASSERT_TRUE(dispatcher.hasPublisher(123)); ASSERT_FALSE(dispatcher.hasPublisher(456)); - ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0)); + ASSERT_EQ(2, dispatcher.send(frame, TX_DEADLINE, tsMono(0), uavcan::CanTxQueue::Volatile, 0, 0xFF)); /* * Validation @@ -276,3 +276,65 @@ TEST(Dispatcher, Spin) ASSERT_EQ(0, dispatcher.spin(tsMono(1100))); ASSERT_LE(1100, clockmock.monotonic); } + + +struct DispatcherTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase +{ + uavcan::RxFrame last_frame; + unsigned int count; + + DispatcherTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher) + : uavcan::LoopbackFrameListenerBase(dispatcher) + , count(0) + { } + + using uavcan::LoopbackFrameListenerBase::startListening; + using uavcan::LoopbackFrameListenerBase::isListening; + + void handleLoopbackFrame(const uavcan::RxFrame& frame) + { + std::cout << "DispatcherTestLoopbackFrameListener: " << frame.toString() << std::endl; + last_frame = frame; + count++; + } +}; + +TEST(Dispatcher, Loopback) +{ + uavcan::PoolManager<1> poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr); + + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock, out_trans_reg); + ASSERT_TRUE(dispatcher.setNodeID(SELF_NODE_ID)); + + { + DispatcherTestLoopbackFrameListener listener(dispatcher); + ASSERT_FALSE(listener.isListening()); + listener.startListening(); + ASSERT_TRUE(listener.isListening()); + + // uint_fast16_t data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame = false + uavcan::Frame frame(123, uavcan::TransferTypeMessageUnicast, SELF_NODE_ID, 2, 0, 0, true); + frame.setPayload(reinterpret_cast("123"), 3); + + ASSERT_TRUE(listener.last_frame == uavcan::RxFrame()); + + ASSERT_LE(0, dispatcher.send(frame, tsMono(1000), tsMono(0), uavcan::CanTxQueue::Persistent, + uavcan::CanIOFlagLoopback, 0xFF)); + + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + + ASSERT_TRUE(listener.last_frame != uavcan::RxFrame()); + ASSERT_TRUE(listener.last_frame == frame); + ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); // Last iface + ASSERT_EQ(2, listener.count); + + ASSERT_EQ(1, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners()); + } + ASSERT_EQ(0, dispatcher.getLoopbackFrameListenerRegistry().getNumListeners()); +} diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index 3c684a11b0..1ae91d44a4 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -144,3 +144,60 @@ TEST(TransferSender, Basic) ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[5])); ASSERT_TRUE(sub_srv_resp.matchAndPop(TRANSFERS[7])); } + + +struct TransferSenderTestLoopbackFrameListener : public uavcan::LoopbackFrameListenerBase +{ + uavcan::RxFrame last_frame; + unsigned int count; + + TransferSenderTestLoopbackFrameListener(uavcan::Dispatcher& dispatcher) + : uavcan::LoopbackFrameListenerBase(dispatcher) + , count(0) + { + startListening(); + } + + void handleLoopbackFrame(const uavcan::RxFrame& frame) + { + last_frame = frame; + count++; + } +}; + +TEST(TransferSender, Loopback) +{ + uavcan::PoolManager<1> poolmgr; + + SystemClockMock clockmock(100); + CanDriverMock driver(2, clockmock); + + uavcan::OutgoingTransferRegistry<8> out_trans_reg(poolmgr); + + static const uavcan::NodeID TX_NODE_ID(64); + uavcan::Dispatcher dispatcher(driver, poolmgr, clockmock, out_trans_reg); + ASSERT_TRUE(dispatcher.setNodeID(TX_NODE_ID)); + + uavcan::DataTypeDescriptor desc = makeDataType(uavcan::DataTypeKindMessage, 1, "Foobar"); + + uavcan::TransferSender sender(dispatcher, desc, uavcan::CanTxQueue::Volatile); + + sender.setCanIOFlags(uavcan::CanIOFlagLoopback); + ASSERT_EQ(uavcan::CanIOFlagLoopback, sender.getCanIOFlags()); + + sender.setIfaceMask(2); + ASSERT_EQ(2, sender.getIfaceMask()); + + TransferSenderTestLoopbackFrameListener listener(dispatcher); + + ASSERT_LE(0, sender.send(reinterpret_cast("123"), 3, tsMono(1000), tsMono(0), + uavcan::TransferTypeMessageBroadcast, 0)); + + ASSERT_EQ(0, listener.count); + ASSERT_EQ(0, dispatcher.spin(tsMono(1000))); + ASSERT_EQ(1, listener.count); + ASSERT_EQ(1, listener.last_frame.getIfaceIndex()); + ASSERT_EQ(3, listener.last_frame.getPayloadLen()); + ASSERT_TRUE(TX_NODE_ID == listener.last_frame.getSrcNodeID()); + ASSERT_TRUE(listener.last_frame.isLast()); +}