Make UAVCAN transport layer capable of sending/receiving messages from custom CAN protocols

libuavcan/include/uavcan/transport/can_io.hpp

@@ -149,6 +149,7 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
 
     LazyConstructor<CanTxQueue> tx_queues_[MaxCanIfaces];
     IfaceFrameCounters counters_[MaxCanIfaces];
+    Protocol iface_protocol_[MaxCanIfaces];
 
     const uint8_t num_ifaces_;
 
@@ -161,6 +162,9 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
     CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock,
                  std::size_t mem_blocks_per_iface = 0);
 
+    bool changeIfaceProtocol(unsigned ifaceId, Protocol protocol);
+    Protocol getIfaceProtocol(unsigned ifaceId);
+
     uint8_t getNumIfaces() const { return num_ifaces_; }
 
     CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const;
@@ -178,6 +182,8 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable
      */
     int send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
              uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
+    int send(const CanFrame& frame, int frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
+             uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags);
     int receive(CanRxFrame& out_frame, MonotonicTime blocking_deadline, CanIOFlags& out_flags);
 };
 

libuavcan/include/uavcan/transport/custom_protocols.hpp

@@ -0,0 +1,45 @@
+#ifndef UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED
+#define UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED
+
+#include <uavcan/driver/can.hpp>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/transport/can_io.hpp>
+#include <uavcan/transport/transfer.hpp>
+
+namespace uavcan
+{
+
+class CustomTransferListener : public LinkedListNode<CustomTransferListener>
+{
+public:
+	Protocol protocol_;
+
+	CustomTransferListener() : protocol_(Protocol::Standard) {;}
+
+	CustomTransferListener(const Protocol protocol) :
+		protocol_(protocol)
+	{;}
+
+	Protocol getCANProtocol() { return protocol_; }
+
+	virtual bool handleFrame(const CanRxFrame& can_frame, const Protocol protocol) = 0;
+};
+
+// right now this class is useless
+class CustomTransferSender
+{
+public:
+	Protocol protocol_;
+
+	CustomTransferSender() : protocol_(Protocol::Standard) {;}
+
+	CustomTransferSender(const Protocol protocol) :
+		protocol_(protocol)
+	{;}
+
+	Protocol getCANProtocol() { return protocol_; }
+};
+
+};
+
+#endif // UAVCAN_CUSTOM_PROTOCOLS_HPP_INCLUDED

libuavcan/include/uavcan/transport/dispatcher.hpp

@@ -14,6 +14,7 @@
 #include <uavcan/transport/outgoing_transfer_registry.hpp>
 #include <uavcan/transport/can_io.hpp>
 #include <uavcan/util/linked_list.hpp>
+#include <uavcan/transport/custom_protocols.hpp>
 
 namespace uavcan
 {
@@ -45,7 +46,6 @@ class UAVCAN_EXPORT LoopbackFrameListenerBase : public LinkedListNode<LoopbackFr
     virtual void handleLoopbackFrame(const RxFrame& frame) = 0;
 };
 
-
 class UAVCAN_EXPORT LoopbackFrameListenerRegistry : Noncopyable
 {
     LinkedListRoot<LoopbackFrameListenerBase> listeners_;
@@ -120,6 +120,8 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable
     ListenerRegistry lsrv_req_;
     ListenerRegistry lsrv_resp_;
 
+    LinkedListRoot<CustomTransferListener> CustomCanListener_list_;
+
 #if !UAVCAN_TINY
     LoopbackFrameListenerRegistry loopback_listeners_;
     IRxFrameListener* rx_listener_;
@@ -144,7 +146,7 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable
 #endif
         , self_node_id_(NodeID::Broadcast)  // Default
         , self_node_id_is_set_(false)
-    { }
+    {;}
 
     /**
      * This version returns strictly when the deadline is reached.
@@ -167,10 +169,12 @@ class UAVCAN_EXPORT Dispatcher : Noncopyable
     bool registerMessageListener(TransferListener* listener);
     bool registerServiceRequestListener(TransferListener* listener);
     bool registerServiceResponseListener(TransferListener* listener);
+    bool registerCustomCanListener(CustomTransferListener* listener);
 
     void unregisterMessageListener(TransferListener* listener);
     void unregisterServiceRequestListener(TransferListener* listener);
     void unregisterServiceResponseListener(TransferListener* listener);
+    void unregisterCustomCanListener(CustomTransferListener* listener);
 
     bool hasSubscriber(DataTypeID dtid) const;
     bool hasPublisher(DataTypeID dtid) const;

libuavcan/include/uavcan/transport/transfer.hpp

@@ -22,6 +22,15 @@ enum TransferType
     TransferTypeMessageBroadcast = 2
 };
 
+enum Protocol
+{
+    Standard = 0,
+    Custom1 = 1,
+    Custom2 = 2,
+    Custom3 = 3,
+    Invalid = 4
+};
+
 static const uint8_t NumTransferTypes = 3;
 
 

libuavcan/src/transport/uc_can_io.cpp

@@ -307,6 +307,32 @@ CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISyste
     }
 }
 
+bool CanIOManager::changeIfaceProtocol(unsigned ifaceId, Protocol protocol)
+{
+    if (ifaceId < getNumIfaces() && protocol != Protocol::Invalid)
+    {
+        iface_protocol_[ifaceId] = protocol;
+
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+Protocol CanIOManager::getIfaceProtocol(unsigned ifaceId)
+{
+    if (ifaceId < getNumIfaces())
+    {
+        return iface_protocol_[ifaceId];
+    }
+    else
+    {
+        return Protocol::Invalid;
+    }
+}
+
 uint8_t CanIOManager::makePendingTxMask() const
 {
     uint8_t write_mask = 0;
@@ -337,11 +363,27 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con
 
 int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
                        uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
+{
+    return send(frame, Protocol::Standard, tx_deadline, blocking_deadline, iface_mask, qos, flags);
+}
+
+int CanIOManager::send(const CanFrame& frame, int frame_protocol, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
+                       uint8_t iface_mask, CanTxQueue::Qos qos, CanIOFlags flags)
 {
     const uint8_t num_ifaces = getNumIfaces();
     const uint8_t all_ifaces_mask = uint8_t((1U << num_ifaces) - 1);
+
     iface_mask &= all_ifaces_mask;
 
+    // make sure we send frames of a specific protocol only for ifaces configured with the same protocol
+    for (unsigned i = 0; i < getNumIfaces(); i++)
+    {
+        if (iface_protocol_[i] != frame_protocol)
+        {
+            iface_mask &= ~(1 << i);
+        }
+    }
+
     if (blocking_deadline > tx_deadline)
     {
         blocking_deadline = tx_deadline;

libuavcan/src/transport/uc_dispatcher.cpp

@@ -149,41 +149,73 @@ void Dispatcher::ListenerRegistry::handleFrame(const RxFrame& frame, bool tao_di
 void Dispatcher::handleFrame(const CanRxFrame& can_frame)
 {
     RxFrame frame;
-    if (!frame.parse(can_frame))
-    {
-        // This is not counted as a transport error
-        UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
-        return;
-    }
 
-    if ((frame.getDstNodeID() != NodeID::Broadcast) &&
-        (frame.getDstNodeID() != getNodeID()))
-    {
-        return;
-    }
+    const Protocol iface_protocol = getCanIOManager().getIfaceProtocol(can_frame.iface_index);
 
-    switch (frame.getTransferType())
+    // next block of operations only valid for UAVCAN, which requires extended frame
+    if (can_frame.isExtended() && iface_protocol == Protocol::Standard)
     {
-    case TransferTypeMessageBroadcast:
-    {
-        lmsg_.handleFrame(frame, tao_disabled_);
-        break;
-    }
-    case TransferTypeServiceRequest:
-    {
-        lsrv_req_.handleFrame(frame, tao_disabled_);
-        break;
+        if (!frame.parse(can_frame))
+        {
+            // This is not counted as a transport error
+            UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
+            return;
+        }
+
+        if ((frame.getDstNodeID() != NodeID::Broadcast) &&
+            (frame.getDstNodeID() != getNodeID()))
+        {
+            return;
+        }
     }
-    case TransferTypeServiceResponse:
+
+    if (iface_protocol == Protocol::Standard && can_frame.isExtended())
     {
-        lsrv_resp_.handleFrame(frame, tao_disabled_);
-        break;
+        switch (frame.getTransferType())
+        {
+        case TransferTypeMessageBroadcast:
+        {
+            lmsg_.handleFrame(frame, tao_disabled_);
+            break;
+        }
+        case TransferTypeServiceRequest:
+        {
+            lsrv_req_.handleFrame(frame, tao_disabled_);
+            break;
+        }
+        case TransferTypeServiceResponse:
+        {
+            lsrv_resp_.handleFrame(frame, tao_disabled_);
+            break;
+        }
+        default:
+        {
+            UAVCAN_ASSERT(0);
+            break;
+        }
+        }
     }
-    default:
+    else
     {
-        UAVCAN_ASSERT(0);
-        break;
-    }
+        bool success = false;
+
+        CustomTransferListener* p = CustomCanListener_list_.get();
+
+        while (p)
+        {
+            CustomTransferListener* const next = p->getNextListNode();
+
+            success = success || p->handleFrame(can_frame, iface_protocol); // p may be modified
+
+            p = next;
+        }
+
+        if (!success)
+        {
+            // This is not counted as a transport error
+            UAVCAN_TRACE("Dispatcher", "Invalid CAN frame received: %s", can_frame.toString().c_str());
+            return;
+        }
     }
 }
 
@@ -300,6 +332,7 @@ int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTim
         UAVCAN_ASSERT(0);
         return -ErrLogic;
     }
+
     return canio_.send(can_frame, tx_deadline, blocking_deadline, iface_mask, qos, flags);
 }
 
@@ -341,6 +374,13 @@ bool Dispatcher::registerServiceResponseListener(TransferListener* listener)
     return lsrv_resp_.add(listener, ListenerRegistry::ManyListeners);  // Multiple callers may call same srv
 }
 
+bool Dispatcher::registerCustomCanListener(CustomTransferListener* listener)
+{
+    CustomCanListener_list_.insert(listener);
+
+    return true;
+}
+
 void Dispatcher::unregisterMessageListener(TransferListener* listener)
 {
     lmsg_.remove(listener);
@@ -356,6 +396,11 @@ void Dispatcher::unregisterServiceResponseListener(TransferListener* listener)
     lsrv_resp_.remove(listener);
 }
 
+void Dispatcher::unregisterCustomCanListener(CustomTransferListener* listener)
+{
+    CustomCanListener_list_.remove(listener);
+}
+
 bool Dispatcher::hasSubscriber(DataTypeID dtid) const
 {
     return lmsg_.exists(dtid);