Loopback frame listener API implemented; TransferSender supports iface masks and CanIOFlags. All of that is necessary to implement network-wide time synchronization.

libuavcan/include/uavcan/transport/dispatcher.hpp

@@ -14,6 +14,44 @@
 namespace uavcan
 {
 
+class Dispatcher;
+
+class LoopbackFrameListenerBase : public LinkedListNode<LoopbackFrameListenerBase>, 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<LoopbackFrameListenerBase> 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);
 

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.

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)

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);
 }
 

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;
 

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<const uint8_t*>("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());
+}

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<const uint8_t*>("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());
+}