Merge branch 'dynamic_node_id_raft' of https://github.com/UAVCAN/uavcan into dynamic_node_id_raft

dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan

@@ -4,11 +4,14 @@
 #
 
 #
-# Given min election timeout and cluster size, the maximum request interval can be derived as follows:
-#   max request interval = (min election timeout) / 2 requests / (cluster size - 1)
-# Obviously, request interval can be lower than that if needed, but not higher.
+# Given min election timeout and cluster size, the maximum recommended request interval can be derived as follows:
+#   max recommended request interval = (min election timeout) / 2 requests / (cluster size - 1)
+# The equation assumes that the Leader requests one Follower at a time, so that there's at most one pending call
+# at any moment. Such behavior is optimal as it creates uniform bus load, but it is actually implementation-specific.
+# Obviously, request interval can be lower than that if needed, but higher values are not recommended as they may
+# cause Followers to initiate premature elections in case of intensive frame losses or delays.
 #
-# Real timeout is randomized in the range (MIN, MAX].
+# Real timeout is randomized in the range (MIN, MAX], according to the Raft paper.
 #
 uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000
 uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000
@@ -19,8 +22,8 @@ uint8 prev_log_index
 uint8 leader_commit
 
 #
-# Worst-case replication time can be computed as:
-#   worst replication time = (127 log entries) * (2 trips of next_index) * (cluster size - 1) * (request interval)
+# Worst-case replication time per Follower can be computed as:
+#   worst replication time = (127 log entries) * (2 trips of next_index) * (request interval per Follower)
 #
 Entry[<=1] entries
 

libuavcan/include/uavcan/node/abstract_node.hpp

@@ -76,6 +76,18 @@ public:
     {
         return getScheduler().spin(getMonotonicTime() + duration);
     }
+
+    /**
+     * This method is designed for non-blocking applications.
+     * Instead of blocking, it returns immediately once all available CAN frames and timer events are processed.
+     * Note that this is unlike plain @ref spin(), which will strictly return when the deadline is reached,
+     * even if there still are unprocessed events.
+     * This method returns 0 if no errors occurred, or a negative error code if something failed (see error.hpp).
+     */
+    int spinOnce()
+    {
+        return getScheduler().spinOnce();
+    }
 };
 
 }

libuavcan/include/uavcan/node/scheduler.hpp

@@ -77,6 +77,17 @@ class UAVCAN_EXPORT Scheduler : Noncopyable
     MonotonicDuration cleanup_period_;
     bool inside_spin_;
 
+    struct InsideSpinSetter
+    {
+        Scheduler& owner;
+        InsideSpinSetter(Scheduler& o)
+            : owner(o)
+        {
+            owner.inside_spin_ = true;
+        }
+        ~InsideSpinSetter() { owner.inside_spin_ = false; }
+    };
+
     MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const;
     void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin);
 
@@ -91,10 +102,18 @@ public:
 
     /**
      * Spin until the deadline, or until some error occurs.
+     * This function will return strictly when the deadline is reached, even if there are unprocessed frames.
      * Returns negative error code.
      */
     int spin(MonotonicTime deadline);
 
+    /**
+     * Non-blocking version of @ref spin() - spins until all pending frames and events are processed,
+     * or until some error occurs. If there's nothing to do, returns immediately.
+     * Returns negative error code.
+     */
+    int spinOnce();
+
     DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; }
 
     Dispatcher& getDispatcher()             { return dispatcher_; }

libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp

@@ -0,0 +1,20 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED
+
+#include <uavcan/protocol/dynamic_node_id_server/centralized/server.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+
+typedef centralized::Server CentralizedServer;
+
+}
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED

libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp

@@ -0,0 +1,208 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace centralized
+{
+/**
+ * This server is an alternative to @ref DistributedServer with the following differences:
+ *  - It is not distributed, so using it means introducing a single point of failure into the system.
+ *  - It takes less code space and requires less RAM, which makes it suitable for resource-constrained applications.
+ *
+ * This version is suitable only for simple non-critical systems.
+ */
+class Server : IAllocationRequestHandler
+             , INodeDiscoveryHandler
+{
+    UniqueID own_unique_id_;
+
+    INode& node_;
+    IEventTracer& tracer_;
+    AllocationRequestManager allocation_request_manager_;
+    NodeDiscoverer node_discoverer_;
+    Storage storage_;
+
+    /*
+     * Private methods
+     */
+    bool isNodeIDTaken(const NodeID node_id) const
+    {
+        return storage_.findByNodeID(node_id) != NULL;
+    }
+
+    void tryPublishAllocationResult(const Storage::Entry& entry)
+    {
+        const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id);
+        if (res < 0)
+        {
+            tracer_.onEvent(TraceError, res);
+            node_.registerInternalFailure("Dynamic allocation response");
+        }
+    }
+
+    /*
+     * Methods of IAllocationRequestHandler
+     */
+    virtual bool canPublishFollowupAllocationResponse() const
+    {
+        return true;    // Because there's only one Centralized server in the system
+    }
+
+    virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id)
+    {
+        const Storage::Entry* const e = storage_.findByUniqueID(unique_id);
+        if (e != NULL)
+        {
+            tryPublishAllocationResult(*e);
+        }
+        else
+        {
+            const NodeID allocated_node_id =
+                NodeIDSelector<Server>(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id);
+
+            if (allocated_node_id.isUnicast())
+            {
+                const int res = storage_.add(allocated_node_id, unique_id);
+                if (res >= 0)
+                {
+                    tryPublishAllocationResult(Storage::Entry(allocated_node_id, unique_id));
+                }
+                else
+                {
+                    tracer_.onEvent(TraceError, res);
+                    node_.registerInternalFailure("CentralizedServer storage add");
+                }
+            }
+            else
+            {
+                UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left");
+            }
+        }
+    }
+
+    /*
+     * Methods of INodeDiscoveryHandler
+     */
+    virtual bool canDiscoverNewNodes() const
+    {
+        return true;    // Because there's only one Centralized server in the system
+    }
+
+    virtual NodeAwareness checkNodeAwareness(NodeID node_id) const
+    {
+        return (storage_.findByNodeID(node_id) == NULL) ? NodeAwarenessUnknown : NodeAwarenessKnownAndCommitted;
+    }
+
+    virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id)
+    {
+        if (storage_.findByNodeID(node_id) != NULL)
+        {
+            UAVCAN_ASSERT(0);   // Such node is already known, the class that called this method should have known that
+            return;
+        }
+
+        const int res = storage_.add(node_id, (unique_id_or_null == NULL) ? UniqueID() : *unique_id_or_null);
+        if (res < 0)
+        {
+            tracer_.onEvent(TraceError, res);
+            node_.registerInternalFailure("CentralizedServer storage add");
+        }
+    }
+
+public:
+    Server(INode& node,
+           IStorageBackend& storage,
+           IEventTracer& tracer)
+        : node_(node)
+        , tracer_(tracer)
+        , allocation_request_manager_(node, tracer, *this)
+        , node_discoverer_(node, tracer, *this)
+        , storage_(storage)
+    { }
+
+    int init(const UniqueID& own_unique_id)
+    {
+        /*
+         * Initializing storage first, because the next step requires it to be loaded
+         */
+        int res = storage_.init();
+        if (res < 0)
+        {
+            return res;
+        }
+
+        /*
+         * Making sure that the server is started with the same node ID
+         */
+        own_unique_id_ = own_unique_id;
+
+        {
+            const Storage::Entry* e = storage_.findByNodeID(node_.getNodeID());
+            if (e != NULL)
+            {
+                if (e->unique_id != own_unique_id_)
+                {
+                    return -ErrInvalidConfiguration;
+                }
+            }
+
+            e = storage_.findByUniqueID(own_unique_id_);
+            if (e != NULL)
+            {
+                if (e->node_id != node_.getNodeID())
+                {
+                    return -ErrInvalidConfiguration;
+                }
+            }
+
+            if (e == NULL)
+            {
+                res = storage_.add(node_.getNodeID(), own_unique_id_);
+                if (res < 0)
+                {
+                    return res;
+                }
+            }
+        }
+
+        /*
+         * Misc
+         */
+        res = allocation_request_manager_.init();
+        if (res < 0)
+        {
+            return res;
+        }
+
+        res = node_discoverer_.init();
+        if (res < 0)
+        {
+            return res;
+        }
+
+        return 0;
+    }
+
+    Storage::Size getNumAllocations() const { return storage_.getSize(); }
+};
+
+}
+}
+}
+
+#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED

libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp

@@ -0,0 +1,253 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp>
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+
+namespace uavcan
+{
+namespace dynamic_node_id_server
+{
+namespace centralized
+{
+/**
+ * This class transparently replicates its state to the storage backend, keeping the most recent state in memory.
+ * Writes are slow, reads are instantaneous.
+ */
+class Storage
+{
+public:
+    typedef uint8_t Size;
+
+    enum { Capacity = NodeID::Max };
+
+    struct Entry
+    {
+        UniqueID unique_id;
+        NodeID node_id;
+
+        Entry() { }
+
+        Entry(const NodeID nid, const UniqueID& uid)
+            : unique_id(uid)
+            , node_id(nid)
+        { }
+
+        bool operator==(const Entry& rhs) const
+        {
+            return unique_id == rhs.unique_id &&
+                   node_id   == rhs.node_id;
+        }
+    };
+
+private:
+    IStorageBackend& storage_;
+    Entry entries_[Capacity];
+    Size size_;
+
+    static IStorageBackend::String getSizeKey() { return "size"; }
+
+    static IStorageBackend::String makeEntryKey(Size index, const char* postfix)
+    {
+        IStorageBackend::String str;
+        // "0_foobar"
+        str.appendFormatted("%d", int(index));
+        str += "_";
+        str += postfix;
+        return str;
+    }
+
+    int readEntryFromStorage(Size index, Entry& out_entry)
+    {
+        const StorageMarshaller io(storage_);
+
+        // Unique ID
+        if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0)
+        {
+            return -ErrFailure;
+        }
+
+        // Node ID
+        uint32_t node_id = 0;
+        if (io.get(makeEntryKey(index, "node_id"), node_id) < 0)
+        {
+            return -ErrFailure;
+        }
+        if (node_id > NodeID::Max || node_id == 0)
+        {
+            return -ErrFailure;
+        }
+        out_entry.node_id = NodeID(static_cast<uint8_t>(node_id));
+
+        return 0;
+    }
+
+    int writeEntryToStorage(Size index, const Entry& entry)
+    {
+        Entry temp = entry;
+
+        StorageMarshaller io(storage_);
+
+        // Unique ID
+        if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0)
+        {
+            return -ErrFailure;
+        }
+
+        // Node ID
+        uint32_t node_id = entry.node_id.get();
+        if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0)
+        {
+            return -ErrFailure;
+        }
+        temp.node_id = NodeID(static_cast<uint8_t>(node_id));
+
+        return (temp == entry) ? 0 : -ErrFailure;
+    }
+
+public:
+    Storage(IStorageBackend& storage)
+        : storage_(storage)
+        , size_(0)
+    { }
+
+    /**
+     * This method reads all entries from the storage.
+     */
+    int init()
+    {
+        StorageMarshaller io(storage_);
+
+        // Reading size
+        size_ = 0;
+        {
+            uint32_t value = 0;
+            if (io.get(getSizeKey(), value) < 0)
+            {
+                if (storage_.get(getSizeKey()).empty())
+                {
+                    int res = io.setAndGetBack(getSizeKey(), value);
+                    if (res < 0)
+                    {
+                        return res;
+                    }
+                    return (value == 0) ? 0 : -ErrFailure;
+                }
+                else
+                {
+                    // There's some data in the storage, but it cannot be parsed - reporting an error
+                    return -ErrFailure;
+                }
+            }
+
+            if (value > Capacity)
+            {
+                return -ErrFailure;
+            }
+
+            size_ = Size(value);
+        }
+
+        // Restoring entries
+        for (Size index = 0; index < size_; index++)
+        {
+            const int result = readEntryFromStorage(index, entries_[index]);
+            if (result < 0)
+            {
+                return result;
+            }
+        }
+
+        return 0;
+    }
+
+    /**
+     * This method invokes storage IO.
+     * Returned value indicates whether the entry was successfully appended.
+     */
+    int add(const NodeID node_id, const UniqueID& unique_id)
+    {
+        if (size_ == Capacity)
+        {
+            return -ErrLogic;
+        }
+
+        if (!node_id.isUnicast())
+        {
+            return -ErrInvalidParam;
+        }
+
+        Entry entry;
+        entry.node_id = node_id;
+        entry.unique_id = unique_id;
+
+        // If next operations fail, we'll get a dangling entry, but it's absolutely OK.
+        int res = writeEntryToStorage(size_, entry);
+        if (res < 0)
+        {
+            return res;
+        }
+
+        // Updating the size
+        StorageMarshaller io(storage_);
+        uint32_t new_size_index = size_ + 1U;
+        res = io.setAndGetBack(getSizeKey(), new_size_index);
+        if (res < 0)
+        {
+            return res;
+        }
+        if (new_size_index != size_ + 1U)
+        {
+            return -ErrFailure;
+        }
+
+        entries_[size_] = entry;
+        size_++;
+
+        return 0;
+    }
+
+    /**
+     * Returns nullptr if there's no such entry.
+     */
+    const Entry* findByNodeID(const NodeID node_id) const
+    {
+        for (Size i = 0; i < size_; i++)
+        {
+            if (entries_[i].node_id == node_id)
+            {
+                return &entries_[i];
+            }
+        }
+        return NULL;
+    }
+
+    /**
+     * Returns nullptr if there's no such entry.
+     */
+    const Entry* findByUniqueID(const UniqueID& unique_id) const
+    {
+        for (Size i = 0; i < size_; i++)
+        {
+            if (entries_[i].unique_id == unique_id)
+            {
+                return &entries_[i];
+            }
+        }
+        return NULL;
+    }
+
+    Size getSize() const { return size_; }
+};
+
+}
+}
+}
+
+#endif // Include guard

libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp

@@ -149,14 +149,6 @@ public:
         , last_index_(0)
     { }
 
-    /**
-     * Initialization is performed as follows (every step may fail and return an error):
-     *  1. Log is restored or initialized.
-     *  2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized
-     *     with zero.
-     *  3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is
-     *     zero, the value will be initialized with zero.
-     */
     int init()
     {
         StorageMarshaller io(storage_);

libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp

@@ -42,6 +42,14 @@ public:
         , log_(storage, tracer)
     { }
 
+    /**
+     * Initialization is performed as follows (every step may fail and return an error):
+     *  1. Log is restored or initialized.
+     *  2. Current term is restored. If there was no current term stored and the log is empty, it will be initialized
+     *     with zero.
+     *  3. VotedFor value is restored. If there was no VotedFor value stored, the log is empty, and the current term is
+     *     zero, the value will be initialized with zero.
+     */
     int init()
     {
         /*

libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp

@@ -7,7 +7,6 @@
 
 #include <uavcan/build_config.hpp>
 #include <uavcan/debug.hpp>
-#include <uavcan/node/timer.hpp>
 #include <uavcan/protocol/dynamic_node_id_server/distributed/types.hpp>
 #include <uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp>
 #include <uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp>
@@ -65,6 +64,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler
      * States
      */
     INode& node_;
+    IEventTracer& tracer_;
     RaftCore raft_core_;
     AllocationRequestManager allocation_request_manager_;
     NodeDiscoverer node_discoverer_;
@@ -230,7 +230,8 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler
         const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id);
         if (res < 0)
         {
-            node_.registerInternalFailure("Dynamic allocation final broadcast");
+            tracer_.onEvent(TraceError, res);
+            node_.registerInternalFailure("Dynamic allocation response");
         }
     }
 
@@ -239,6 +240,7 @@ public:
            IStorageBackend& storage,
            IEventTracer& tracer)
         : node_(node)
+        , tracer_(tracer)
         , raft_core_(node, storage, tracer, *this)
         , allocation_request_manager_(node, tracer, *this)
         , node_discoverer_(node, tracer, *this)

libuavcan/include/uavcan/transport/dispatcher.hpp

@@ -119,8 +119,16 @@ public:
         , self_node_id_is_set_(false)
     { }
 
+    /**
+     * This version returns strictly when the deadline is reached.
+     */
     int spin(MonotonicTime deadline);
 
+    /**
+     * This version does not return until all available frames are processed.
+     */
+    int spinOnce();
+
     /**
      * Refer to CanIOManager::send() for the parameter description
      */

libuavcan/src/node/uc_scheduler.cpp

@@ -159,7 +159,8 @@ int Scheduler::spin(MonotonicTime deadline)
         UAVCAN_ASSERT(0);
         return -ErrRecursiveCall;
     }
-    inside_spin_ = true;
+    InsideSpinSetter iss(*this);
+    UAVCAN_ASSERT(inside_spin_);
 
     int retval = 0;
     while (true)
@@ -179,7 +180,28 @@ int Scheduler::spin(MonotonicTime deadline)
         }
     }
 
-    inside_spin_ = false;
+    return retval;
+}
+
+int Scheduler::spinOnce()
+{
+    if (inside_spin_)  // Preventing recursive calls
+    {
+        UAVCAN_ASSERT(0);
+        return -ErrRecursiveCall;
+    }
+    InsideSpinSetter iss(*this);
+    UAVCAN_ASSERT(inside_spin_);
+
+    const int retval = dispatcher_.spinOnce();
+    if (retval < 0)
+    {
+        return retval;
+    }
+
+    const MonotonicTime ts = deadline_scheduler_.pollAndGetMonotonicTime(getSystemClock());
+    pollCleanup(ts, unsigned(retval));
+
     return retval;
 }
 

libuavcan/src/transport/uc_dispatcher.cpp

@@ -229,6 +229,40 @@ int Dispatcher::spin(MonotonicTime deadline)
     return num_frames_processed;
 }
 
+int Dispatcher::spinOnce()
+{
+    int num_frames_processed = 0;
+
+    while (true)
+    {
+        CanIOFlags flags = 0;
+        CanRxFrame frame;
+        const int res = canio_.receive(frame, MonotonicTime(), flags);
+        if (res < 0)
+        {
+            return res;
+        }
+        else if (res > 0)
+        {
+            if (flags & CanIOFlagLoopback)
+            {
+                handleLoopbackFrame(frame);
+            }
+            else
+            {
+                num_frames_processed++;
+                handleFrame(frame);
+            }
+        }
+        else
+        {
+            break;      // No frames left
+        }
+    }
+
+    return num_frames_processed;
+}
+
 int Dispatcher::send(const Frame& frame, MonotonicTime tx_deadline, MonotonicTime blocking_deadline,
                      CanTxQueue::Qos qos, CanIOFlags flags, uint8_t iface_mask)
 {

libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp

@@ -0,0 +1,77 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include "../../helpers.hpp"
+#include "../event_tracer.hpp"
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+using uavcan::dynamic_node_id_server::UniqueID;
+
+
+TEST(dynamic_node_id_server_centralized_Server, Basic)
+{
+    using namespace uavcan::dynamic_node_id_server;
+    using namespace uavcan::protocol::dynamic_node_id;
+    using namespace uavcan::protocol::dynamic_node_id::server;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<Allocation> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
+
+    EventTracer tracer;
+    MemoryStorageBackend storage;
+
+    // Node A is Allocator, Node B is Allocatee
+    InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+    UniqueID own_unique_id;
+    own_unique_id[0] = 0xAA;
+    own_unique_id[3] = 0xCC;
+    own_unique_id[7] = 0xEE;
+    own_unique_id[9] = 0xBB;
+
+    /*
+     * Server
+     */
+    uavcan::dynamic_node_id_server::CentralizedServer server(nodes.a, storage, tracer);
+
+    ASSERT_LE(0, server.init(own_unique_id));
+
+    ASSERT_EQ(1, server.getNumAllocations());   // Server's own node ID
+
+    /*
+     * Client
+     */
+    uavcan::DynamicNodeIDClient client(nodes.b);
+    uavcan::protocol::HardwareVersion hwver;
+    for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++)
+    {
+        hwver.unique_id[i] = i;
+    }
+    const uavcan::NodeID PreferredNodeID = 42;
+    ASSERT_LE(0, client.start(hwver, PreferredNodeID));
+
+    /*
+     * Fire
+     */
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000));
+
+    ASSERT_TRUE(client.isAllocationComplete());
+    ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
+
+    ASSERT_EQ(2, server.getNumAllocations());   // Server's own node ID + client
+}
+
+
+TEST(dynamic_node_id_server_centralized, ObjectSizes)
+{
+    using namespace uavcan::dynamic_node_id_server;
+    std::cout << "centralized::Storage: " << sizeof(centralized::Storage) << std::endl;
+    std::cout << "centralized::Server:  " << sizeof(centralized::Server) << std::endl;
+}

libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp

@@ -0,0 +1,174 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp>
+#include "../../helpers.hpp"
+#include "../memory_storage_backend.hpp"
+
+
+TEST(dynamic_node_id_server_centralized_Storage, Initialization)
+{
+    using namespace uavcan::dynamic_node_id_server::centralized;
+
+    // No data in the storage - initializing empty
+    {
+        MemoryStorageBackend storage;
+        Storage stor(storage);
+
+        ASSERT_EQ(0, storage.getNumKeys());
+        ASSERT_LE(0, stor.init());
+
+        ASSERT_EQ(1, storage.getNumKeys());
+        ASSERT_EQ(0, stor.getSize());
+
+        ASSERT_FALSE(stor.findByNodeID(1));
+        ASSERT_FALSE(stor.findByNodeID(0));
+    }
+    // Nonempty storage, one item
+    {
+        MemoryStorageBackend storage;
+        Storage stor(storage);
+
+        storage.set("size", "1");
+        ASSERT_EQ(-uavcan::ErrFailure, stor.init());     // Expected one entry, none found
+        ASSERT_EQ(1, storage.getNumKeys());
+
+        storage.set("0_unique_id", "00000000000000000000000000000000");
+        storage.set("0_node_id",   "0");
+        ASSERT_EQ(-uavcan::ErrFailure, stor.init());     // Invalid entry - zero Node ID
+
+        storage.set("0_node_id",   "1");
+        ASSERT_LE(0, stor.init());                       // OK now
+
+        ASSERT_EQ(3, storage.getNumKeys());
+        ASSERT_EQ(1, stor.getSize());
+
+        ASSERT_TRUE(stor.findByNodeID(1));
+        ASSERT_FALSE(stor.findByNodeID(0));
+    }
+    // Nonempty storage, broken data
+    {
+        MemoryStorageBackend storage;
+        Storage stor(storage);
+
+        storage.set("size", "foobar");
+        ASSERT_EQ(-uavcan::ErrFailure, stor.init());     // Bad value
+
+        storage.set("size", "128");
+        ASSERT_EQ(-uavcan::ErrFailure, stor.init());     // Bad value
+
+        storage.set("size", "1");
+        ASSERT_EQ(-uavcan::ErrFailure, stor.init());     // No items
+        ASSERT_EQ(1, storage.getNumKeys());
+
+        storage.set("0_unique_id", "00000000000000000000000000000000");
+        storage.set("0_node_id",   "128");               // Bad value (127 max)
+        ASSERT_EQ(-uavcan::ErrFailure, stor.init());     // Failed
+
+        storage.set("0_node_id",   "127");
+        ASSERT_LE(0, stor.init());                       // OK now
+        ASSERT_EQ(1, stor.getSize());
+
+        ASSERT_TRUE(stor.findByNodeID(127));
+        ASSERT_FALSE(stor.findByNodeID(0));
+
+        ASSERT_EQ(3, storage.getNumKeys());
+    }
+    // Nonempty storage, many items
+    {
+        MemoryStorageBackend storage;
+        Storage stor(storage);
+
+        storage.set("size",        "2");
+        storage.set("0_unique_id", "00000000000000000000000000000000");
+        storage.set("0_node_id",   "1");
+        storage.set("1_unique_id", "0123456789abcdef0123456789abcdef");
+        storage.set("1_node_id",   "127");
+
+        ASSERT_LE(0, stor.init());                       // OK now
+        ASSERT_EQ(5, storage.getNumKeys());
+        ASSERT_EQ(2, stor.getSize());
+
+        ASSERT_TRUE(stor.findByNodeID(1));
+        ASSERT_TRUE(stor.findByNodeID(127));
+        ASSERT_FALSE(stor.findByNodeID(0));
+
+        uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid;
+        uid[0] = 0x01;
+        uid[1] = 0x23;
+        uid[2] = 0x45;
+        uid[3] = 0x67;
+        uid[4] = 0x89;
+        uid[5] = 0xab;
+        uid[6] = 0xcd;
+        uid[7] = 0xef;
+        uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8);
+
+        ASSERT_TRUE(stor.findByUniqueID(uid));
+        ASSERT_EQ(127, stor.findByUniqueID(uid)->node_id.get());
+        ASSERT_EQ(uid, stor.findByNodeID(127)->unique_id);
+    }
+}
+
+
+TEST(dynamic_node_id_server_centralized_Storage, Basic)
+{
+    using namespace uavcan::dynamic_node_id_server::centralized;
+
+    MemoryStorageBackend storage;
+    Storage stor(storage);
+
+    ASSERT_EQ(0, storage.getNumKeys());
+    ASSERT_LE(0, stor.init());
+    storage.print();
+    ASSERT_EQ(1, storage.getNumKeys());
+
+    /*
+     * Adding one entry to the log, making sure it appears in the storage
+     */
+    Storage::Entry entry;
+    entry.node_id = 1;
+    entry.unique_id[0] = 1;
+    ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id));
+
+    ASSERT_EQ("1",                                storage.get("size"));
+    ASSERT_EQ("01000000000000000000000000000000", storage.get("0_unique_id"));
+    ASSERT_EQ("1",                                storage.get("0_node_id"));
+
+    ASSERT_EQ(3, storage.getNumKeys());
+    ASSERT_EQ(1, stor.getSize());
+
+    /*
+     * Adding another entry while storage is failing
+     */
+    storage.failOnSetCalls(true);
+
+    ASSERT_EQ(3, storage.getNumKeys());
+
+    entry.node_id = 2;
+    entry.unique_id[0] = 2;
+    ASSERT_GT(0, stor.add(entry.node_id, entry.unique_id));
+
+    ASSERT_EQ(3, storage.getNumKeys());  // No new entries, we failed
+
+    ASSERT_EQ(1, stor.getSize());
+
+    /*
+     * Making sure add() fails when the log is full
+     */
+    storage.failOnSetCalls(false);
+
+    while (stor.getSize() < stor.Capacity)
+    {
+        ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id));
+
+        entry.node_id = uint8_t(uavcan::min(entry.node_id.get() + 1U, 127U));
+        entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U);
+    }
+
+    ASSERT_GT(0, stor.add(123, entry.unique_id));  // Failing because full
+
+    storage.print();
+}

libuavcan/test/transport/dispatcher.cpp

@@ -163,7 +163,7 @@ TEST(Dispatcher, Reception)
 
     while (true)
     {
-        const int res = dispatcher.spin(tsMono(0));
+        const int res = dispatcher.spinOnce();
         ASSERT_LE(0, res);
         clockmock.advance(100);
         if (res == 0)
@@ -298,7 +298,7 @@ TEST(Dispatcher, Spin)
     ASSERT_EQ(100, clockmock.monotonic);
     ASSERT_EQ(0, dispatcher.spin(tsMono(1000)));
     ASSERT_LE(1000, clockmock.monotonic);
-    ASSERT_EQ(0, dispatcher.spin(tsMono(0)));
+    ASSERT_EQ(0, dispatcher.spinOnce());
     ASSERT_LE(1000, clockmock.monotonic);
     ASSERT_EQ(0, dispatcher.spin(tsMono(1100)));
     ASSERT_LE(1100, clockmock.monotonic);