Proper allocation logic with basic test

libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp

@@ -460,6 +460,11 @@ public:
      */
     virtual void onEntryCommitted(const Entry& entry) = 0;
 
+    /**
+     * Assume false by default.
+     */
+    virtual void onLeaderChange(bool local_node_is_leader) = 0;
+
     virtual ~ILeaderLogCommitHandler() { }
 };
 
@@ -655,7 +660,7 @@ public:
     inline LazyConstructor<LogEntryInfo> traverseLogFromEndUntil(const Predicate& predicate) const
     {
         UAVCAN_ASSERT(try_implicit_cast<bool>(predicate, true));
-        for (int index = static_cast<int>(persistent_state_.getLog().getLastIndex()); index--; index >= 0)
+        for (int index = static_cast<int>(persistent_state_.getLog().getLastIndex()); index >= 0; index--)
         {
             const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index));
             UAVCAN_ASSERT(entry != NULL);
@@ -737,8 +742,8 @@ public:
 /**
  * This class implements the top-level allocation logic and server API.
  */
-class DynamicNodeIDAllocationServer : public dynamic_node_id_server_impl::IAllocationRequestHandler
+class DynamicNodeIDAllocationServer : private dynamic_node_id_server_impl::IAllocationRequestHandler
-                                    , public dynamic_node_id_server_impl::ILeaderLogCommitHandler
+                                    , private dynamic_node_id_server_impl::ILeaderLogCommitHandler
 {
     typedef MethodBinder<DynamicNodeIDAllocationServer*,
                          void (DynamicNodeIDAllocationServer::*)
@@ -767,10 +772,19 @@ class DynamicNodeIDAllocationServer : public dynamic_node_id_server_impl::IAlloc
 
     INode& getNode() { return get_node_info_client_.getNode(); }
 
-    virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id);
+    bool isNodeIDTaken(const NodeID node_id) const;
+    NodeID findFreeNodeID(const NodeID node_id) const;
+
+    void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id);
+
+    virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id);
 
     virtual void onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry);
 
+    virtual void onLeaderChange(bool local_node_is_leader);
+
+    void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry);
+
 public:
     DynamicNodeIDAllocationServer(INode& node,
                                   IDynamicNodeIDStorageBackend& storage,

libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp

@@ -1003,6 +1003,11 @@ void RaftCore::updateCandidate()
 
 void RaftCore::updateLeader()
 {
+    if (cluster_.getClusterSize() == 1)
+    {
+        setActiveMode(false);           // Haha
+    }
+
     if (active_mode_ || (next_server_index_ > 0))
     {
         const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_);
@@ -1062,6 +1067,12 @@ void RaftCore::switchState(const ServerState new_state)
                      int(server_state_), int(new_state));
         trace(TraceRaftStateSwitch, new_state);
 
+        if ((ServerStateLeader == server_state_) ||
+            (ServerStateLeader == new_state))
+        {
+            log_commit_handler_.onLeaderChange(ServerStateLeader == new_state);
+        }
+
         server_state_ = new_state;
 
         cluster_.resetAllServerIndices();
@@ -1721,18 +1732,137 @@ int AllocationRequestManager::broadcastAllocationResponse(const IAllocationReque
     return allocation_pub_.broadcast(msg);
 }
 
+/*
+ * There's no reason to remove these types from the class scope, except that otherwise the half-broken Eclipse CDT
+ * indexer goes bananas.
+ */
+struct UniqueIDLogPredicate
+{
+    const IAllocationRequestHandler::UniqueID unique_id;
+
+    UniqueIDLogPredicate(const IAllocationRequestHandler::UniqueID& uid)
+        : unique_id(uid)
+    { }
+
+    bool operator()(const RaftCore::LogEntryInfo& info) const
+    {
+        return info.entry.unique_id == unique_id;
+    }
+};
+
+struct NodeIDLogPredicate
+{
+    const NodeID node_id;
+
+    NodeIDLogPredicate(const NodeID& nid)
+        : node_id(nid)
+    { }
+
+    bool operator()(const RaftCore::LogEntryInfo& info) const
+    {
+        return info.entry.node_id == node_id.get();
+    }
+};
+
 } // dynamic_node_id_server_impl
 
 /*
  * DynamicNodeIDAllocationServer
  */
-void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id)
+bool DynamicNodeIDAllocationServer::isNodeIDTaken(const NodeID node_id) const
 {
-    // TODO implement proper allocation logic
+    UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Testing if node ID %d is taken", int(node_id.get()));
-    (void)raft_core_.appendLog(unique_id, preferred_node_id);
+    return raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::NodeIDLogPredicate(node_id));
+}
+
+NodeID DynamicNodeIDAllocationServer::findFreeNodeID(const NodeID preferred_node_id) const
+{
+    uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max;
+
+    // Up
+    while (candidate <= NodeID::Max)
+    {
+        if (!isNodeIDTaken(candidate))
+        {
+            return candidate;
+        }
+        candidate++;
+    }
+
+    candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max;
+    candidate--;        // This has been tested already
+
+    // Down
+    while (candidate > 0)
+    {
+        if (!isNodeIDTaken(candidate))
+        {
+            return candidate;
+        }
+        candidate--;
+    }
+
+    return NodeID();
+}
+
+void DynamicNodeIDAllocationServer::allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id)
+{
+    const NodeID allocated_node_id = findFreeNodeID(preferred_node_id);
+    if (!allocated_node_id.isUnicast())
+    {
+        UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Request ignored - no free node ID left");
+        return;
+    }
+
+    UAVCAN_TRACE("DynamicNodeIDAllocationServer", "New node ID allocated: %d", int(allocated_node_id.get()));
+    const int res = raft_core_.appendLog(unique_id, allocated_node_id);
+    if (res < 0)
+    {
+        getNode().registerInternalFailure("Raft log append");
+    }
+}
+
+void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id)
+{
+    // TODO: allocation requests must not be served if the list of unidentified nodes is not empty
+
+    const LazyConstructor<dynamic_node_id_server_impl::RaftCore::LogEntryInfo> result =
+        raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::UniqueIDLogPredicate(unique_id));
+
+     if (result.isConstructed())
+     {
+         if (result->committed)
+         {
+             tryPublishAllocationResult(result->entry);
+             UAVCAN_TRACE("DynamicNodeIDAllocationServer",
+                          "Allocation request served with existing allocation; node ID %d",
+                          int(result->entry.node_id));
+         }
+         else
+         {
+             UAVCAN_TRACE("DynamicNodeIDAllocationServer",
+                          "Allocation request ignored - allocation exists but not committed yet; node ID %d",
+                          int(result->entry.node_id));
+         }
+     }
+     else
+     {
+         allocateNewNode(unique_id, preferred_node_id);
+     }
 }
 
 void DynamicNodeIDAllocationServer::onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry)
+{
+    tryPublishAllocationResult(entry);
+}
+
+void DynamicNodeIDAllocationServer::onLeaderChange(bool local_node_is_leader)
+{
+    UAVCAN_TRACE("DynamicNodeIDAllocationServer", "I am leader: %d", int(local_node_is_leader));
+    allocation_request_manager_.setActive(local_node_is_leader);
+}
+
+void DynamicNodeIDAllocationServer::tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry)
 {
     const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id);
     if (res < 0)

libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp

@@ -85,6 +85,11 @@ class CommitHandler : public uavcan::dynamic_node_id_server_impl::ILeaderLogComm
         std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl;
     }
 
+    virtual void onLeaderChange(bool local_node_is_leader)
+    {
+        std::cout << "I AM LEADER: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl;
+    }
+
 public:
     CommitHandler(const std::string& id) : id_(id) { }
 };
@@ -1040,6 +1045,53 @@ TEST(DynamicNodeIDAllocationServer, AllocationRequestManager)
 }
 
 
+TEST(DynamicNodeIDAllocationServer, Main)
+{
+    using namespace uavcan::dynamic_node_id_server_impl;
+    using namespace uavcan::protocol::dynamic_node_id;
+    using namespace uavcan::protocol::dynamic_node_id::server;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<Discovery> _reg1;
+    uavcan::DefaultDataTypeRegistrator<AppendEntries> _reg2;
+    uavcan::DefaultDataTypeRegistrator<RequestVote> _reg3;
+    uavcan::DefaultDataTypeRegistrator<Allocation> _reg4;
+
+    EventTracer tracer;
+    StorageBackend storage;
+
+    // Node A is Allocator, Node B is Allocatee
+    InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast);
+
+    /*
+     * Server
+     */
+    uavcan::DynamicNodeIDAllocationServer server(nodes.a, storage, tracer);
+
+    ASSERT_LE(0, server.init(1));
+
+    /*
+     * Client
+     */
+    uavcan::DynamicNodeIDAllocationClient 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(4000));
+
+    ASSERT_TRUE(client.isAllocationComplete());
+    ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());
+}
+
+
 TEST(DynamicNodeIDAllocationServer, ObjectSizes)
 {
     std::cout << "Log:                      "