Merge branch 'dynamic_node_id'

CMakeLists.txt

@@ -34,4 +34,4 @@ add_subdirectory(libuavcan)
 if (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
     message(STATUS "Adding Linux support library")
     add_subdirectory(libuavcan_drivers/linux)
-endif ()
+endif ()

dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan

@@ -0,0 +1,64 @@
+#
+# This message is used for dynamic Node ID allocation. This algorithm is an optional extension to the UAVCAN
+# specification, and it is not mandatory to support it.
+#
+# On the client's side the protocol is defined through the following set of rules:
+#
+# Rule A. On initialization:
+# 1. The client subscribes to this message.
+# 2. The client starts the Request Timer with interval of DEFAULT_REQUEST_INTERVAL_MS.
+#
+# Rule B. On expiration of Request Timer:
+# 1. Request Timer restarts.
+# 2. The client broadcasts a first-stage Allocation request message, where the fields are assigned following values:
+#    node_id                 - preferred node ID, or zero if the client doesn't have any preference
+#    first_part_of_unique_id - true
+#    unique_id               - first MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of unique ID
+#
+# Rule C. On any Allocation message, even if other rules also match:
+# 1. Request Timer restarts.
+#
+# Rule D. On an Allocation message WHERE (source node ID is non-anonymous) AND (client's unique ID starts with the
+# bytes available in the field unique_id) AND (unique_id is less than 16 bytes long):
+# 1. The client broadcasts a second-stage Allocation request message, where the fields are assigned following values:
+#    node_id                 - same value as in the first-stage
+#    first_part_of_unique_id - false
+#    unique_id               - at most MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of local unique ID with an offset
+#                              equal to number of bytes in the received unique ID
+#
+# Rule E. On an Allocation message WHERE (source node ID is non-anonymous) AND (unique_id fully matches client's
+# unique ID) AND (node_id in the received message is not zero):
+# 1. Request Timer stops.
+# 2. The client initializes its node_id with the received value.
+# 3. The client terminates subscription to Allocation messages.
+# 4. Exit.
+#
+
+#
+# Recommended request transmission period.
+#
+uint16 DEFAULT_REQUEST_PERIOD_MS = 1000
+
+#
+# Any request message can accommodate no more than this number of bytes of unique ID.
+# This limitation is needed to ensure that all request transfers are single-frame.
+#
+uint8 MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST = 7
+
+#
+# If transfer is anonymous, this is the preferred ID.
+# If transfer is non-anonymous, this is allocated ID.
+#
+uint7 node_id
+
+#
+# If transfer is anonymous, this field indicates first-stage request.
+# If transfer is non-anonymous, this field should be ignored.
+#
+bool first_part_of_unique_id
+
+#
+# If transfer is anonymous, this array must not contain more than MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST items.
+# Note that array is tail-optimized, i.e. it will not be prepended with length field.
+#
+uint8[<=16] unique_id

dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan

@@ -1,5 +1,5 @@
 #
-# This service initiates a firmware update on a remote node.
+# This service initiates firmware update on a remote node.
 #
 # The node that is being updated (slave) will retrieve the firmware image file 'image_file_remote_path' from the node
 # 'source_node_id' using the file read service, then it will update the firmware and reboot. Alternatively, this
@@ -23,6 +23,7 @@ Path image_file_remote_path  # Empty to invoke an alternative bootloader
 
 uint8 ERROR_OK               = 0
 uint8 ERROR_INVALID_MODE     = 1    # Cannot perform the update right now (e.g. the vehicle is operating)
+uint8 ERROR_IN_PROGRESS      = 2    # Firmware update is already in progess, and the slave doesn't want to restart
 uint8 ERROR_UNKNOWN          = 255
 
 uint8 error

libuavcan/include/uavcan/node/generic_publisher.hpp

@@ -60,6 +60,15 @@ public:
     MonotonicDuration getTxTimeout() const { return tx_timeout_; }
     void setTxTimeout(MonotonicDuration tx_timeout);
 
+    /**
+     * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive.
+     * This option allows to enable sending anonymous transfers from passive mode.
+     */
+    void allowAnonymousTransfers()
+    {
+        sender_->allowAnonymousTransfers();
+    }
+
     INode& getNode() const { return node_; }
 };
 

libuavcan/include/uavcan/node/generic_subscriber.hpp

@@ -67,6 +67,7 @@ public:
     TransferID getTransferID()       const { return safeget<TransferID, &IncomingTransfer::getTransferID>(); }
     NodeID getSrcNodeID()            const { return safeget<NodeID, &IncomingTransfer::getSrcNodeID>(); }
     uint8_t getIfaceIndex()          const { return safeget<uint8_t, &IncomingTransfer::getIfaceIndex>(); }
+    bool isAnonymousTransfer()       const { return safeget<bool, &IncomingTransfer::isAnonymousTransfer>(); }
 };
 
 /**
@@ -200,6 +201,15 @@ protected:
         return genericStart(&Dispatcher::registerServiceResponseListener);
     }
 
+    /**
+     * By default, anonymous transfers will be ignored.
+     * This option allows to enable reception of anonymous transfers.
+     */
+    void allowAnonymousTransfers()
+    {
+        forwarder_->allowAnonymousTransfers();
+    }
+
     /**
      * Terminate the subscription.
      * Dispatcher core will remove this instance from the subscribers list.

libuavcan/include/uavcan/node/publisher.hpp

@@ -87,6 +87,7 @@ public:
      */
     using BaseType::init;
 
+    using BaseType::allowAnonymousTransfers;
     using BaseType::getTransferSender;
     using BaseType::getMinTxTimeout;
     using BaseType::getMaxTxTimeout;

libuavcan/include/uavcan/node/subscriber.hpp

@@ -111,6 +111,7 @@ public:
         return BaseType::startAsMessageListener();
     }
 
+    using BaseType::allowAnonymousTransfers;
     using BaseType::stop;
     using BaseType::getFailureCount;
 };

libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp

@@ -0,0 +1,91 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED
+
+#include <uavcan/node/subscriber.hpp>
+#include <uavcan/node/publisher.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/protocol/dynamic_node_id/Allocation.hpp>
+#include <uavcan/protocol/HardwareVersion.hpp>
+
+namespace uavcan
+{
+/**
+ * This class implements client-side logic of dynamic node ID allocation procedure.
+ *
+ * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined
+ * by the specification, until a Node ID is granted by the allocator.
+ *
+ * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests
+ * and listening for responses.
+ *
+ * Once dynamic allocation is complete (or not needed anymore), the object can be deleted.
+ */
+class DynamicNodeIDAllocationClient : private TimerBase
+{
+    typedef MethodBinder<DynamicNodeIDAllocationClient*,
+                         void (DynamicNodeIDAllocationClient::*)
+                             (const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>&)>
+        AllocationCallback;
+
+    Publisher<protocol::dynamic_node_id::Allocation> dnida_pub_;
+    Subscriber<protocol::dynamic_node_id::Allocation, AllocationCallback> dnida_sub_;
+
+    uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize];
+    NodeID preferred_node_id_;
+    NodeID allocated_node_id_;
+    NodeID allocator_node_id_;
+
+    void terminate();
+
+    virtual void handleTimerEvent(const TimerEvent&);
+
+    void handleAllocation(const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg);
+
+public:
+    DynamicNodeIDAllocationClient(INode& node)
+        : TimerBase(node)
+        , dnida_pub_(node)
+        , dnida_sub_(node)
+    { }
+
+    /**
+     * @param hardware_version  Hardware version information, where unique_id must be set correctly.
+     * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if
+     *                          the application doesn't have any preference (this is default).
+     * @return                  Zero on success
+     *                          Negative error code on failure
+     *                          -ErrLogic if 1. the node is not in passive mode or 2. the client is already started
+     */
+    int start(const protocol::HardwareVersion& hardware_version, const NodeID preferred_node_id = NodeID::Broadcast);
+
+    /**
+     * Use this method to determine when allocation is complete.
+     */
+    bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); }
+
+    /**
+     * This method allows to retrieve the node ID that was allocated to the local node.
+     * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast).
+     * @return          If allocation is complete, a valid unicast node ID will be returned.
+     *                  If allocation is not complete yet, a non-unicast node ID will be returned.
+     */
+    NodeID getAllocatedNodeID() const { return allocated_node_id_; }
+
+    /**
+     * This method allows to retrieve node ID of the allocator that granted our Node ID.
+     * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast).
+     * @return          If allocation is complete, a valid unicast node ID will be returned.
+     *                  If allocation is not complete yet, an non-unicast node ID will be returned.
+     */
+    NodeID getAllocatorNodeID() const { return allocator_node_id_; }
+};
+
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED

libuavcan/include/uavcan/std.hpp

@@ -7,6 +7,7 @@
 
 #include <uavcan/build_config.hpp>
 #include <cstdarg>
+#include <cstddef>
 
 #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
 # error UAVCAN_CPP_VERSION

libuavcan/include/uavcan/transport/dispatcher.hpp

@@ -115,6 +115,7 @@ public:
         : canio_(driver, allocator, sysclock)
         , sysclock_(sysclock)
         , outgoing_transfer_reg_(otr)
+        , self_node_id_(NodeID::Broadcast)  // Default
         , self_node_id_is_set_(false)
     { }
 

libuavcan/include/uavcan/transport/frame.hpp

@@ -50,7 +50,7 @@ public:
     {
         UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast());
         UAVCAN_ASSERT(data_type_id.isValid());
-        UAVCAN_ASSERT(src_node_id != dst_node_id);
+        UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true);
         UAVCAN_ASSERT(frame_index <= MaxIndex);
     }
 

libuavcan/include/uavcan/transport/transfer_listener.hpp

@@ -50,6 +50,11 @@ public:
      */
     virtual void release() { }
 
+    /**
+     * Whether this is a anonymous transfer
+     */
+    virtual bool isAnonymousTransfer() const { return false; }
+
     MonotonicTime getMonotonicTimestamp() const { return ts_mono_; }
     UtcTime getUtcTimestamp()             const { return ts_utc_; }
     TransferType getTransferType()        const { return transfer_type_; }
@@ -68,6 +73,7 @@ class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer
 public:
     explicit SingleFrameIncomingTransfer(const RxFrame& frm);
     virtual int read(unsigned offset, uint8_t* data, unsigned len) const;
+    virtual bool isAnonymousTransfer() const;
 };
 
 /**
@@ -93,6 +99,7 @@ class UAVCAN_EXPORT TransferListenerBase : public LinkedListNode<TransferListene
     MapBase<TransferBufferManagerKey, TransferReceiver>& receivers_;
     ITransferBufferManager& bufmgr_;
     TransferPerfCounter& perf_;
+    bool allow_anonymous_transfers_;
 
     class TimedOutReceiverPredicate
     {
@@ -119,17 +126,25 @@ protected:
         , receivers_(receivers)
         , bufmgr_(bufmgr)
         , perf_(perf)
+        , allow_anonymous_transfers_(false)
     { }
 
     virtual ~TransferListenerBase() { }
 
     void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba);
+    void handleAnonymousTransferReception(const RxFrame& frame);
 
     virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0;
 
 public:
     const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; }
 
+    /**
+     * By default, anonymous transfers will be ignored.
+     * This option allows to enable reception of anonymous transfers.
+     */
+    void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; }
+
     void cleanup(MonotonicTime ts);
 
     virtual void handleFrame(const RxFrame& frame);

libuavcan/include/uavcan/transport/transfer_sender.hpp

@@ -25,6 +25,7 @@ class UAVCAN_EXPORT TransferSender
     const TransferCRC crc_base_;
     CanIOFlags flags_;
     uint8_t iface_mask_;
+    bool allow_anonymous_transfers_;
 
     Dispatcher& dispatcher_;
 
@@ -46,6 +47,7 @@ public:
         , crc_base_(data_type.getSignature().toTransferCRC())
         , flags_(CanIOFlags(0))
         , iface_mask_(AllIfacesMask)
+        , allow_anonymous_transfers_(false)
         , dispatcher_(dispatcher)
     { }
 
@@ -59,6 +61,15 @@ public:
         iface_mask_ = iface_mask;
     }
 
+    /**
+     * Anonymous transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if
+     * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID).
+     * By default, this class will return an error if it is asked to send a transfer while the
+     * node is configured in passive mode. However, if this option is enabled, it will be possible
+     * to send anonymous transfers from passive mode.
+     */
+    void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; }
+
     /**
      * Send with explicit Transfer ID.
      * Should be used only for service responses, where response TID should match request TID.

libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp

@@ -0,0 +1,177 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
+
+namespace uavcan
+{
+
+void DynamicNodeIDAllocationClient::terminate()
+{
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated");
+    stop();
+    dnida_sub_.stop();
+}
+
+void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&)
+{
+    // This method implements Rule B
+    UAVCAN_ASSERT(preferred_node_id_.isValid());
+    if (isAllocationComplete())
+    {
+        UAVCAN_ASSERT(0);
+        terminate();
+        return;
+    }
+
+    // Filling the message
+    protocol::dynamic_node_id::Allocation msg;
+    msg.node_id = preferred_node_id_.get();
+    msg.first_part_of_unique_id = true;
+
+    msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST);
+    copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin());
+    UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_));
+
+    // Broadcasting
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Broadcasting 1st stage: preferred ID: %d",
+                 static_cast<int>(preferred_node_id_.get()));
+    const int res = dnida_pub_.broadcast(msg);
+    if (res < 0)
+    {
+        dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed");
+    }
+}
+
+void DynamicNodeIDAllocationClient::handleAllocation(
+    const ReceivedDataStructure<protocol::dynamic_node_id::Allocation>& msg)
+{
+    /*
+     * TODO This method can blow the stack easily
+     */
+    UAVCAN_ASSERT(preferred_node_id_.isValid());
+    if (isAllocationComplete())
+    {
+        UAVCAN_ASSERT(0);
+        terminate();
+        return;
+    }
+
+    startPeriodic(getPeriod()); // Restarting the timer - Rule C
+    UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Request timer reset because of Allocation message from %d",
+                 static_cast<int>(msg.getSrcNodeID().get()));
+
+    // Rule D
+    if (!msg.isAnonymousTransfer() &&
+        msg.unique_id.size() > 0 &&
+        msg.unique_id.size() < msg.unique_id.capacity() &&
+        equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_))
+    {
+        // Filling the response message
+        const uint8_t size_of_unique_id_in_response =
+            min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST,
+                static_cast<uint8_t>(msg.unique_id.capacity() - msg.unique_id.size()));
+
+        protocol::dynamic_node_id::Allocation second_stage;
+        second_stage.node_id = preferred_node_id_.get();
+        second_stage.first_part_of_unique_id = false;
+
+        second_stage.unique_id.resize(size_of_unique_id_in_response);
+
+        copy(unique_id_ + msg.unique_id.size(),
+             unique_id_ + msg.unique_id.size() + size_of_unique_id_in_response,
+             second_stage.unique_id.begin());
+
+        UAVCAN_ASSERT(equal(second_stage.unique_id.begin(),
+                            second_stage.unique_id.end(),
+                            unique_id_ + msg.unique_id.size()));
+
+        // Broadcasting the response
+        UAVCAN_TRACE("DynamicNodeIDAllocationClient",
+                     "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d",
+                     static_cast<int>(preferred_node_id_.get()), static_cast<int>(second_stage.unique_id.size()));
+        const int res = dnida_pub_.broadcast(second_stage);
+        if (res < 0)
+        {
+            dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed");
+        }
+    }
+
+    // Rule E
+    if (!msg.isAnonymousTransfer() &&
+        msg.unique_id.size() == msg.unique_id.capacity() &&
+        equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) &&
+        msg.node_id > 0)
+    {
+        allocated_node_id_ = msg.node_id;
+        allocator_node_id_ = msg.getSrcNodeID();
+        UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Allocation complete, node ID %d provided by %d",
+                     static_cast<int>(allocated_node_id_.get()), static_cast<int>(allocator_node_id_.get()));
+        terminate();
+        UAVCAN_ASSERT(isAllocationComplete());
+    }
+}
+
+int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version,
+                                         const NodeID preferred_node_id)
+{
+    terminate();
+
+    // Allocation is not possible if node ID is already set
+    if (dnida_pub_.getNode().getNodeID().isUnicast())
+    {
+        return -ErrLogic;
+    }
+
+    // Unique ID initialization & validation
+    copy(hardware_version.unique_id.begin(), hardware_version.unique_id.end(), unique_id_);
+    bool unique_id_is_zero = true;
+    for (uint8_t i = 0; i < sizeof(unique_id_); i++)
+    {
+        if (unique_id_[i] != 0)
+        {
+            unique_id_is_zero = false;
+            break;
+        }
+    }
+
+    if (unique_id_is_zero)
+    {
+        return -ErrInvalidParam;
+    }
+
+    if (!preferred_node_id.isValid())  // Only broadcast and unicast are allowed
+    {
+        return -ErrInvalidParam;
+    }
+
+    // Initializing the fields
+    preferred_node_id_ = preferred_node_id;
+    allocated_node_id_ = NodeID();
+    allocator_node_id_ = NodeID();
+    UAVCAN_ASSERT(preferred_node_id_.isValid());
+    UAVCAN_ASSERT(!allocated_node_id_.isValid());
+    UAVCAN_ASSERT(!allocator_node_id_.isValid());
+
+    // Initializing node objects - Rule A
+    int res = dnida_pub_.init();
+    if (res < 0)
+    {
+        return res;
+    }
+    dnida_pub_.allowAnonymousTransfers();
+
+    res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation));
+    if (res < 0)
+    {
+        return res;
+    }
+    dnida_sub_.allowAnonymousTransfers();
+
+    startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS));
+
+    return 0;
+}
+
+}

libuavcan/src/transport/uc_frame.cpp

@@ -170,9 +170,12 @@ bool Frame::isValid() const
     const bool invalid =
         (frame_index_ > MaxIndex) ||
         ((frame_index_ == MaxIndex) && !last_frame_) ||
-        (!src_node_id_.isUnicast()) ||
+        (!src_node_id_.isValid()) ||
         (!dst_node_id_.isValid()) ||
-        (src_node_id_ == dst_node_id_) ||
+        (src_node_id_.isUnicast() ? (src_node_id_ == dst_node_id_) : false) ||
+        (src_node_id_.isBroadcast()
+            ? (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast))
+            : false) ||
         ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) ||
         (transfer_type_ >= NumTransferTypes) ||
         (static_cast<int>(payload_len_) > getMaxPayloadLen()) ||

libuavcan/src/transport/uc_transfer_listener.cpp

@@ -50,6 +50,11 @@ int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned l
     return int(len);
 }
 
+bool SingleFrameIncomingTransfer::isAnonymousTransfer() const
+{
+    return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast();
+}
+
 /*
  * MultiFrameIncomingTransfer
  */
@@ -172,6 +177,16 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF
     }
 }
 
+void TransferListenerBase::handleAnonymousTransferReception(const RxFrame& frame)
+{
+    if (allow_anonymous_transfers_)
+    {
+        perf_.addRxTransfer();
+        SingleFrameIncomingTransfer it(frame);
+        handleIncomingTransfer(it);
+    }
+}
+
 void TransferListenerBase::cleanup(MonotonicTime ts)
 {
     receivers_.removeWhere(TimedOutReceiverPredicate(ts, bufmgr_));
@@ -180,26 +195,40 @@ void TransferListenerBase::cleanup(MonotonicTime ts)
 
 void TransferListenerBase::handleFrame(const RxFrame& frame)
 {
-    const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
+    if (frame.getSrcNodeID().isUnicast())       // Normal transfer
-
-    TransferReceiver* recv = receivers_.access(key);
-    if (recv == NULL)
     {
-        if (!frame.isFirst())
+        const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType());
-        {
-            return;
-        }
 
-        TransferReceiver new_recv;
+        TransferReceiver* recv = receivers_.access(key);
-        recv = receivers_.insert(key, new_recv);
         if (recv == NULL)
         {
-            UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
+            if (!frame.isFirst())
-            return;
+            {
+                return;
+            }
+
+            TransferReceiver new_recv;
+            recv = receivers_.insert(key, new_recv);
+            if (recv == NULL)
+            {
+                UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str());
+                return;
+            }
         }
+        TransferBufferAccessor tba(bufmgr_, key);
+        handleReception(*recv, frame, tba);
+    }
+    else if (frame.getSrcNodeID().isBroadcast() &&
+             frame.isFirst() &&
+             frame.isLast() &&
+             frame.getDstNodeID().isBroadcast())        // Anonymous transfer
+    {
+        handleAnonymousTransferReception(frame);
+    }
+    else
+    {
+        UAVCAN_TRACE("TransferListenerBase", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame
     }
-    TransferBufferAccessor tba(bufmgr_, key);
-    handleReception(*recv, frame, tba);
 }
 
 }

libuavcan/src/transport/uc_transfer_sender.cpp

@@ -19,15 +19,35 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic
                          MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id,
                          TransferID tid)
 {
+    if (payload_len > MaxTransferPayloadLen)
+    {
+        UAVCAN_ASSERT(0);
+        return -ErrInvalidParam;
+    }
+
+    Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid);
+    UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str());
+
+    /*
+     * Checking if we're allowed to send.
+     * In passive mode we can send only anonymous transfers, if they are enabled.
+     */
     if (dispatcher_.isPassiveMode())
     {
-        return -ErrPassiveMode;
+        const bool allow = allow_anonymous_transfers_ &&
+                           (transfer_type == TransferTypeMessageBroadcast) &&
+                           (int(payload_len) <= frame.getMaxPayloadLen());
+        if (!allow)
+        {
+            return -ErrPassiveMode;
+        }
     }
 
     dispatcher_.getTransferPerfCounter().addTxTransfer();
 
-    Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid);
+    /*
-
+     * Sending frames
+     */
     if (frame.getMaxPayloadLen() >= int(payload_len))           // Single Frame Transfer
     {
         const int res = frame.setPayload(payload, payload_len);

libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp

@@ -0,0 +1,182 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_allocation_client.hpp>
+#include "helpers.hpp"
+
+
+TEST(DynamicNodeIDAllocationClient, Basic)
+{
+    // Node A is Allocator, Node B is Allocatee
+    InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+    uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
+    (void)_reg1;
+
+    /*
+     * Client initialization
+     */
+    uavcan::protocol::HardwareVersion hwver;
+
+    ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver));  // Empty hardware version is not allowed
+
+    for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
+    {
+        hwver.unique_id[i] = i;
+    }
+
+    ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID()));
+
+    const uavcan::NodeID PreferredNodeID = 42;
+    ASSERT_LE(0, dnidac.start(hwver, PreferredNodeID));
+
+    ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+    ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+    ASSERT_FALSE(dnidac.isAllocationComplete());
+
+    /*
+     * Subscriber (server emulation)
+     */
+    SubscriberWithCollector<uavcan::protocol::dynamic_node_id::Allocation> dynid_sub(nodes.a);
+    ASSERT_LE(0, dynid_sub.start());
+    dynid_sub.subscriber.allowAnonymousTransfers();
+
+    /*
+     * Monitoring requests at 1Hz
+     */
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+    ASSERT_TRUE(dynid_sub.collector.msg.get());
+    std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl;
+    ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+    ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id);
+    ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
+                              dynid_sub.collector.msg->unique_id.end(),
+                              hwver.unique_id.begin()));
+    dynid_sub.collector.msg.reset();
+
+    // Rate validation
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
+    ASSERT_FALSE(dynid_sub.collector.msg.get());
+
+    // Second - rate is 1 Hz
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
+    ASSERT_TRUE(dynid_sub.collector.msg.get());
+    dynid_sub.collector.msg.reset();
+
+    ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+    ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+    ASSERT_FALSE(dnidac.isAllocationComplete());
+
+    /*
+     * Publisher (server emulation)
+     */
+    uavcan::Publisher<uavcan::protocol::dynamic_node_id::Allocation> dynid_pub(nodes.a);
+    ASSERT_LE(0, dynid_pub.init());
+
+    /*
+     * Sending some some Allocation messages - the timer will keep restarting
+     */
+    for (int i = 0; i < 10; i++)
+    {
+        uavcan::protocol::dynamic_node_id::Allocation msg;  // Contents of the message doesn't matter
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210));
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+    }
+
+    /*
+     * Responding with partially matching unique ID - the client will respond with second-stage request immediately
+     */
+    {
+        uavcan::protocol::dynamic_node_id::Allocation msg;
+        msg.unique_id.resize(7);
+        uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 7, msg.unique_id.begin());
+
+        std::cout << "First-stage offer:\n" << msg << std::endl;
+
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+
+        ASSERT_TRUE(dynid_sub.collector.msg.get());
+        std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl;
+        ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+        ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id);
+        ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
+                                  dynid_sub.collector.msg->unique_id.end(),
+                                  hwver.unique_id.begin() + 7));
+        dynid_sub.collector.msg.reset();
+    }
+
+    /*
+     * Responding with second-stage offer, expecting the last request back
+     */
+    {
+        uavcan::protocol::dynamic_node_id::Allocation msg;
+        msg.unique_id.resize(14);
+        uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 14, msg.unique_id.begin());
+
+        std::cout << "Second-stage offer:\n" << msg << std::endl;
+
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+
+        ASSERT_TRUE(dynid_sub.collector.msg.get());
+        std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl;
+        ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id);
+        ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id);
+        ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(),
+                                  dynid_sub.collector.msg->unique_id.end(),
+                                  hwver.unique_id.begin() + 14));
+        dynid_sub.collector.msg.reset();
+    }
+
+    ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid());
+    ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid());
+    ASSERT_FALSE(dnidac.isAllocationComplete());
+
+    /*
+     * Now we have full unique ID for this client received, and it is possible to grant allocation
+     */
+    {
+        uavcan::protocol::dynamic_node_id::Allocation msg;
+        msg.unique_id.resize(16);
+        msg.node_id = 72;
+        uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.end(), msg.unique_id.begin());
+
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+        ASSERT_LE(0, dynid_pub.broadcast(msg));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+        ASSERT_FALSE(dynid_sub.collector.msg.get());
+    }
+
+    ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID());
+    ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID());
+    ASSERT_TRUE(dnidac.isAllocationComplete());
+}
+
+
+TEST(DynamicNodeIDAllocationClient, NonPassiveMode)
+{
+    InterlinkedTestNodesWithSysClock nodes;
+
+    uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b);
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::dynamic_node_id::Allocation> _reg1;
+    (void)_reg1;
+
+    uavcan::protocol::HardwareVersion hwver;
+    for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
+    {
+        hwver.unique_id[i] = i;
+    }
+
+    ASSERT_LE(-uavcan::ErrLogic, dnidac.start(hwver));
+}

libuavcan/test/transport/frame.cpp

@@ -162,7 +162,18 @@ TEST(Frame, FrameParsing)
 
     can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression
              (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19);
-    ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID
+    ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with unicast transfer
+
+    can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression
+             (2 << 0) | (0 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19);
+    ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with multiframe broadcast transfer
+
+    /*
+     * Broadcast SNID exceptions
+     */
+    can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression
+             (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19);
+    ASSERT_TRUE(frame.parse(can)); // Broadcast Src Node ID with single frame broadcast transfer
 }
 
 

libuavcan/test/transport/transfer_listener.cpp

@@ -243,3 +243,38 @@ TEST(TransferListener, MaximumTransferLength)
 
     ASSERT_TRUE(subscriber.isEmpty());
 }
+
+
+TEST(TransferListener, AnonymousTransfers)
+{
+    const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A");
+
+    uavcan::PoolManager<1> poolmgr;
+    uavcan::TransferPerfCounter perf;
+    TestListener<0, 0, 0> subscriber(perf, type, poolmgr);
+
+    TransferListenerEmulator emulator(subscriber, type);
+    const Transfer transfers[] =
+    {
+        emulator.makeTransfer(uavcan::TransferTypeMessageUnicast,   0, "12345678"),  // Invalid - not broadcast
+        emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "12345678"),  // Valid
+        emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "123456789"), // Invalid - not SFT
+        emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "")           // Valid
+    };
+
+    emulator.send(transfers);
+
+    // Nothing will be received, because anonymous transfers are disabled by default
+    ASSERT_TRUE(subscriber.isEmpty());
+
+    subscriber.allowAnonymousTransfers();
+
+    // Re-send everything again
+    emulator.send(transfers);
+
+    // Now the anonymous transfers are enabled
+    ASSERT_TRUE(subscriber.matchAndPop(transfers[1]));    // Only SFT broadcast will be accepted
+    ASSERT_TRUE(subscriber.matchAndPop(transfers[3]));
+
+    ASSERT_TRUE(subscriber.isEmpty());
+}

libuavcan/test/transport/transfer_sender.cpp

@@ -235,11 +235,24 @@ TEST(TransferSender, PassiveMode)
 
     static const uint8_t Payload[] = {1, 2, 3, 4, 5};
 
+    // By default, sending in passive mode is not enabled
     ASSERT_EQ(-uavcan::ErrPassiveMode,
               sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
                           uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast));
 
+    // Overriding the default
+    sender.allowAnonymousTransfers();
+
+    // OK, now we can broadcast in any mode
+    ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
+                             uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast));
+
+    // ...but not unicast or anything else
+    ASSERT_EQ(-uavcan::ErrPassiveMode,
+              sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(),
+                          uavcan::TransferTypeMessageUnicast, uavcan::NodeID(42)));
+
     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount());
-    EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount());
+    EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount());
     EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount());
 }

libuavcan/test/transport/transfer_test_helpers.hpp

@@ -129,6 +129,14 @@ public:
         const Transfer rx(transfer, Base::getDataTypeDescriptor());
         transfers_.push(rx);
         std::cout << "Received transfer: " << rx.toString() << std::endl;
+
+        const bool single_frame = dynamic_cast<uavcan::SingleFrameIncomingTransfer*>(&transfer) != NULL;
+
+        const bool anonymous = single_frame &&
+                               transfer.getSrcNodeID().isBroadcast() &&
+                               (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast);
+
+        ASSERT_EQ(anonymous, transfer.isAnonymousTransfer());
     }
 
     bool matchAndPop(const Transfer& reference)