Merge pull request #2 from UAVCAN/dynamic_node_id_raft

Dynamic node id raft update to latest

CMakeLists.txt

@@ -33,5 +33,6 @@ add_subdirectory(libuavcan)
 #
 if (${CMAKE_SYSTEM_NAME} MATCHES "Linux")
     message(STATUS "Adding Linux support library")
+    add_subdirectory(libuavcan_drivers/posix)
     add_subdirectory(libuavcan_drivers/linux)
 endif ()

dsdl/uavcan/equipment/CoarseOrientation.uavcan

@@ -17,4 +17,4 @@ float32 ANGLE_MULTIPLIER = 4.7746482927568605
 
 int5[3] fixed_axis_roll_pitch_yaw
 
-bool defined    # False if the orientation is actually not defined
+bool orientation_defined    # False if the orientation is actually not defined

dsdl/uavcan/protocol/debug/1789.KeyValue.uavcan

@@ -2,7 +2,13 @@
 # Generic named parameter (key/value pair).
 #
 
-bool alignment
-uint8[<=100] key
+#
+# Integers are exactly representable in the range (-2^24, 2^24) which is (-16'777'216, 16'777'216).
+#
+float32 value
 
-float64[<=1] value
+#
+# Tail array optimization is enabled, so if key length does not exceed 4 characters,
+# the whole message can fit into one CAN frame.
+#
+uint8[<=34] key

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

@@ -3,16 +3,29 @@
 # Please refer to the specification for details.
 #
 
-uint16 DEFAULT_REQUEST_TIMEOUT_MS = 100
-
-uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1000 # request timeout * max cluster size * 2 requests
+#
+# 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], according to the Raft paper.
+#
+uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000
+uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000
 
 uint32 term
 uint32 prev_log_term
 uint8 prev_log_index
 uint8 leader_commit
 
-Entry[<=5] entries
+#
+# 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
 
 ---
 

dsdl/uavcan/protocol/file/218.Read.uavcan

@@ -1,11 +1,15 @@
 #
-# Read contents of the file from remote node.
-# Empty data in response means that the offset is out of file boundaries.
-# Non-empty data means the end of file is not reached yet, even if the length is less than maximum.
+# Read file from a remote node.
+# 
+# There are two possible outcomes of a successful service call:
+#  1. Data array size equals its capacity. This means that the end of the file is not reached yet.
+#  2. Data array size is less than its capacity, possibly zero. This means that the end of file is reached.
+# 
 # Thus, if the client needs to fetch the entire file, it should repeatedly call this service while increasing the
-# offset, until the empty data is returned.
+# offset, until incomplete data is returned.
+#
 # If the object pointed by 'path' cannot be read (e.g. it is a directory or it does not exist), appropriate error code
-# will be returned.
+# will be returned, and data array will be empty.
 #
 
 uint32 offset
@@ -14,4 +18,4 @@ Path path
 ---
 
 Error error
-uint8[<=250] data
+uint8[<=256] data

dsdl/uavcan/protocol/file/Error.uavcan

@@ -3,16 +3,17 @@
 # File operation result code.
 #
 
-int16 OK             = 0
-int16 UNKNOWN_ERROR  = 32767
+int16 OK                = 0
+int16 UNKNOWN_ERROR     = 32767
 
 # These error codes match some standard UNIX errno values
-int16 NOT_FOUND      = 2
-int16 IO_ERROR       = 5
-int16 ACCESS_DENIED  = 13
-int16 IS_DIRECTORY   = 21 # I.e. attempt to read/write on a path that points to a directory
-int16 INVALID_VALUE  = 22 # E.g. file name is not valid for the target file system
-int16 FILE_TOO_LARGE = 27
-int16 OUT_OF_SPACE   = 28
+int16 NOT_FOUND         = 2
+int16 IO_ERROR          = 5
+int16 ACCESS_DENIED     = 13
+int16 IS_DIRECTORY      = 21 # I.e. attempt to read/write on a path that points to a directory
+int16 INVALID_VALUE     = 22 # E.g. file name is not valid for the target file system
+int16 FILE_TOO_LARGE    = 27
+int16 OUT_OF_SPACE      = 28
+int16 NOT_IMPLEMENTED   = 38
 
 int16 value

libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/__init__.py

@@ -260,7 +260,7 @@ def make_template_expander(filename):
     template_text = re.sub(r'([^\$]{0,1})\$\{([^\}]+)\}', r'\1$!\2!$', template_text)
 
     # Flow control expression transformation: % foo: ==> <!--(foo)-->
-    template_text = re.sub(r'(?m)^(\ *)\%\ *([^\:]+?):{0,1}$', r'\1<!--(\2)-->', template_text)
+    template_text = re.sub(r'(?m)^(\ *)\%\ *(.+?):{0,1}$', r'\1<!--(\2)-->', template_text)
 
     # Block termination transformation: <!--(endfoo)--> ==> <!--(end)-->
     template_text = re.sub(r'\<\!--\(end[a-z]+\)--\>', r'<!--(end)-->', template_text)

libuavcan/dsdl_compiler/libuavcan_dsdl_compiler/data_type_template.tmpl

@@ -311,7 +311,7 @@ const ::uavcan::DefaultDataTypeRegistrator< ${t.cpp_full_type_name} > _uavcan_gd
 // No default registration
 % endif
 
-% for nsc in t.cpp_namespace_components:
+% for nsc in t.cpp_namespace_components[::-1]:
 } // Namespace ${nsc}
 % endfor
 
@@ -387,8 +387,8 @@ ${define_streaming_operator(type_name=t.cpp_full_type_name + '::Response')}
 ${define_streaming_operator(type_name=t.cpp_full_type_name)}
 % endif
 
-% for nsc in t.cpp_namespace_components:
-}
+% for nsc in t.cpp_namespace_components[::-1]:
+} // Namespace ${nsc}
 % endfor
 
 #endif // ${t.include_guard}

libuavcan/include/uavcan/marshal/array.hpp

@@ -1087,6 +1087,19 @@ inline bool operator!=(const R& rhs, const Array<T, ArrayMode, MaxSize>& lhs)
     return lhs.operator!=(rhs);
 }
 
+/**
+ * Shortcut for string-like array type instantiation.
+ * The proper way of doing this is actually "template<> using ... = ...", but this feature is not available in
+ * older C++ revisions which the library has to support.
+ */
+template <unsigned MaxSize>
+class MakeString
+{
+    MakeString();       // This class is not instantiatable.
+public:
+    typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeSaturate>, ArrayModeDynamic, MaxSize> Type;
+};
+
 /**
  * YAML streamer specification for any Array<>
  */

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/generic_subscriber.hpp

@@ -189,16 +189,21 @@ protected:
 
     int startAsMessageListener()
     {
+        UAVCAN_TRACE("GenericSubscriber", "Start as message listener; dtname=%s", DataSpec::getDataTypeFullName());
         return genericStart(&Dispatcher::registerMessageListener);
     }
 
     int startAsServiceRequestListener()
     {
+        UAVCAN_TRACE("GenericSubscriber", "Start as service request listener; dtname=%s",
+                     DataSpec::getDataTypeFullName());
         return genericStart(&Dispatcher::registerServiceRequestListener);
     }
 
     int startAsServiceResponseListener()
     {
+        UAVCAN_TRACE("GenericSubscriber", "Start as service response listener; dtname=%s",
+                     DataSpec::getDataTypeFullName());
         return genericStart(&Dispatcher::registerServiceResponseListener);
     }
 
@@ -217,6 +222,7 @@ protected:
      */
     void stop()
     {
+        UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName());
         GenericSubscriberBase::stop(forwarder_);
     }
 

libuavcan/include/uavcan/node/node.hpp

@@ -85,11 +85,13 @@ class UAVCAN_EXPORT Node : public INode
     TransportStatsProvider proto_tsp_;
 #endif
 
+    uint64_t internal_failure_cnt_;
     bool started_;
 
 protected:
     virtual void registerInternalFailure(const char* msg)
     {
+        internal_failure_cnt_++;
         UAVCAN_TRACE("Node", "Internal failure: %s", msg);
 #if UAVCAN_TINY
         (void)msg;
@@ -111,6 +113,7 @@ public:
         , proto_rrs_(*this)
         , proto_tsp_(*this)
 #endif
+        , internal_failure_cnt_(0)
         , started_(false)
     { }
 
@@ -139,6 +142,8 @@ public:
 
     bool isStarted() const { return started_; }
 
+    uint64_t getInternalFailureCount() const { return internal_failure_cnt_; }
+
     /**
      * Starts the node and publishes uavcan.protocol.NodeStatus immediately.
      * Does not so anything if the node is already started.

libuavcan/include/uavcan/node/scheduler.hpp

@@ -32,6 +32,7 @@ public:
 
     void startWithDeadline(MonotonicTime deadline);
     void startWithDelay(MonotonicDuration delay);
+    void generateDeadlineImmediately() { startWithDeadline(MonotonicTime::fromUSec(1)); }
 
     void stop();
 
@@ -76,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);
 
@@ -90,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/node/service_client.hpp

@@ -6,6 +6,7 @@
 #define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED
 
 #include <uavcan/build_config.hpp>
+#include <uavcan/util/multiset.hpp>
 #include <uavcan/node/generic_publisher.hpp>
 #include <uavcan/node/generic_subscriber.hpp>
 
@@ -20,12 +21,39 @@
 namespace uavcan
 {
 
-template <typename ServiceDataType>
+template <typename ServiceDataType, unsigned NumStaticReceiversAndBuffers>
 class UAVCAN_EXPORT ServiceResponseTransferListenerInstantiationHelper
 {
 public: // so much templating it hurts
     typedef typename TransferListenerInstantiationHelper<typename ServiceDataType::Response,
-                                                         1, 1, ServiceResponseTransferListener>::Type Type;
+                                                         NumStaticReceiversAndBuffers,
+                                                         NumStaticReceiversAndBuffers,
+                                                         TransferListenerWithFilter>::Type Type;
+};
+
+/**
+ * This struct describes a pending service call.
+ * Refer to @ref ServiceClient to learn more about service calls.
+ */
+struct ServiceCallID
+{
+    NodeID server_node_id;
+    TransferID transfer_id;
+
+    ServiceCallID() { }
+
+    ServiceCallID(NodeID arg_server_node_id, TransferID arg_transfer_id)
+        : server_node_id(arg_server_node_id)
+        , transfer_id(arg_transfer_id)
+    { }
+
+    bool operator==(const ServiceCallID rhs) const
+    {
+        return (rhs.server_node_id == server_node_id) &&
+               (rhs.transfer_id == transfer_id);
+    }
+
+    bool isValid() const { return server_node_id.isUnicast(); }
 };
 
 /**
@@ -33,29 +61,39 @@ public: // so much templating it hurts
  * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason.
  */
 template <typename DataType>
-struct UAVCAN_EXPORT ServiceCallResult
+class UAVCAN_EXPORT ServiceCallResult
 {
+public:
     typedef ReceivedDataStructure<typename DataType::Response> ResponseFieldType;
 
     enum Status { Success, ErrorTimeout };
 
-    const Status status;              ///< Whether successful or not. Failure to decode the response causes timeout.
-    NodeID server_node_id;            ///< Node ID of the server this call was addressed to.
-    ResponseFieldType& response;      ///< Returned data structure. Undefined if the service call has failed.
+private:
+    const Status status_;               ///< Whether successful or not. Failure to decode the response causes timeout.
+    ServiceCallID call_id_;             ///< Identifies the call
+    ResponseFieldType& response_;       ///< Returned data structure. Value undefined if the service call has failed.
 
-    ServiceCallResult(Status arg_status, NodeID arg_server_node_id, ResponseFieldType& arg_response)
-        : status(arg_status)
-        , server_node_id(arg_server_node_id)
-        , response(arg_response)
+public:
+    ServiceCallResult(Status arg_status, ServiceCallID arg_call_id, ResponseFieldType& arg_response)
+        : status_(arg_status)
+        , call_id_(arg_call_id)
+        , response_(arg_response)
     {
-        UAVCAN_ASSERT(server_node_id.isUnicast());
-        UAVCAN_ASSERT((status == Success) || (status == ErrorTimeout));
+        UAVCAN_ASSERT(call_id_.isValid());
+        UAVCAN_ASSERT((status_ == Success) || (status_ == ErrorTimeout));
     }
 
     /**
      * Shortcut to quickly check whether the call was successful.
      */
-    bool isSuccessful() const { return status == Success; }
+    bool isSuccessful() const { return status_ == Success; }
+
+    Status getStatus() const { return status_; }
+
+    ServiceCallID getCallID() const { return call_id_; }
+
+    const ResponseFieldType& getResponse() const { return response_; }
+    ResponseFieldType& getResponse() { return response_; }
 };
 
 /**
@@ -67,10 +105,11 @@ static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
 {
     s << "# Service call result [" << DataType::getDataTypeFullName() << "] "
       << (scr.isSuccessful() ? "OK" : "FAILURE")
-      << " server_node_id=" << int(scr.server_node_id.get()) << "\n";
+      << " server_node_id=" << int(scr.getCallID().server_node_id.get())
+      << " tid=" << int(scr.getCallID().transfer_id.get()) << "\n";
     if (scr.isSuccessful())
     {
-        s << scr.response;
+        s << scr.getResponse();
     }
     else
     {
@@ -82,64 +121,109 @@ static Stream& operator<<(Stream& s, const ServiceCallResult<DataType>& scr)
 /**
  * Do not use directly.
  */
-class ServiceClientBase : protected DeadlineHandler
+class ServiceClientBase : protected ITransferAcceptanceFilter
+                        , protected DeadlineHandler
 {
     const DataTypeDescriptor* data_type_descriptor_;  ///< This will be initialized at the time of first call
 
 protected:
-    MonotonicDuration request_timeout_;
-    bool pending_;
+    class CallState : DeadlineHandler
+    {
+        ServiceClientBase& owner_;
+        const ServiceCallID id_;
+        bool timed_out_;
 
-    explicit ServiceClientBase(INode& node)
+        virtual void handleDeadline(MonotonicTime);
+
+    public:
+        CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id)
+            : DeadlineHandler(node.getScheduler())
+            , owner_(owner)
+            , id_(call_id)
+            , timed_out_(false)
+        {
+            UAVCAN_ASSERT(id_.isValid());
+            DeadlineHandler::startWithDelay(owner_.request_timeout_);
+        }
+
+        ServiceCallID getCallID() const { return id_; }
+
+        bool hasTimedOut() const { return timed_out_; }
+
+        static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); }
+
+        bool operator==(const CallState& rhs) const
+        {
+            return (&owner_ == &rhs.owner_) && (id_ == rhs.id_);
+        }
+    };
+
+    struct CallStateMatchingPredicate
+    {
+        const ServiceCallID id;
+        CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { }
+        bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); }
+    };
+
+    struct ServerSearchPredicate
+    {
+        const NodeID server_node_id;
+        ServerSearchPredicate(NodeID nid) : server_node_id(nid) { }
+        bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; }
+    };
+
+    MonotonicDuration request_timeout_;
+
+    ServiceClientBase(INode& node)
         : DeadlineHandler(node.getScheduler())
         , data_type_descriptor_(NULL)
         , request_timeout_(getDefaultRequestTimeout())
-        , pending_(false)
     { }
 
     virtual ~ServiceClientBase() { }
 
-    int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, TransferID& out_transfer_id);
+    int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id);
 
 public:
-    /**
-     * Returns true if the service call is currently in progress.
-     */
-    bool isPending() const { return pending_; }
-
     /**
      * It's not recommended to override default timeouts.
+     * Change of this value will not affect pending calls.
      */
     static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(500); }
     static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); }
     static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); }
-
-    /**
-     * Returns the service response waiting deadline, if pending.
-     */
-    using DeadlineHandler::getDeadline;
 };
 
 /**
  * Use this class to invoke services on remote nodes.
  *
+ * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent
+ * calls is limited only by amount of available pool memory.
+ *
  * @tparam DataType_        Service data type.
  *
  * @tparam Callback_        Service response will be delivered through the callback of this type.
  *                          In C++11 mode this type defaults to std::function<>.
  *                          In C++03 mode this type defaults to a plain function pointer; use binder to
  *                          call member functions as callbacks.
+ *
+ * @tparam NumStaticCalls_  Number of concurrent calls that the class will be able to handle without using the
+ *                          memory pool. Note that this is NOT the maximum possible number of concurrent calls,
+ *                          there's no such limit. Defaults to one.
  */
 template <typename DataType_,
 #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
-          typename Callback_ = std::function<void (const ServiceCallResult<DataType_>&)>
+          typename Callback_ = std::function<void (const ServiceCallResult<DataType_>&)>,
 #else
-          typename Callback_ = void (*)(const ServiceCallResult<DataType_>&)
+          typename Callback_ = void (*)(const ServiceCallResult<DataType_>&),
 #endif
+          unsigned NumStaticCalls_ = 1
           >
 class UAVCAN_EXPORT ServiceClient
-    : public GenericSubscriber<DataType_, typename DataType_::Response,
-                               typename ServiceResponseTransferListenerInstantiationHelper<DataType_>::Type >
+    : public GenericSubscriber<DataType_,
+                               typename DataType_::Response,
+                               typename ServiceResponseTransferListenerInstantiationHelper<DataType_,
+                                                                                           NumStaticCalls_>::Type>
     , public ServiceClientBase
 {
 public:
@@ -149,16 +233,43 @@ public:
     typedef ServiceCallResult<DataType> ServiceCallResultType;
     typedef Callback_ Callback;
 
+    enum { NumStaticCalls = NumStaticCalls_ };
+
 private:
     typedef ServiceClient<DataType, Callback> SelfType;
     typedef GenericPublisher<DataType, RequestType> PublisherType;
-    typedef typename ServiceResponseTransferListenerInstantiationHelper<DataType>::Type TransferListenerType;
+    typedef typename ServiceResponseTransferListenerInstantiationHelper<DataType, NumStaticCalls>::Type
+            TransferListenerType;
     typedef GenericSubscriber<DataType, ResponseType, TransferListenerType> SubscriberType;
 
+    typedef Multiset<CallState, NumStaticCalls> CallRegistry;
+
+    struct TimeoutCallbackCaller
+    {
+        ServiceClient& owner;
+
+        TimeoutCallbackCaller(ServiceClient& arg_owner) : owner(arg_owner) { }
+
+        void operator()(const CallState& state)
+        {
+            if (state.hasTimedOut())
+            {
+                UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s",
+                             int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()),
+                             DataType::getDataTypeFullName());
+                ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(),
+                                             owner.getReceivedStructStorage());    // Mutable!
+                owner.invokeCallback(result);
+            }
+        }
+    };
+
+    CallRegistry call_registry_;
+
     PublisherType publisher_;
     Callback callback_;
 
-    bool isCallbackValid() const { return try_implicit_cast<bool>(callback_, true); }
+    virtual bool shouldAcceptFrame(const RxFrame& frame) const; // Called from the transfer listener
 
     void invokeCallback(ServiceCallResultType& result);
 
@@ -166,6 +277,8 @@ private:
 
     virtual void handleDeadline(MonotonicTime);
 
+    int addCallState(ServiceCallID call_id);
+
 public:
     /**
      * @param node      Node instance this client will be registered with.
@@ -174,6 +287,7 @@ public:
     explicit ServiceClient(INode& node, const Callback& callback = Callback())
         : SubscriberType(node)
         , ServiceClientBase(node)
+        , call_registry_(node.getAllocator())
         , publisher_(node, getDefaultRequestTimeout())
         , callback_(callback)
     {
@@ -183,7 +297,7 @@ public:
 #endif
     }
 
-    virtual ~ServiceClient() { cancel(); }
+    virtual ~ServiceClient() { cancelAllCalls(); }
 
     /**
      * Shall be called before first use.
@@ -199,21 +313,39 @@ public:
      * This method transmits the service request and returns immediately.
      *
      * Service response will be delivered into the application via the callback.
-     * Note that the callback will ALWAYS be called even if the service call has timed out; the
+     * Note that the callback will ALWAYS be called even if the service call times out; the
      * actual result of the call (success/failure) will be passed to the callback as well.
      *
-     * If this client instance is already pending service response, it will be cancelled and the new
-     * call will be performed.
-     *
      * Returns negative error code.
      */
     int call(NodeID server_node_id, const RequestType& request);
 
     /**
-     * Cancel the pending service call.
-     * Does nothing if it is not pending.
+     * Same as plain @ref call() above, but this overload also returns the call ID of the new call.
+     * The call ID structure can be used to cancel this request later if needed.
      */
-    void cancel();
+    int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id);
+
+    /**
+     * Cancels certain call referred via call ID structure.
+     */
+    void cancelCall(ServiceCallID call_id);
+
+    /**
+     * Cancels all pending calls.
+     */
+    void cancelAllCalls();
+
+    /**
+     * Checks whether there's currently a pending call addressed to the specified node ID.
+     */
+    bool hasPendingCallToServer(NodeID server_node_id) const;
+
+    /**
+     * This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned.
+     * Warning: average complexity is O(index); worst case complexity is O(size).
+     */
+    ServiceCallID getCallIDByIndex(unsigned index) const;
 
     /**
      * Service response callback must be set prior service call.
@@ -221,6 +353,18 @@ public:
     const Callback& getCallback() const { return callback_; }
     void setCallback(const Callback& cb) { callback_ = cb; }
 
+    /**
+     * Complexity is O(N) of number of pending calls.
+     * Note that the number of pending calls will not be updated until the callback is executed.
+     */
+    unsigned getNumPendingCalls() const { return call_registry_.getSize(); }
+
+    /**
+     * Complexity is O(1).
+     * Note that the number of pending calls will not be updated until the callback is executed.
+     */
+    bool hasPendingCalls() const { return !call_registry_.isEmpty(); }
+
     /**
      * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either:
      * - Transient failure in the transport layer.
@@ -229,7 +373,7 @@ public:
     uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); }
 
     /**
-     * Request timeouts.
+     * Request timeouts. Note that changing the request timeout will not affect calls that are already pending.
      * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts.
      * Not recommended to change.
      */
@@ -246,10 +390,10 @@ public:
 
 // ----------------------------------------------------------------------------
 
-template <typename DataType_, typename Callback_>
-void ServiceClient<DataType_, Callback_>::invokeCallback(ServiceCallResultType& result)
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+void ServiceClient<DataType_, Callback_, NumStaticCalls_>::invokeCallback(ServiceCallResultType& result)
 {
-    if (isCallbackValid())
+    if (try_implicit_cast<bool>(callback_, true))
     {
         callback_(result);
     }
@@ -259,118 +403,172 @@ void ServiceClient<DataType_, Callback_>::invokeCallback(ServiceCallResultType&
     }
 }
 
-template <typename DataType_, typename Callback_>
-void ServiceClient<DataType_, Callback_>::handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+bool ServiceClient<DataType_, Callback_, NumStaticCalls_>::shouldAcceptFrame(const RxFrame& frame) const
+{
+    UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher
+
+    return NULL != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(),
+                                                                                frame.getTransferID())));
+
+}
+
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+void ServiceClient<DataType_, Callback_, NumStaticCalls_>::
+handleReceivedDataStruct(ReceivedDataStructure<ResponseType>& response)
 {
     UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse);
-    const TransferListenerType* const listener = SubscriberType::getTransferListener();
-    if (listener)
+
+    ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID());
+    cancelCall(call_id);
+    ServiceCallResultType result(ServiceCallResultType::Success, call_id, response);    // Mutable!
+    invokeCallback(result);
+}
+
+
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+void ServiceClient<DataType_, Callback_, NumStaticCalls_>::handleDeadline(MonotonicTime)
+{
+    UAVCAN_TRACE("ServiceClient", "Shared deadline event received");
+    /*
+     * Invoking callbacks for timed out call state objects.
+     */
+    TimeoutCallbackCaller callback_caller(*this);
+    call_registry_.template forEach<TimeoutCallbackCaller&>(callback_caller);
+    /*
+     * Removing timed out objects.
+     * This operation cannot be merged with the previous one because that will not work with recursive calls.
+     */
+    call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate);
+    /*
+     * Subscriber does not need to be registered if we don't have any pending calls.
+     * Removing it makes processing of incoming frames a bit faster.
+     */
+    if (call_registry_.isEmpty())
     {
-        const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
-        ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, response);
-        cancel();
-        invokeCallback(result);
-    }
-    else
-    {
-        UAVCAN_ASSERT(0);
-        cancel();
+        SubscriberType::stop();
     }
 }
 
-template <typename DataType_, typename Callback_>
-void ServiceClient<DataType_, Callback_>::handleDeadline(MonotonicTime)
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+int ServiceClient<DataType_, Callback_, NumStaticCalls_>::addCallState(ServiceCallID call_id)
 {
-    const TransferListenerType* const listener = SubscriberType::getTransferListener();
-    if (listener)
+    if (call_registry_.isEmpty())
     {
-        const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams();
-        ReceivedDataStructure<ResponseType>& ref = SubscriberType::getReceivedStructStorage();
-        ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref);
+        const int subscriber_res = SubscriberType::startAsServiceResponseListener();
+        if (subscriber_res < 0)
+        {
+            UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
+            return subscriber_res;
+        }
+    }
 
-        UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s",
-                     erp.src_node_id.get(), DataType::getDataTypeFullName());
-        cancel();
-        invokeCallback(result);
-    }
-    else
+    if (NULL == call_registry_.template emplace<INode&, ServiceClientBase&, ServiceCallID>(SubscriberType::getNode(),
+                                                                                           *this, call_id))
     {
-        UAVCAN_ASSERT(0);
-        cancel();
+        SubscriberType::stop();
+        return -ErrMemory;
     }
+
+    return 0;
 }
 
-template <typename DataType_, typename Callback_>
-int ServiceClient<DataType_, Callback_>::call(NodeID server_node_id, const RequestType& request)
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+int ServiceClient<DataType_, Callback_, NumStaticCalls_>::call(NodeID server_node_id, const RequestType& request)
 {
-    cancel();
-    if (!isCallbackValid())
+   ServiceCallID dummy;
+   return call(server_node_id, request, dummy);
+}
+
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+int ServiceClient<DataType_, Callback_, NumStaticCalls_>::call(NodeID server_node_id, const RequestType& request,
+                                                               ServiceCallID& out_call_id)
+{
+    if (!try_implicit_cast<bool>(callback_, true))
     {
         UAVCAN_TRACE("ServiceClient", "Invalid callback");
-        return -ErrInvalidParam;
+        return -ErrInvalidConfiguration;
     }
 
     /*
      * Common procedures that don't depend on the struct data type
      */
-    TransferID transfer_id;
     const int prep_res =
-        prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, transfer_id);
+        prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id);
     if (prep_res < 0)
     {
         UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res);
-        cancel();
         return prep_res;
     }
 
     /*
-     * Starting the subscriber
+     * Initializing the call state - this will start the subscriber ad-hoc
      */
-    const int subscriber_res = SubscriberType::startAsServiceResponseListener();
-    if (subscriber_res < 0)
+    const int call_state_res = addCallState(out_call_id);
+    if (call_state_res < 0)
     {
-        UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res);
-        cancel();
-        return subscriber_res;
+        UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res);
+        return call_state_res;
     }
 
     /*
-     * Configuring the listener so it will accept only the matching response
+     * Configuring the listener so it will accept only the matching responses
+     * TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder)
      */
     TransferListenerType* const tl = SubscriberType::getTransferListener();
     if (tl == NULL)
     {
         UAVCAN_ASSERT(0);  // Must have been created
-        cancel();
+        cancelCall(out_call_id);
         return -ErrLogic;
     }
-    const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id);
-    tl->setExpectedResponseParams(erp);
+    tl->installAcceptanceFilter(this);
 
     /*
      * Publishing the request
      */
-    const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id);
+    const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id,
+                                                 out_call_id.transfer_id);
     if (publisher_res < 0)
     {
-        cancel();
+        cancelCall(out_call_id);
+        return publisher_res;
     }
+
+    UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id);
     return publisher_res;
 }
 
-template <typename DataType_, typename Callback_>
-void ServiceClient<DataType_, Callback_>::cancel()
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+void ServiceClient<DataType_, Callback_, NumStaticCalls_>::cancelCall(ServiceCallID call_id)
 {
-    pending_ = false;
-    SubscriberType::stop();
-    DeadlineHandler::stop();
-    TransferListenerType* const tl = SubscriberType::getTransferListener();
-    if (tl)
+    call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id));
+    if (call_registry_.isEmpty())
     {
-        tl->stopAcceptingAnything();
+        SubscriberType::stop();
     }
 }
 
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+void ServiceClient<DataType_, Callback_, NumStaticCalls_>::cancelAllCalls()
+{
+    call_registry_.clear();
+    SubscriberType::stop();
+}
+
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+bool ServiceClient<DataType_, Callback_, NumStaticCalls_>::hasPendingCallToServer(NodeID server_node_id) const
+{
+    return NULL != call_registry_.find(ServerSearchPredicate(server_node_id));
+}
+
+template <typename DataType_, typename Callback_, unsigned NumStaticCalls_>
+ServiceCallID ServiceClient<DataType_, Callback_, NumStaticCalls_>::getCallIDByIndex(unsigned index) const
+{
+    const CallState* const id = call_registry_.getByIndex(index);
+    return (id == NULL) ? ServiceCallID() : id->getCallID();
+}
+
 }
 
 #endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED

libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp

@@ -26,7 +26,7 @@ namespace uavcan
  *
  * Once dynamic allocation is complete (or not needed anymore), the object can be deleted.
  */
-class DynamicNodeIDClient : private TimerBase
+class UAVCAN_EXPORT DynamicNodeIDClient : private TimerBase
 {
     typedef MethodBinder<DynamicNodeIDClient*,
                          void (DynamicNodeIDClient::*)

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

@@ -35,11 +35,6 @@ public:
      */
     virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0;
 
-    /**
-     * This method will be invoked when there's an Allocation message detected on the bus.
-     */
-    virtual void handleAllocationActivityDetection(const ReceivedDataStructure<Allocation>& msg) = 0;
-
     virtual ~IAllocationRequestHandler() { }
 };
 
@@ -132,7 +127,6 @@ class AllocationRequestManager
     void handleAllocation(const ReceivedDataStructure<Allocation>& msg)
     {
         trace(TraceAllocationActivity, msg.getSrcNodeID().get());
-        handler_.handleAllocationActivityDetection(msg);
 
         if (!msg.isAnonymousTransfer())
         {
@@ -251,6 +245,7 @@ public:
             return res;
         }
         (void)allocation_pub_.setPriority(TransferPriorityLow);
+        allocation_pub_.setTxTimeout(MonotonicDuration::fromMSec(Allocation::DEFAULT_REQUEST_PERIOD_MS));
 
         res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation));
         if (res < 0)

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/cluster_manager.hpp

@@ -68,8 +68,6 @@ private:
     uint8_t cluster_size_;
     uint8_t num_known_servers_;
 
-    bool had_discovery_activity_;
-
     static IStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; }
 
     INode&       getNode()       { return discovery_sub_.getNode(); }
@@ -89,22 +87,6 @@ private:
         return NULL;
     }
 
-    void addServer(NodeID node_id)
-    {
-        UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2));
-        if (!isKnownServer(node_id) && node_id.isUnicast())
-        {
-            tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get());
-            servers_[num_known_servers_].node_id = node_id;
-            servers_[num_known_servers_].resetIndices(log_);
-            num_known_servers_ = static_cast<uint8_t>(num_known_servers_ + 1U);
-        }
-        else
-        {
-            UAVCAN_ASSERT(0);
-        }
-    }
-
     virtual void handleTimerEvent(const TimerEvent&)
     {
         UAVCAN_ASSERT(num_known_servers_ < cluster_size_);
@@ -167,8 +149,6 @@ private:
             return;
         }
 
-        had_discovery_activity_ = true;
-
         /*
          * Updating the set of known servers
          */
@@ -220,7 +200,6 @@ public:
         , discovery_pub_(node)
         , cluster_size_(0)
         , num_known_servers_(0)
-        , had_discovery_activity_(false)
     { }
 
     /**
@@ -301,6 +280,25 @@ public:
         return 0;
     }
 
+    /**
+     * Adds once server regardless of the discovery logic.
+     */
+    void addServer(NodeID node_id)
+    {
+        UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize);
+        if (!isKnownServer(node_id) && node_id.isUnicast())
+        {
+            tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get());
+            servers_[num_known_servers_].node_id = node_id;
+            servers_[num_known_servers_].resetIndices(log_);
+            num_known_servers_ = static_cast<uint8_t>(num_known_servers_ + 1U);
+        }
+        else
+        {
+            UAVCAN_ASSERT(0);
+        }
+    }
+
     /**
      * Whether such server has been discovered.
      */
@@ -414,19 +412,6 @@ public:
         }
     }
 
-    /**
-     * This method returns true if there was at least one Discovery message received since last call.
-     */
-    bool hadDiscoveryActivity()
-    {
-        if (had_discovery_activity_)
-        {
-            had_discovery_activity_ = false;
-            return true;
-        }
-        return false;
-    }
-
     /**
      * Number of known servers can only grow, and it never exceeds the cluster size value.
      * This number does not include the local server.

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/raft_core.hpp

@@ -5,6 +5,7 @@
 #ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED
 #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_RAFT_CORE_HPP_INCLUDED
 
+#include <cstdlib>
 #include <uavcan/build_config.hpp>
 #include <uavcan/debug.hpp>
 #include <uavcan/util/method_binder.hpp>
@@ -51,29 +52,26 @@ public:
  * It does not implement client-server interaction at all; instead it just exposes a public method for adding
  * allocation entries.
  *
+ * Note that this class uses std::rand(), so the RNG must be properly seeded by the application.
+ *
  * Activity registration:
  *   - persistent state update error
  *   - switch to candidate (this defines timeout between reelections)
  *   - newer term in response (also switch to follower)
  *   - append entries request with term >= currentTerm
  *   - vote granted
- *
- * Active state switch logic:
- *   Activation (this is default state):
- *     - vote request
- *     - allocation activity detected
- *     - only if leader:
- *       - discovery activity detected
- *       - log is not fully replicated (this includes the case when the cluster is not fully discovered)
- *
- *   Deactivation:
- *     - switch to follower state
- *     - persistent state update error
- *     - only if leader:
- *       - all log entries are fully replicated
  */
 class RaftCore : private TimerBase
 {
+public:
+    enum ServerState
+    {
+        ServerStateFollower,
+        ServerStateCandidate,
+        ServerStateLeader
+    };
+
+private:
     typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ReceivedDataStructure<AppendEntries::Request>&,
                                                        ServiceResponseDataStructure<AppendEntries::Response>&)>
         AppendEntriesCallback;
@@ -88,13 +86,6 @@ class RaftCore : private TimerBase
     typedef MethodBinder<RaftCore*, void (RaftCore::*)(const ServiceCallResult<RequestVote>&)>
         RequestVoteResponseCallback;
 
-    enum ServerState
-    {
-        ServerStateFollower,
-        ServerStateCandidate,
-        ServerStateLeader
-    };
-
     struct PendingAppendEntriesFields
     {
         Log::Index prev_log_index;
@@ -109,8 +100,7 @@ class RaftCore : private TimerBase
     /*
      * Constants
      */
-    const MonotonicDuration update_interval_;           ///< AE requests will be issued at this rate
-    const MonotonicDuration base_activity_timeout_;
+    enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 };
 
     IEventTracer& tracer_;
     IRaftLeaderMonitor& leader_monitor_;
@@ -123,7 +113,7 @@ class RaftCore : private TimerBase
     Log::Index commit_index_;
 
     MonotonicTime last_activity_timestamp_;
-    bool active_mode_;
+    MonotonicDuration randomized_activity_timeout_;
     ServerState server_state_;
 
     uint8_t next_server_index_;         ///< Next server to query AE from
@@ -137,10 +127,7 @@ class RaftCore : private TimerBase
     ServiceServer<AppendEntries, AppendEntriesCallback>         append_entries_srv_;
     ServiceClient<AppendEntries, AppendEntriesResponseCallback> append_entries_client_;
     ServiceServer<RequestVote, RequestVoteCallback> request_vote_srv_;
-
-    enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 };
-    LazyConstructor<ServiceClient<RequestVote, RequestVoteResponseCallback> >
-        request_vote_clients_[NumRequestVoteClients];
+    ServiceClient<RequestVote, RequestVoteResponseCallback, MaxNumFollowers> request_vote_client_;
 
     /*
      * Methods
@@ -150,16 +137,49 @@ class RaftCore : private TimerBase
     INode&       getNode()       { return append_entries_srv_.getNode(); }
     const INode& getNode() const { return append_entries_srv_.getNode(); }
 
-    void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); }
+    void checkInvariants() const
+    {
+        // Commit index
+        UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex());
+
+        // Term
+        UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) != NULL);
+        UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term
+                      <= persistent_state_.getCurrentTerm());
+
+        // Elections
+        UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() ||
+                      persistent_state_.getVotedFor() == getNode().getNodeID());
+        UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize());
+
+        // Transport
+        UAVCAN_ASSERT(append_entries_client_.getNumPendingCalls() <= 1);
+        UAVCAN_ASSERT(request_vote_client_.getNumPendingCalls() <= cluster_.getNumKnownServers());
+        UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !append_entries_client_.hasPendingCalls());
+        UAVCAN_ASSERT(server_state_ != ServerStateLeader    || !request_vote_client_.hasPendingCalls());
+        UAVCAN_ASSERT(server_state_ != ServerStateFollower  ||
+                      (!append_entries_client_.hasPendingCalls() && !request_vote_client_.hasPendingCalls()));
+    }
+
+    void registerActivity()
+    {
+        last_activity_timestamp_ = getNode().getMonotonicTime();
+
+        static const int32_t randomization_range_msec = AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS -
+                                                        AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS;
+
+        const int32_t random_msec = (std::rand() % randomization_range_msec) + 1;
+
+        randomized_activity_timeout_ =
+            MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec);
+
+        UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS);
+        UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS);
+    }
 
     bool isActivityTimedOut() const
     {
-        const int multiplier = static_cast<int>(getNode().getNodeID().get()) - 1;
-
-        const MonotonicDuration activity_timeout =
-            MonotonicDuration::fromUSec(base_activity_timeout_.toUSec() + update_interval_.toUSec() * multiplier);
-
-        return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout);
+        return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_);
     }
 
     void handlePersistentStateUpdateError(int error)
@@ -167,13 +187,12 @@ class RaftCore : private TimerBase
         UAVCAN_ASSERT(error < 0);
         trace(TraceRaftPersistStateUpdateError, error);
         switchState(ServerStateFollower);
-        setActiveMode(false);               // Goodnight sweet prince
         registerActivity();                 // Deferring reelections
     }
 
     void updateFollower()
     {
-        if (active_mode_ && isActivityTimedOut())
+        if (isActivityTimedOut())
         {
             switchState(ServerStateCandidate);
             registerActivity();
@@ -182,8 +201,6 @@ class RaftCore : private TimerBase
 
     void updateCandidate()
     {
-        UAVCAN_ASSERT(active_mode_);
-
         if (num_votes_received_in_this_campaign_ > 0)
         {
             trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_);
@@ -218,7 +235,7 @@ class RaftCore : private TimerBase
             req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term;
             req.term = persistent_state_.getCurrentTerm();
 
-            for (uint8_t i = 0; i < NumRequestVoteClients; i++)
+            for (uint8_t i = 0; i < MaxNumFollowers; i++)
             {
                 const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i);
                 if (!node_id.isUnicast())
@@ -230,7 +247,7 @@ class RaftCore : private TimerBase
                              "Requesting vote from %d", int(node_id.get()));
                 trace(TraceRaftVoteRequestInitiation, node_id.get());
 
-                res = request_vote_clients_[i]->call(node_id, req);
+                res = request_vote_client_.call(node_id, req);
                 if (res < 0)
                 {
                     trace(TraceError, res);
@@ -241,12 +258,12 @@ class RaftCore : private TimerBase
 
     void updateLeader()
     {
-        if (cluster_.getClusterSize() == 1)
+        if (append_entries_client_.hasPendingCalls())
         {
-            setActiveMode(false);           // Haha
+            append_entries_client_.cancelAllCalls();    // Refer to the response callback to learn why
         }
 
-        if (active_mode_ || (next_server_index_ > 0))
+        if (cluster_.getClusterSize() > 1)
         {
             const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_);
             UAVCAN_ASSERT(node_id.isUnicast());
@@ -325,11 +342,8 @@ class RaftCore : private TimerBase
         next_server_index_ = 0;
         num_votes_received_in_this_campaign_ = 0;
 
-        for (uint8_t i = 0; i < NumRequestVoteClients; i++)
-        {
-            request_vote_clients_[i]->cancel();
-        }
-        append_entries_client_.cancel();
+        request_vote_client_.cancelAllCalls();
+        append_entries_client_.cancelAllCalls();
 
         /*
          * Calling the switch handler
@@ -342,18 +356,6 @@ class RaftCore : private TimerBase
         }
     }
 
-    void setActiveMode(bool new_active)
-    {
-        if (active_mode_ != new_active)
-        {
-            UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Active switch: %d --> %d",
-                         int(active_mode_), int(new_active));
-            trace(TraceRaftActiveSwitch, new_active);
-
-            active_mode_ = new_active;
-        }
-    }
-
     void tryIncrementCurrentTermFromResponse(Term new_term)
     {
         trace(TraceRaftNewerTermInResponse, new_term);
@@ -364,7 +366,6 @@ class RaftCore : private TimerBase
         }
         registerActivity();                             // Deferring future elections
         switchState(ServerStateFollower);
-        setActiveMode(false);
     }
 
     void propagateCommitIndex()
@@ -373,29 +374,12 @@ class RaftCore : private TimerBase
         UAVCAN_ASSERT(server_state_ == ServerStateLeader);
         UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex());
 
-        if (commit_index_ == persistent_state_.getLog().getLastIndex())
+        if (commit_index_ < persistent_state_.getLog().getLastIndex())
         {
-            // All local entries are committed
-            bool commit_index_fully_replicated = true;
-
-            for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++)
-            {
-                const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i));
-                if (match_index != commit_index_)
-                {
-                    commit_index_fully_replicated = false;
-                    break;
-                }
-            }
-
-            const bool all_done = commit_index_fully_replicated && cluster_.isClusterDiscovered();
-            setActiveMode(!all_done);  // Enable passive mode if commit index is the same on all nodes
-        }
-        else
-        {
-            // Not all local entries are committed
-            setActiveMode(true);
-
+            /*
+             * Not all local entries are committed.
+             * Deciding if it is safe to increment commit index.
+             */
             uint8_t num_nodes_with_next_log_entry_available = 1; // Local node
             for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++)
             {
@@ -421,11 +405,20 @@ class RaftCore : private TimerBase
     void handleAppendEntriesRequest(const ReceivedDataStructure<AppendEntries::Request>& request,
                                     ServiceResponseDataStructure<AppendEntries::Response>& response)
     {
+        checkInvariants();
+
         if (!cluster_.isKnownServer(request.getSrcNodeID()))
         {
-            trace(TraceRaftRequestIgnored, request.getSrcNodeID().get());
-            response.setResponseEnabled(false);
-            return;
+            if (cluster_.isClusterDiscovered())
+            {
+                trace(TraceRaftRequestIgnored, request.getSrcNodeID().get());
+                response.setResponseEnabled(false);
+                return;
+            }
+            else
+            {
+                cluster_.addServer(request.getSrcNodeID());
+            }
         }
 
         UAVCAN_ASSERT(response.isResponseEnabled());  // This is default
@@ -471,7 +464,6 @@ class RaftCore : private TimerBase
 
         registerActivity();
         switchState(ServerStateFollower);
-        setActiveMode(false);
 
         /*
          * Step 2
@@ -544,28 +536,31 @@ class RaftCore : private TimerBase
     void handleAppendEntriesResponse(const ServiceCallResult<AppendEntries>& result)
     {
         UAVCAN_ASSERT(server_state_ == ServerStateLeader);  // When state switches, all requests must be cancelled
+        checkInvariants();
 
         if (!result.isSuccessful())
         {
             return;
         }
 
-        if (result.response.term > persistent_state_.getCurrentTerm())
+        if (result.getResponse().term > persistent_state_.getCurrentTerm())
         {
-            tryIncrementCurrentTermFromResponse(result.response.term);
+            tryIncrementCurrentTermFromResponse(result.getResponse().term);
         }
         else
         {
-            if (result.response.success)
+            if (result.getResponse().success)
             {
-                cluster_.incrementServerNextIndexBy(result.server_node_id, pending_append_entries_fields_.num_entries);
-                cluster_.setServerMatchIndex(result.server_node_id,
+                cluster_.incrementServerNextIndexBy(result.getCallID().server_node_id,
+                                                    pending_append_entries_fields_.num_entries);
+                cluster_.setServerMatchIndex(result.getCallID().server_node_id,
                                              Log::Index(pending_append_entries_fields_.prev_log_index +
                                                         pending_append_entries_fields_.num_entries));
             }
             else
             {
-                cluster_.decrementServerNextIndex(result.server_node_id);
+                cluster_.decrementServerNextIndex(result.getCallID().server_node_id);
+                trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get());
             }
         }
 
@@ -576,6 +571,7 @@ class RaftCore : private TimerBase
     void handleRequestVoteRequest(const ReceivedDataStructure<RequestVote::Request>& request,
                                   ServiceResponseDataStructure<RequestVote::Response>& response)
     {
+        checkInvariants();
         trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get());
 
         if (!cluster_.isKnownServer(request.getSrcNodeID()))
@@ -587,14 +583,14 @@ class RaftCore : private TimerBase
 
         UAVCAN_ASSERT(response.isResponseEnabled());  // This is default
 
-        setActiveMode(true);
-
         /*
          * Checking if our current state is up to date.
          * The request will be ignored if persistent state cannot be updated.
          */
         if (request.term > persistent_state_.getCurrentTerm())
         {
+            switchState(ServerStateFollower);   // Our term is stale, so we can't serve as leader
+
             int res = persistent_state_.setCurrentTerm(request.term);
             if (res < 0)
             {
@@ -632,6 +628,7 @@ class RaftCore : private TimerBase
 
             if (response.vote_granted)
             {
+                switchState(ServerStateFollower);   // Avoiding race condition when Candidate
                 registerActivity();                 // This is necessary to avoid excessive elections
 
                 const int res = persistent_state_.setVotedFor(request.getSrcNodeID());
@@ -648,21 +645,22 @@ class RaftCore : private TimerBase
     void handleRequestVoteResponse(const ServiceCallResult<RequestVote>& result)
     {
         UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled
+        checkInvariants();
 
         if (!result.isSuccessful())
         {
             return;
         }
 
-        trace(TraceRaftVoteRequestSucceeded, result.server_node_id.get());
+        trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get());
 
-        if (result.response.term > persistent_state_.getCurrentTerm())
+        if (result.getResponse().term > persistent_state_.getCurrentTerm())
         {
-            tryIncrementCurrentTermFromResponse(result.response.term);
+            tryIncrementCurrentTermFromResponse(result.getResponse().term);
         }
         else
         {
-            if (result.response.vote_granted)
+            if (result.getResponse().vote_granted)
             {
                 num_votes_received_in_this_campaign_++;
             }
@@ -673,10 +671,7 @@ class RaftCore : private TimerBase
 
     virtual void handleTimerEvent(const TimerEvent&)
     {
-        if (cluster_.hadDiscoveryActivity() && isLeader())
-        {
-            setActiveMode(true);
-        }
+        checkInvariants();
 
         switch (server_state_)
         {
@@ -707,33 +702,24 @@ public:
     RaftCore(INode& node,
              IStorageBackend& storage,
              IEventTracer& tracer,
-             IRaftLeaderMonitor& leader_monitor,
-             MonotonicDuration update_interval =
-                 MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS),
-             MonotonicDuration base_activity_timeout =
-                 MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_BASE_ELECTION_TIMEOUT_MS))
+             IRaftLeaderMonitor& leader_monitor)
         : TimerBase(node)
-        , update_interval_(update_interval)
-        , base_activity_timeout_(base_activity_timeout)
         , tracer_(tracer)
         , leader_monitor_(leader_monitor)
         , persistent_state_(storage, tracer)
         , cluster_(node, storage, persistent_state_.getLog(), tracer)
         , commit_index_(0)                                  // Per Raft paper, commitIndex must be initialized to zero
         , last_activity_timestamp_(node.getMonotonicTime())
-        , active_mode_(true)
+        , randomized_activity_timeout_(
+            MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS))
         , server_state_(ServerStateFollower)
         , next_server_index_(0)
         , num_votes_received_in_this_campaign_(0)
         , append_entries_srv_(node)
         , append_entries_client_(node)
         , request_vote_srv_(node)
-    {
-        for (uint8_t i = 0; i < NumRequestVoteClients; i++)
-        {
-            request_vote_clients_[i].construct<INode&>(node);
-        }
-    }
+        , request_vote_client_(node)
+    { }
 
     /**
      * Once started, the logic runs in the background until destructor is called.
@@ -746,13 +732,13 @@ public:
         /*
          * Initializing state variables
          */
-        last_activity_timestamp_ = getNode().getMonotonicTime();
-        active_mode_ = true;
         server_state_ = ServerStateFollower;
         next_server_index_ = 0;
         num_votes_received_in_this_campaign_ = 0;
         commit_index_ = 0;
 
+        registerActivity();
+
         /*
          * Initializing internals
          */
@@ -787,36 +773,41 @@ public:
         }
         append_entries_client_.setCallback(AppendEntriesResponseCallback(this,
                                                                          &RaftCore::handleAppendEntriesResponse));
-        append_entries_client_.setRequestTimeout(update_interval_);
 
-        for (uint8_t i = 0; i < NumRequestVoteClients; i++)
+        res = request_vote_client_.init();
+        if (res < 0)
         {
-            res = request_vote_clients_[i]->init();
-            if (res < 0)
-            {
-                return res;
-            }
-            request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this,
-                                                                              &RaftCore::handleRequestVoteResponse));
-            request_vote_clients_[i]->setRequestTimeout(update_interval_);
+            return res;
         }
+        request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse));
 
-        startPeriodic(update_interval_);
+        /*
+         * Initializing timing constants
+         * Refer to the specification for the formula
+         */
+        const uint8_t num_followers = static_cast<uint8_t>(cluster_.getClusterSize() - 1);
 
-        trace(TraceRaftCoreInited, update_interval_.toUSec());
+        const MonotonicDuration update_interval =
+            MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS /
+                                        2 / max(static_cast<uint8_t>(2), num_followers));
+
+        UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore",
+                     "Update interval: %ld msec", static_cast<long>(update_interval.toMSec()));
+
+        append_entries_client_.setRequestTimeout(min(append_entries_client_.getDefaultRequestTimeout(),
+                                                     update_interval));
+
+        request_vote_client_.setRequestTimeout(min(request_vote_client_.getDefaultRequestTimeout(),
+                                                   update_interval));
+
+        startPeriodic(update_interval);
+
+        trace(TraceRaftCoreInited, update_interval.toUSec());
 
         UAVCAN_ASSERT(res >= 0);
         return 0;
     }
 
-    /**
-     * Normally should be called when there's allocation activity on the bus.
-     */
-    void forceActiveMode()
-    {
-        setActiveMode(true); // If the current state was Follower, active mode may be toggling quickly for some time
-    }
-
     /**
      * This function is mostly needed for testing.
      */
@@ -906,6 +897,16 @@ public:
         // Remember that index zero contains a special-purpose entry that doesn't count as allocation
         return persistent_state_.getLog().getLastIndex();
     }
+
+    /**
+     * These accessors are needed for debugging, visualization and testing.
+     */
+    const PersistentState& getPersistentState() const { return persistent_state_; }
+    const ClusterManager& getClusterManager()   const { return cluster_; }
+    MonotonicTime getLastActivityTimestamp()    const { return last_activity_timestamp_; }
+    ServerState getServerState()                const { return server_state_; }
+    MonotonicDuration getUpdateInterval()       const { return getPeriod(); }
+    MonotonicDuration getRandomizedTimeout()    const { return randomized_activity_timeout_; }
 };
 
 }

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>
@@ -24,9 +23,9 @@ namespace distributed
 /**
  * This class implements the top-level allocation logic and server API.
  */
-class Server : IAllocationRequestHandler
-             , INodeDiscoveryHandler
-             , IRaftLeaderMonitor
+class UAVCAN_EXPORT Server : IAllocationRequestHandler
+                           , INodeDiscoveryHandler
+                           , IRaftLeaderMonitor
 {
     struct UniqueIDLogPredicate
     {
@@ -65,6 +64,7 @@ class Server : IAllocationRequestHandler
      * States
      */
     INode& node_;
+    IEventTracer& tracer_;
     RaftCore raft_core_;
     AllocationRequestManager allocation_request_manager_;
     NodeDiscoverer node_discoverer_;
@@ -135,11 +135,6 @@ class Server : IAllocationRequestHandler
          }
     }
 
-    virtual void handleAllocationActivityDetection(const ReceivedDataStructure<Allocation>&)
-    {
-        raft_core_.forceActiveMode();
-    }
-
     /*
      * Methods of INodeDiscoveryHandler
      */
@@ -235,7 +230,8 @@ class 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");
         }
     }
 
@@ -244,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)
@@ -295,6 +292,80 @@ public:
     }
 
     Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); }
+
+    /**
+     * These accessors are needed for debugging, visualization and testing.
+     */
+    const RaftCore& getRaftCore() const { return raft_core_; }
+    const NodeDiscoverer& getNodeDiscoverer() const { return node_discoverer_; }
+};
+
+/**
+ * This structure represents immediate state of the server.
+ * It can be used for state visualization and debugging.
+ */
+struct StateReport
+{
+    uint8_t cluster_size;
+
+    RaftCore::ServerState state;
+
+    Log::Index last_log_index;
+    Log::Index commit_index;
+
+    Term last_log_term;
+    Term current_term;
+
+    NodeID voted_for;
+
+    MonotonicTime last_activity_timestamp;
+    MonotonicDuration randomized_timeout;
+
+    uint8_t num_unknown_nodes;
+
+    struct FollowerState
+    {
+        NodeID node_id;
+        Log::Index next_index;
+        Log::Index match_index;
+
+        FollowerState()
+            : next_index(0)
+            , match_index(0)
+        { }
+    } followers[ClusterManager::MaxClusterSize - 1];
+
+    StateReport(const Server& s)
+        : cluster_size           (s.getRaftCore().getClusterManager().getClusterSize())
+        , state                  (s.getRaftCore().getServerState())
+        , last_log_index         (s.getRaftCore().getPersistentState().getLog().getLastIndex())
+        , commit_index           (s.getRaftCore().getCommitIndex())
+        , last_log_term          (0)    // See below
+        , current_term           (s.getRaftCore().getPersistentState().getCurrentTerm())
+        , voted_for              (s.getRaftCore().getPersistentState().getVotedFor())
+        , last_activity_timestamp(s.getRaftCore().getLastActivityTimestamp())
+        , randomized_timeout     (s.getRaftCore().getRandomizedTimeout())
+        , num_unknown_nodes      (s.getNodeDiscoverer().getNumUnknownNodes())
+    {
+        const Entry* const e = s.getRaftCore().getPersistentState().getLog().getEntryAtIndex(last_log_index);
+        UAVCAN_ASSERT(e != NULL);
+        if (e != NULL)
+        {
+            last_log_term = e->term;
+        }
+
+        for (uint8_t i = 0; i < (cluster_size - 1U); i++)
+        {
+            const ClusterManager& mgr = s.getRaftCore().getClusterManager();
+            const NodeID node_id = mgr.getRemoteServerNodeIDAtIndex(i);
+            if (node_id.isUnicast())
+            {
+                followers[i].node_id     = node_id;
+                followers[i].next_index  = mgr.getServerNextIndex(node_id);
+                followers[i].match_index = mgr.getServerMatchIndex(node_id);
+            }
+        }
+    }
 };
 
 }

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

@@ -16,7 +16,7 @@ namespace dynamic_node_id_server
  * @ref IEventTracer.
  * Event codes cannot be changed, only new ones can be added.
  */
-enum TraceCode
+enum UAVCAN_EXPORT TraceCode
 {
     // Event name                          Argument
     // 0
@@ -38,7 +38,7 @@ enum TraceCode
     TraceRaftCoreInited,                // update interval in usec
     TraceRaftStateSwitch,               // 0 - Follower, 1 - Candidate, 2 - Leader
     // 15
-    TraceRaftActiveSwitch,              // 0 - Passive, 1 - Active
+    Trace0,
     TraceRaftNewLogEntry,               // node ID value
     TraceRaftRequestIgnored,            // node ID of the client
     TraceRaftVoteRequestReceived,       // node ID of the client
@@ -52,7 +52,7 @@ enum TraceCode
     // 25
     TraceRaftAppendEntriesCallFailure,  // error code (may be negated)
     TraceRaftElectionComplete,          // number of votes collected
-    Trace1,
+    TraceRaftAppendEntriesRespUnsucfl,  // node ID of the client
     Trace2,
     Trace3,
     // 30
@@ -87,7 +87,7 @@ enum TraceCode
 /**
  * This interface allows the application to trace events that happen in the server.
  */
-class IEventTracer
+class UAVCAN_EXPORT IEventTracer
 {
 public:
 #if UAVCAN_TOSTRING
@@ -115,7 +115,7 @@ public:
             "RaftBadClusterSizeReceived",
             "RaftCoreInited",
             "RaftStateSwitch",
-            "RaftActiveSwitch",
+            "",
             "RaftNewLogEntry",
             "RaftRequestIgnored",
             "RaftVoteRequestReceived",
@@ -127,7 +127,7 @@ public:
             "RaftNewEntryCommitted",
             "RaftAppendEntriesCallFailure",
             "RaftElectionComplete",
-            "",
+            "RaftAppendEntriesRespUnsucfl",
             "",
             "",
             "AllocationFollowupResponse",

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

@@ -92,6 +92,8 @@ class NodeDiscoverer : TimerBase
      */
     enum { MaxAttemptsToGetNodeInfo = 5 };
 
+    enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3)
+
     enum { NumNodeStatusStaticReceivers = 64 };
 
     /*
@@ -104,7 +106,7 @@ class NodeDiscoverer : TimerBase
     NodeMap node_map_;                                  ///< Will not work in UAVCAN_TINY
 
     ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback> get_node_info_client_;
-    Subscriber<protocol::NodeStatus, NodeStatusCallback, NumNodeStatusStaticReceivers> node_status_sub_;
+    Subscriber<protocol::NodeStatus, NodeStatusCallback, NumNodeStatusStaticReceivers, 0> node_status_sub_;
 
     /*
      * Methods
@@ -121,34 +123,9 @@ class NodeDiscoverer : TimerBase
 
     NodeID pickNextNodeToQuery() const
     {
-        const unsigned node_map_size = node_map_.getSize();
-
-        // Searching the lowest number of attempts made
-        uint8_t lowest_number_of_attempts = static_cast<uint8_t>(MaxAttemptsToGetNodeInfo + 1U);
-        for (unsigned i = 0; i < node_map_size; i++)
-        {
-            const NodeMap::KVPair* const kv = node_map_.getByIndex(i);
-            UAVCAN_ASSERT(kv != NULL);
-            lowest_number_of_attempts = min(lowest_number_of_attempts, kv->value.num_get_node_info_attempts);
-        }
-
-        // Now, among nodes with this number of attempts selecting the one with highest uptime.
-        NodeID output;
-        uint32_t largest_uptime = 0;
-        for (unsigned i = 0; i < node_map_size; i++)
-        {
-            const NodeMap::KVPair* const kv = node_map_.getByIndex(i);
-            UAVCAN_ASSERT(kv != NULL);
-            if ((kv->value.num_get_node_info_attempts == lowest_number_of_attempts) &&
-                (kv->value.last_seen_uptime >= largest_uptime))
-            {
-                largest_uptime = kv->value.last_seen_uptime;
-                output = kv->key;
-            }
-        }
-
-        // An invalid node ID will be returned only if there's no nodes at all.
-        return output;
+        // This essentially means that we pick first available node. Remember that the map is unordered.
+        const NodeMap::KVPair* const pair = node_map_.getByIndex(0);
+        return (pair == NULL) ? NodeID() : pair->key;
     }
 
     bool needToQuery(NodeID node_id)
@@ -233,14 +210,14 @@ class NodeDiscoverer : TimerBase
         if (result.isSuccessful())
         {
             UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "GetNodeInfo response from %d",
-                         int(result.server_node_id.get()));
-            finalizeNodeDiscovery(&result.response.hardware_version.unique_id, result.server_node_id);
+                         int(result.getCallID().server_node_id.get()));
+            finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id);
         }
         else
         {
-            trace(TraceDiscoveryGetNodeInfoFailure, result.server_node_id.get());
+            trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get());
 
-            NodeData* const data = node_map_.access(result.server_node_id);
+            NodeData* const data = node_map_.access(result.getCallID().server_node_id);
             if (data == NULL)
             {
                 return;         // Probably it is a known node now
@@ -248,11 +225,11 @@ class NodeDiscoverer : TimerBase
 
             UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer",
                          "GetNodeInfo request to %d has timed out, %d attempts",
-                         int(result.server_node_id.get()), int(data->num_get_node_info_attempts));
+                         int(result.getCallID().server_node_id.get()), int(data->num_get_node_info_attempts));
             data->num_get_node_info_attempts++;
             if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo)
             {
-                finalizeNodeDiscovery(NULL, result.server_node_id);
+                finalizeNodeDiscovery(NULL, result.getCallID().server_node_id);
                 // Warning: data pointer is invalidated now
             }
         }
@@ -260,16 +237,11 @@ class NodeDiscoverer : TimerBase
 
     void handleTimerEvent(const TimerEvent&)
     {
-        if (get_node_info_client_.isPending())
+        if (get_node_info_client_.hasPendingCalls())
         {
             return;
         }
 
-        if (!handler_.canDiscoverNewNodes())
-        {
-            return;     // Timer must continue to run in order to not stuck when it unlocks
-        }
-
         const NodeID node_id = pickNextNodeToQueryAndCleanupMap();
         if (!node_id.isUnicast())
         {
@@ -278,6 +250,11 @@ class NodeDiscoverer : TimerBase
             return;
         }
 
+        if (!handler_.canDiscoverNewNodes())
+        {
+            return;     // Timer must continue to run in order to not stuck when it unlocks
+        }
+
         trace(TraceDiscoveryGetNodeInfoRequest, node_id.get());
 
         UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d",
@@ -319,7 +296,7 @@ class NodeDiscoverer : TimerBase
 
         if (!isRunning())
         {
-            startPeriodic(get_node_info_client_.getRequestTimeout() * 2);
+            startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs));
             trace(TraceDiscoveryTimerStart, getPeriod().toUSec());
         }
     }
@@ -359,6 +336,12 @@ public:
      * Returns true if there's at least one node with pending GetNodeInfo.
      */
     bool hasUnknownNodes() const { return !node_map_.isEmpty(); }
+
+    /**
+     * Returns number of nodes that are being queried at the moment.
+     * This method is needed for testing and state visualization.
+     */
+    uint8_t getNumUnknownNodes() const { return static_cast<uint8_t>(node_map_.getSize()); }
 };
 
 }

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

@@ -50,7 +50,6 @@ public:
         }
 
         candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes;
-        candidate--;        // This has been tested already
 
         // Down
         while (candidate > 0)

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

@@ -19,7 +19,7 @@ namespace dynamic_node_id_server
  * and efficient implementation of storage backends, e.g. based on text files.
  * Keys and values may contain only non-whitespace, non-formatting printable characters.
  */
-class IStorageBackend
+class UAVCAN_EXPORT IStorageBackend
 {
 public:
     /**
@@ -37,7 +37,7 @@ public:
      * This type is used to exchange data chunks with the backend.
      * It doesn't use any dynamic memory; please refer to the Array<> class for details.
      */
-    typedef Array<IntegerSpec<8, SignednessUnsigned, CastModeTruncate>, ArrayModeDynamic, MaxStringLength> String;
+    typedef MakeString<MaxStringLength>::Type String;
 
     /**
      * Read one value from the storage.

libuavcan/include/uavcan/protocol/file_server.hpp

@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/node/service_server.hpp>
+#include <uavcan/util/method_binder.hpp>
+// UAVCAN types
+#include <uavcan/protocol/file/GetInfo.hpp>
+#include <uavcan/protocol/file/GetDirectoryEntryInfo.hpp>
+#include <uavcan/protocol/file/Read.hpp>
+#include <uavcan/protocol/file/Write.hpp>
+#include <uavcan/protocol/file/Delete.hpp>
+
+namespace uavcan
+{
+/**
+ * The file server backend should implement this interface.
+ * Note that error codes returned by these methods are defined in uavcan.protocol.file.Error; these are
+ * not the same as libuavcan-internal error codes defined in uavcan.error.hpp.
+ */
+class UAVCAN_EXPORT IFileServerBackend
+{
+public:
+    typedef protocol::file::Path::FieldTypes::path Path;
+    typedef protocol::file::EntryType EntryType;
+    typedef protocol::file::Error Error;
+
+    /**
+     * Use this class to compute CRC64 for uavcan.protocol.file.GetInfo.
+     */
+    typedef DataTypeSignatureCRC FileCRC;
+
+    /**
+     * All read operations must return this number of bytes, unless end of file is reached.
+     */
+    enum { ReadSize = protocol::file::Read::Response::FieldTypes::data::MaxSize };
+
+    /**
+     * Shortcut for uavcan.protocol.file.Path.SEPARATOR.
+     */
+    static char getPathSeparator() { return static_cast<char>(protocol::file::Path::SEPARATOR); }
+
+    /**
+     * Backend for uavcan.protocol.file.GetInfo.
+     * Refer to uavcan.protocol.file.EntryType for the list of available bit flags.
+     * Implementation of this method is required.
+     * On success the method must return zero.
+     */
+    virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) = 0;
+
+    /**
+     * Backend for uavcan.protocol.file.Read.
+     * Implementation of this method is required.
+     * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except
+     * if the end of file is reached.
+     * On success the method must return zero.
+     */
+    virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0;
+
+    // Methods below are optional.
+
+    /**
+     * Backend for uavcan.protocol.file.Write.
+     * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
+     * On success the method must return zero.
+     */
+    virtual int16_t write(const Path& path, const uint32_t offset, const uint8_t* buffer, const uint16_t size)
+    {
+        (void)path;
+        (void)offset;
+        (void)buffer;
+        (void)size;
+        return Error::NOT_IMPLEMENTED;
+    }
+
+    /**
+     * Backend for uavcan.protocol.file.Delete. ('delete' is a C++ keyword, so 'remove' is used instead)
+     * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
+     * On success the method must return zero.
+     */
+    virtual int16_t remove(const Path& path)
+    {
+        (void)path;
+        return Error::NOT_IMPLEMENTED;
+    }
+
+    /**
+     * Backend for uavcan.protocol.file.GetDirectoryEntryInfo.
+     * Refer to uavcan.protocol.file.EntryType for the list of available bit flags.
+     * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED.
+     * On success the method must return zero.
+     */
+    virtual int16_t getDirectoryEntryInfo(const Path& directory_path, const uint32_t entry_index,
+                                          EntryType& out_type, Path& out_entry_full_path)
+    {
+        (void)directory_path;
+        (void)entry_index;
+        (void)out_type;
+        (void)out_entry_full_path;
+        return Error::NOT_IMPLEMENTED;
+    }
+
+    virtual ~IFileServerBackend() { }
+};
+
+/**
+ * Basic file server implements only the following services:
+ *      uavcan.protocol.file.GetInfo
+ *      uavcan.protocol.file.Read
+ * Also see @ref IFileServerBackend.
+ */
+class BasicFileServer
+{
+    typedef MethodBinder<BasicFileServer*,
+        void (BasicFileServer::*)(const protocol::file::GetInfo::Request&, protocol::file::GetInfo::Response&)>
+            GetInfoCallback;
+
+    typedef MethodBinder<BasicFileServer*,
+        void (BasicFileServer::*)(const protocol::file::Read::Request&, protocol::file::Read::Response&)>
+            ReadCallback;
+
+    ServiceServer<protocol::file::GetInfo, GetInfoCallback> get_info_srv_;
+    ServiceServer<protocol::file::Read, ReadCallback> read_srv_;
+
+    void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp)
+    {
+        resp.error.value = backend_.getInfo(req.path.path, resp.crc64, resp.size, resp.entry_type);
+    }
+
+    void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp)
+    {
+        uint16_t inout_size = resp.data.capacity();
+
+        resp.data.resize(inout_size);
+
+        resp.error.value = backend_.read(req.path.path, req.offset, resp.data.begin(), inout_size);
+
+        if (resp.error.value != protocol::file::Error::OK)
+        {
+            inout_size = 0;
+        }
+
+        if (inout_size > resp.data.capacity())
+        {
+            UAVCAN_ASSERT(0);
+            resp.error.value = protocol::file::Error::UNKNOWN_ERROR;
+        }
+        else
+        {
+            resp.data.resize(inout_size);
+        }
+    }
+
+protected:
+    IFileServerBackend& backend_;       ///< Derived types can use it
+
+public:
+    BasicFileServer(INode& node, IFileServerBackend& backend)
+        : get_info_srv_(node)
+        , read_srv_(node)
+        , backend_(backend)
+    { }
+
+    int start()
+    {
+        int res = get_info_srv_.start(GetInfoCallback(this, &BasicFileServer::handleGetInfo));
+        if (res < 0)
+        {
+            return res;
+        }
+
+        res = read_srv_.start(ReadCallback(this, &BasicFileServer::handleRead));
+        if (res < 0)
+        {
+            return res;
+        }
+
+        return 0;
+    }
+};
+
+/**
+ * Full file server implements all file services:
+ *      uavcan.protocol.file.GetInfo
+ *      uavcan.protocol.file.Read
+ *      uavcan.protocol.file.Write
+ *      uavcan.protocol.file.Delete
+ *      uavcan.protocol.file.GetDirectoryEntryInfo
+ * Also see @ref IFileServerBackend.
+ */
+class FileServer : protected BasicFileServer
+{
+    typedef MethodBinder<FileServer*,
+        void (FileServer::*)(const protocol::file::Write::Request&, protocol::file::Write::Response&)>
+            WriteCallback;
+
+    typedef MethodBinder<FileServer*,
+        void (FileServer::*)(const protocol::file::Delete::Request&, protocol::file::Delete::Response&)>
+            DeleteCallback;
+
+    typedef MethodBinder<FileServer*,
+        void (FileServer::*)(const protocol::file::GetDirectoryEntryInfo::Request&,
+                             protocol::file::GetDirectoryEntryInfo::Response&)>
+            GetDirectoryEntryInfoCallback;
+
+    ServiceServer<protocol::file::Write, WriteCallback> write_srv_;
+    ServiceServer<protocol::file::Delete, DeleteCallback> delete_srv_;
+    ServiceServer<protocol::file::GetDirectoryEntryInfo, GetDirectoryEntryInfoCallback> get_directory_entry_info_srv_;
+
+    void handleWrite(const protocol::file::Write::Request& req, protocol::file::Write::Response& resp)
+    {
+        resp.error.value = backend_.write(req.path.path, req.offset, req.data.begin(), req.data.size());
+    }
+
+    void handleDelete(const protocol::file::Delete::Request& req, protocol::file::Delete::Response& resp)
+    {
+        resp.error.value = backend_.remove(req.path.path);
+    }
+
+    void handleGetDirectoryEntryInfo(const protocol::file::GetDirectoryEntryInfo::Request& req,
+                                     protocol::file::GetDirectoryEntryInfo::Response& resp)
+    {
+        resp.error.value = backend_.getDirectoryEntryInfo(req.directory_path.path, req.entry_index,
+                                                          resp.entry_type, resp.entry_full_path.path);
+    }
+
+public:
+    FileServer(INode& node, IFileServerBackend& backend)
+        : BasicFileServer(node, backend)
+        , write_srv_(node)
+        , delete_srv_(node)
+        , get_directory_entry_info_srv_(node)
+    { }
+
+    int start()
+    {
+        int res = BasicFileServer::start();
+        if (res < 0)
+        {
+            return res;
+        }
+
+        res = write_srv_.start(WriteCallback(this, &FileServer::handleWrite));
+        if (res < 0)
+        {
+            return res;
+        }
+
+        res = delete_srv_.start(DeleteCallback(this, &FileServer::handleDelete));
+        if (res < 0)
+        {
+            return res;
+        }
+
+        res = get_directory_entry_info_srv_.start(
+            GetDirectoryEntryInfoCallback(this, &FileServer::handleGetDirectoryEntryInfo));
+        if (res < 0)
+        {
+            return res;
+        }
+
+        return 0;
+    }
+};
+
+}
+
+#endif // Include guard

libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp

@@ -0,0 +1,476 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/util/method_binder.hpp>
+#include <uavcan/util/map.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+// UAVCAN types
+#include <uavcan/protocol/file/BeginFirmwareUpdate.hpp>
+
+namespace uavcan
+{
+/**
+ * Application-specific firmware version checking logic.
+ * Refer to @ref FirmwareUpdateTrigger for details.
+ */
+class IFirmwareVersionChecker
+{
+public:
+    /**
+     * This value is limited by the pool block size minus some extra data. Please refer to the Map<> documentation
+     * for details. If this size is set too high, the compilation will fail in the Map<> template.
+     */
+    enum { MaxFirmwareFilePathLength = 40 };
+
+    /**
+     * This type is used to store path to firmware file that the target node will retrieve using the
+     * service uavcan.protocol.file.Read. Note that the maximum length is limited due to some specifics of
+     * libuavcan (@ref MaxFirmwareFilePathLength), this is NOT a protocol-level limitation.
+     */
+    typedef MakeString<MaxFirmwareFilePathLength>::Type FirmwareFilePath;
+
+    /**
+     * This method will be invoked when the class obtains a response to GetNodeInfo request.
+     *
+     * @param node_id                   Node ID that this GetNodeInfo response was received from.
+     *
+     * @param node_info                 Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details.
+     *
+     * @param out_firmware_file_path    The implementation should return the firmware image path via this argument.
+     *                                  Note that this path must be reachable via uavcan.protocol.file.Read service.
+     *                                  Refer to @ref FileServer and @ref BasicFileServer for details.
+     *
+     * @return                          True - the class will begin sending update requests.
+     *                                  False - the node will be ignored, no request will be sent.
+     */
+    virtual bool shouldRequestFirmwareUpdate(NodeID node_id, const protocol::GetNodeInfo::Response& node_info,
+                                             FirmwareFilePath& out_firmware_file_path) = 0;
+
+    /**
+     * This method will be invoked when a node responds to the update request with an error. If the request simply
+     * times out, this method will not be invoked.
+     * Note that if by the time of arrival of the response the node is already removed, this method will not be called.
+     *
+     * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting
+     *               is not needed anymore. This method will not be invoked.
+     *
+     * @param node_id                   Node ID that returned this error.
+     *
+     * @param error_response            Contents of the error response. It contains error code and text.
+     *
+     * @param out_firmware_file_path    New firmware path if a retry is needed. Note that this argument will be
+     *                                  initialized with old path, so if the same path needs to be reused, this
+     *                                  argument should be left unchanged.
+     *
+     * @return                          True - the class will continue sending update requests with new firmware path.
+     *                                  False - the node will be forgotten, new requests will not be sent.
+     */
+    virtual bool shouldRetryFirmwareUpdate(NodeID node_id,
+                                           const protocol::file::BeginFirmwareUpdate::Response& error_response,
+                                           FirmwareFilePath& out_firmware_file_path) = 0;
+
+    /**
+     * This node is invoked when the node responds to the update request with confirmation.
+     * Note that if by the time of arrival of the response the node is already removed, this method will not be called.
+     *
+     * Implementation is optional; default one does nothing.
+     *
+     * @param node_id   Node ID that confirmed the request.
+     *
+     * @param response  Actual response.
+     */
+    virtual void handleFirmwareUpdateConfirmation(NodeID node_id,
+                                                  const protocol::file::BeginFirmwareUpdate::Response& response)
+    {
+        (void)node_id;
+        (void)response;
+    }
+
+    virtual ~IFirmwareVersionChecker() { }
+};
+
+/**
+ * This class subscribes to updates from @ref NodeInfoRetriever in order to detect nodes that need firmware
+ * updates. The decision process of whether a firmware update is needed is relayed to the application via
+ * @ref IFirmwareVersionChecker. If the application confirms that the update is needed, this class will begin
+ * sending uavcan.protocol.file.BeginFirmwareUpdate periodically (period is configurable) to every node that
+ * needs an update in a round-robin fashion. There are the following termination conditions for the periodical
+ * sending process:
+ *
+ * - The node responds with confirmation. In this case the class will forget about the node on the assumption
+ *   that its job is done here. Confirmation will be reported to the application via the interface.
+ *
+ * - The node responds with an error, and the error code is not ERROR_IN_PROGRESS. In this case the class will
+ *   request the application via the interface mentioned above about its further actions - either give up or
+ *   retry, possibly with a different firmware.
+ *
+ * - The node responds with error ERROR_IN_PROGRESS. In this case the class behaves exactly in the same way as if
+ *   response was successful (because the firmware is alredy being updated, so the goal is fulfilled).
+ *   Confirmation will be reported to the application via the interface.
+ *
+ * - The node goes offline or restarts. In this case the node will be immediately forgotten, and the process
+ *   will repeat again later because the node info retriever re-queries GetNodeInfo every time when a node restarts.
+ *
+ * Since the target node (i.e. node that is being updated) will try to retrieve the specified firmware file using
+ * the file services (uavcan.protocol.file.*), the provided firmware path must be reachable for the file server
+ * (@ref FileServer, @ref BasicFileServer). Normally, an application that serves as UAVCAN firmware update server
+ * will include at least the following components:
+ * - this firmware update trigger;
+ * - dynamic node ID allocation server;
+ * - file server.
+ *
+ * Implementation details: the class uses memory pool to keep the list of nodes that have not responded yet, which
+ * limits the maximum length of the path to the firmware file, which is covered in @ref IFirmwareVersionChecker.
+ * To somewhat relieve the maximum path length limitation, the class can be supplied with a common prefix that
+ * will be prepended to firmware pathes before sending requests.
+ * Interval at which requests are being sent is configurable, but the default value should cover the needs of
+ * virtually all use cases (as always).
+ */
+class FirmwareUpdateTrigger : public INodeInfoListener,
+                              private TimerBase
+{
+    typedef MethodBinder<FirmwareUpdateTrigger*,
+        void (FirmwareUpdateTrigger::*)(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>&)>
+            BeginFirmwareUpdateResponseCallback;
+
+    typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath;
+
+    enum { DefaultRequestIntervalMs = 1000 };   ///< Shall not be less than default service response timeout.
+
+    struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable
+    {
+        enum { DefaultOutput = 0xFFU };
+
+        const FirmwareUpdateTrigger& owner;
+        uint8_t output;
+
+        NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner)
+            : owner(arg_owner)
+            , output(DefaultOutput)
+        { }
+
+        bool operator()(const NodeID node_id, const FirmwareFilePath&)
+        {
+            if (node_id.get() > owner.last_queried_node_id_ &&
+                !owner.begin_fw_update_client_.hasPendingCallToServer(node_id))
+            {
+                output = min(output, node_id.get());
+            }
+            return false;
+        }
+    };
+
+    /*
+     * State
+     */
+    ServiceClient<protocol::file::BeginFirmwareUpdate, BeginFirmwareUpdateResponseCallback> begin_fw_update_client_;
+
+    IFirmwareVersionChecker& checker_;
+
+    NodeInfoRetriever* node_info_retriever_;
+
+    Map<NodeID, FirmwareFilePath> pending_nodes_;
+
+    MonotonicDuration request_interval_;
+
+    FirmwareFilePath common_path_prefix_;
+
+    mutable uint8_t last_queried_node_id_;
+
+    /*
+     * Methods of INodeInfoListener
+     */
+    virtual void handleNodeInfoUnavailable(NodeID node_id)
+    {
+        UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get()));
+        pending_nodes_.remove(node_id); // For extra paranoia
+    }
+
+    virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info)
+    {
+        FirmwareFilePath firmware_file_path;
+        const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path);
+        if (update_needed)
+        {
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s",
+                         int(node_id.get()), firmware_file_path.c_str());
+            trySetPendingNode(node_id, firmware_file_path);
+        }
+        else
+        {
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get()));
+            pending_nodes_.remove(node_id);
+        }
+    }
+
+    virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
+    {
+        if (event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE ||
+            !event.status.known)
+        {
+            pending_nodes_.remove(event.node_id);
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get()));
+        }
+    }
+
+    /*
+     * Own methods
+     */
+    INode& getNode() { return begin_fw_update_client_.getNode(); }
+
+    void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path)
+    {
+        if (NULL != pending_nodes_.insert(node_id, path))
+        {
+            if (!TimerBase::isRunning())
+            {
+                TimerBase::startPeriodic(request_interval_);
+                UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer started");
+            }
+        }
+        else
+        {
+            getNode().registerInternalFailure("FirmwareUpdateTrigger OOM");
+        }
+    }
+
+    NodeID pickNextNodeID() const
+    {
+        // We can't do index search because indices are unstable in Map<>
+        // First try - from the current node up
+        NextNodeIDSearchPredicate s1(*this);
+        (void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s1);
+
+        if (s1.output != NextNodeIDSearchPredicate::DefaultOutput)
+        {
+            last_queried_node_id_ = s1.output;
+        }
+        else if (last_queried_node_id_ != 0)
+        {
+            // Nothing was found, resetting the selector and trying again
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_));
+            last_queried_node_id_ = 0;
+
+            NextNodeIDSearchPredicate s2(*this);
+            (void)pending_nodes_.find<NextNodeIDSearchPredicate&>(s2);
+
+            if (s2.output != NextNodeIDSearchPredicate::DefaultOutput)
+            {
+                last_queried_node_id_ = s2.output;
+            }
+        }
+        else
+        {
+            ; // Hopeless
+        }
+        UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u",
+                     int(last_queried_node_id_), pending_nodes_.getSize(),
+                     begin_fw_update_client_.getNumPendingCalls());
+        return last_queried_node_id_;
+    }
+
+    void handleBeginFirmwareUpdateResponse(const ServiceCallResult<protocol::file::BeginFirmwareUpdate>& result)
+    {
+        if (!result.isSuccessful())
+        {
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry",
+                         int(result.getCallID().server_node_id.get()));
+            return;
+        }
+
+        FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id);
+        if (old_path == NULL)
+        {
+            // The entry has been removed, assuming that it's not needed anymore
+            return;
+        }
+
+        const bool confirmed =
+            result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK ||
+            result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS;
+
+        if (confirmed)
+        {
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request",
+                         int(result.getCallID().server_node_id.get()));
+            pending_nodes_.remove(result.getCallID().server_node_id);
+            checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse());
+        }
+        else
+        {
+            UAVCAN_ASSERT(old_path != NULL);
+            UAVCAN_ASSERT(TimerBase::isRunning());
+            // We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer
+
+            const bool update_needed =
+                checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path);
+
+            if (!update_needed)
+            {
+                UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry",
+                             int(result.getCallID().server_node_id.get()));
+                pending_nodes_.remove(result.getCallID().server_node_id);
+            }
+        }
+    }
+
+    virtual void handleTimerEvent(const TimerEvent&)
+    {
+        if (pending_nodes_.isEmpty())
+        {
+            TimerBase::stop();
+            UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped");
+            return;
+        }
+
+        const NodeID node_id = pickNextNodeID();
+        if (!node_id.isUnicast())
+        {
+            return;
+        }
+
+        FirmwareFilePath* const path = pending_nodes_.access(node_id);
+        if (path == NULL)
+        {
+            UAVCAN_ASSERT(0);   // pickNextNodeID() returned a node ID that is not present in the map
+            return;
+        }
+
+        protocol::file::BeginFirmwareUpdate::Request req;
+
+        req.source_node_id = getNode().getNodeID().get();
+        if (!common_path_prefix_.empty())
+        {
+            req.image_file_remote_path.path += common_path_prefix_.c_str();
+            req.image_file_remote_path.path.push_back(protocol::file::Path::SEPARATOR);
+        }
+        req.image_file_remote_path.path += path->c_str();
+
+        UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s",
+                     int(node_id.get()), req.image_file_remote_path.path.c_str());
+
+        const int call_res = begin_fw_update_client_.call(node_id, req);
+        if (call_res < 0)
+        {
+            getNode().registerInternalFailure("FirmwareUpdateTrigger call");
+        }
+    }
+
+public:
+    FirmwareUpdateTrigger(INode& node, IFirmwareVersionChecker& checker)
+        : TimerBase(node)
+        , begin_fw_update_client_(node)
+        , checker_(checker)
+        , node_info_retriever_(NULL)
+        , pending_nodes_(node.getAllocator())
+        , request_interval_(MonotonicDuration::fromMSec(DefaultRequestIntervalMs))
+        , last_queried_node_id_(0)
+    { }
+
+    ~FirmwareUpdateTrigger()
+    {
+        if (node_info_retriever_ != NULL)
+        {
+            node_info_retriever_->removeListener(this);
+        }
+    }
+
+    /**
+     * Starts the object. Once started, it can't be stopped unless destroyed.
+     *
+     * @param node_info_retriever       The object will register itself against this retriever.
+     *                                  When the destructor is called, the object will unregister itself.
+     *
+     * @param common_path_prefix        If set, this path will be prefixed to all firmware pathes provided by the
+     *                                  application interface. The prefix does not need to end with path separator;
+     *                                  the last trailing one will be removed (so use '//' if you need '/').
+     *                                  By default the prefix is empty.
+     *
+     * @return                          Negative error code.
+     */
+    int start(NodeInfoRetriever& node_info_retriever,
+              const FirmwareFilePath& arg_common_path_prefix = FirmwareFilePath())
+    {
+        /*
+         * Configuring the node info retriever
+         */
+        node_info_retriever_ = &node_info_retriever;
+
+        int res = node_info_retriever_->addListener(this);
+        if (res < 0)
+        {
+            return res;
+        }
+
+        /*
+         * Initializing the prefix, removing only the last trailing path separator.
+         */
+        common_path_prefix_ = arg_common_path_prefix;
+
+        if (!common_path_prefix_.empty() &&
+            *(common_path_prefix_.end() - 1) == protocol::file::Path::SEPARATOR)
+        {
+            common_path_prefix_.resize(uint8_t(common_path_prefix_.size() - 1U));
+        }
+
+        /*
+         * Initializing the client
+         */
+        res = begin_fw_update_client_.init();
+        if (res < 0)
+        {
+            return res;
+        }
+        begin_fw_update_client_.setCallback(
+            BeginFirmwareUpdateResponseCallback(this, &FirmwareUpdateTrigger::handleBeginFirmwareUpdateResponse));
+
+        // The timer will be started ad-hoc
+        return 0;
+    }
+
+    /**
+     * Interval at which uavcan.protocol.file.BeginFirmwareUpdate requests are being sent.
+     * Note that default value should be OK for any use case.
+     */
+    MonotonicDuration getRequestInterval() const { return request_interval_; }
+    void setRequestInterval(const MonotonicDuration interval)
+    {
+        if (interval.isPositive())
+        {
+            request_interval_ = interval;
+            if (TimerBase::isRunning())     // Restarting with new interval
+            {
+                TimerBase::startPeriodic(request_interval_);
+            }
+        }
+        else
+        {
+            UAVCAN_ASSERT(0);
+        }
+    }
+
+    /**
+     * This method is mostly needed for testing.
+     * When triggering is not in progress, the class consumes zero CPU time.
+     */
+    bool isTimerRunning() const { return TimerBase::isRunning(); }
+
+    unsigned getNumPendingNodes() const
+    {
+        const unsigned ret = pending_nodes_.getSize();
+        UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true);
+        return ret;
+    }
+
+};
+
+}
+
+#endif // Include guard

libuavcan/include/uavcan/protocol/network_compat_checker.hpp

@@ -87,7 +87,7 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable
 
     int waitForCATSResponse()
     {
-        while (cats_cln_.isPending())
+        while (cats_cln_.hasPendingCalls())
         {
             const int res = getNode().spin(MonotonicDuration::fromMSec(10));
             if (res < 0 || !result_.isOk())
@@ -119,15 +119,15 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable
         if (last_cats_request_ok_)
         {
             const DataTypeSignature sign = GlobalDataTypeRegistry::instance().
-                computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids);
+                computeAggregateSignature(checking_dtkind_, resp.getResponse().mutually_known_ids);
 
             UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu",
-                         int(resp.server_node_id.get()), static_cast<unsigned long long>(sign.get()),
-                         static_cast<unsigned long long>(resp.response.aggregate_signature));
+                         int(resp.getCallID().server_node_id.get()), static_cast<unsigned long long>(sign.get()),
+                         static_cast<unsigned long long>(resp.getResponse().aggregate_signature));
 
-            if (sign.get() != resp.response.aggregate_signature)
+            if (sign.get() != resp.getResponse().aggregate_signature)
             {
-                result_.conflicting_node = resp.server_node_id;
+                result_.conflicting_node = resp.getCallID().server_node_id;
             }
         }
     }
@@ -138,7 +138,7 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable
         StaticAssert<DataTypeKindService == int(protocol::DataTypeKind::SERVICE)>::check();
 
         UAVCAN_ASSERT(nid.isUnicast());
-        UAVCAN_ASSERT(!cats_cln_.isPending());
+        UAVCAN_ASSERT(!cats_cln_.hasPendingCalls());
 
         checking_dtkind_ = kind;
         protocol::ComputeAggregateTypeSignature::Request request;
@@ -253,7 +253,7 @@ public:
 
     exit:
         ns_sub_.stop();
-        cats_cln_.cancel();
+        cats_cln_.cancelAllCalls();
         return res;
     }
 

libuavcan/include/uavcan/protocol/node_info_retriever.hpp

@@ -0,0 +1,450 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
+#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED
+
+#include <uavcan/build_config.hpp>
+#include <uavcan/debug.hpp>
+#include <uavcan/util/multiset.hpp>
+#include <uavcan/node/service_client.hpp>
+#include <uavcan/node/timer.hpp>
+#include <uavcan/protocol/node_status_monitor.hpp>
+#include <uavcan/protocol/GetNodeInfo.hpp>
+
+namespace uavcan
+{
+/**
+ * Classes that need to receive GetNodeInfo responses should implement this interface.
+ */
+class UAVCAN_EXPORT INodeInfoListener
+{
+public:
+    /**
+     * Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or
+     * becomes online for the first time.
+     * @param node_id   Node ID of the node
+     * @param response  Node info struct
+     */
+    virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0;
+
+    /**
+     * Called when the retriever decides that the node does not support the GetNodeInfo service.
+     * This method will never be called if the number of attempts is unlimited.
+     */
+    virtual void handleNodeInfoUnavailable(NodeID node_id) = 0;
+
+    /**
+     * This call is routed directly from @ref NodeStatusMonitor.
+     * Default implementation does nothing.
+     * @param event     Node status change event
+     */
+    virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event)
+    {
+        (void)event;
+    }
+
+    /**
+     * This call is routed directly from @ref NodeStatusMonitor.
+     * Default implementation does nothing.
+     * @param msg       Node status message
+     */
+    virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+    {
+        (void)msg;
+    }
+
+    virtual ~INodeInfoListener() { }
+};
+
+/**
+ * This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts.
+ * It does a number of attempts in case if there's a communication failure before assuming that the node does not
+ * implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit
+ * virtually any use case, but they can be overriden if needed - refer to the setter methods below for details.
+ *
+ * Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local
+ * node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo
+ * service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are
+ * unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate
+ * for the end application, the request interval can be overriden via @ref setRequestInterval().
+ *
+ * Following the above explained requirements, the default request interval is defined as follows:
+ *      request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes)
+ * Which yields 40 ms.
+ *
+ * Given default service timeout 500 ms and the defined above request frequency 40 ms, the maximum number of
+ * concurrent requests will be:
+ *      max concurrent requests = ceil(500 [ms] timeout / 40 [ms] request interval)
+ * Which yields 13 requests.
+ *
+ * Keep the above equations in mind when changing the default request interval.
+ *
+ * Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never
+ * exceed one. This is actually the most likely scenario.
+ *
+ * Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc.
+ *
+ * Events from this class can be routed to many listeners, @ref INodeInfoListener.
+ */
+class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor
+                                      , TimerBase
+{
+public:
+    enum { MaxNumRequestAttempts = 254 };
+    enum { UnlimitedRequestAttempts = 0 };
+
+private:
+    typedef MethodBinder<NodeInfoRetriever*,
+                         void (NodeInfoRetriever::*)(const ServiceCallResult<protocol::GetNodeInfo>&)>
+            GetNodeInfoResponseCallback;
+
+    struct Entry
+    {
+        uint32_t uptime_sec;
+        uint8_t num_attempts_made;
+        bool request_needed;                    ///< Always false for unknown nodes
+        bool updated_since_last_attempt;        ///< Always false for unknown nodes
+
+        Entry()
+            : uptime_sec(0)
+            , num_attempts_made(0)
+            , request_needed(false)
+            , updated_since_last_attempt(false)
+        {
+#if UAVCAN_DEBUG
+            StaticAssert<sizeof(Entry) <= 8>::check();
+#endif
+        }
+    };
+
+    struct NodeInfoRetrievedHandlerCaller
+    {
+        const NodeID node_id;
+        const protocol::GetNodeInfo::Response& node_info;
+
+        NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info)
+            : node_id(arg_node_id)
+            , node_info(arg_node_info)
+        { }
+
+        bool operator()(INodeInfoListener* key)
+        {
+            UAVCAN_ASSERT(key != NULL);
+            key->handleNodeInfoRetrieved(node_id, node_info);
+            return false;
+        }
+    };
+
+    template <typename Event>
+    struct GenericHandlerCaller
+    {
+        void (INodeInfoListener::* const method)(Event);
+        Event event;
+
+        GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event)
+            : method(arg_method)
+            , event(arg_event)
+        { }
+
+        bool operator()(INodeInfoListener* key)
+        {
+            UAVCAN_ASSERT(key != NULL);
+            (key->*method)(event);
+            return false;
+        }
+    };
+
+    enum { NumStaticCalls = 2 };
+    enum { DefaultNumRequestAttempts = 16 };
+    enum { DefaultTimerIntervalMSec = 40 };  ///< Read explanation in the class documentation
+
+    /*
+     * State
+     */
+    Entry entries_[NodeID::Max];  // [1, NodeID::Max]
+
+    Multiset<INodeInfoListener*, 2> listeners_;
+
+    ServiceClient<protocol::GetNodeInfo, GetNodeInfoResponseCallback, NumStaticCalls> get_node_info_client_;
+
+    MonotonicDuration request_interval_;
+
+    mutable uint8_t last_picked_node_;
+
+    uint8_t num_attempts_;
+
+    /*
+     * Methods
+     */
+    const Entry& getEntry(NodeID node_id) const { return const_cast<NodeInfoRetriever*>(this)->getEntry(node_id); }
+    Entry&       getEntry(NodeID node_id)
+    {
+        if (node_id.get() < 1 || node_id.get() > NodeID::Max)
+        {
+            handleFatalError("NodeInfoRetriever NodeID");
+        }
+        return entries_[node_id.get() - 1];
+    }
+
+    void startTimerIfNotRunning()
+    {
+        if (!TimerBase::isRunning())
+        {
+            TimerBase::startPeriodic(request_interval_);
+            UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str());
+        }
+    }
+
+    NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const
+    {
+        out_at_least_one_request_needed = false;
+
+        for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin
+        {
+            last_picked_node_++;
+            if (last_picked_node_ > NodeID::Max)
+            {
+                last_picked_node_ = 1;
+            }
+            UAVCAN_ASSERT((last_picked_node_ >= 1) &&
+                          (last_picked_node_ <= NodeID::Max));
+
+            const Entry& entry = getEntry(last_picked_node_);
+
+            if (entry.request_needed)
+            {
+                out_at_least_one_request_needed = true;
+
+                if (entry.updated_since_last_attempt &&
+                    !get_node_info_client_.hasPendingCallToServer(last_picked_node_))
+                {
+                    UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_));
+                    return NodeID(last_picked_node_);
+                }
+            }
+        }
+
+        return NodeID();        // No node could be found
+    }
+
+    virtual void handleTimerEvent(const TimerEvent&)
+    {
+        bool at_least_one_request_needed = false;
+        const NodeID next = pickNextNodeToQuery(at_least_one_request_needed);
+
+        if (next.isUnicast())
+        {
+            UAVCAN_ASSERT(at_least_one_request_needed);
+            getEntry(next).updated_since_last_attempt = false;
+            const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request());
+            if (res < 0)
+            {
+                get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call");
+            }
+        }
+        else
+        {
+            if (!at_least_one_request_needed)
+            {
+                TimerBase::stop();
+                UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped");
+            }
+        }
+    }
+
+    virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event)
+    {
+        const bool was_offline = !event.old_status.known ||
+                                 (event.old_status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
+
+        const bool offline_now = !event.status.known ||
+                                 (event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE);
+
+        if (was_offline || offline_now)
+        {
+            Entry& entry = getEntry(event.node_id);
+
+            entry.request_needed = !offline_now;
+            entry.num_attempts_made = 0;
+
+            UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d",
+                         int(event.node_id.get()), int(entry.request_needed));
+
+            if (entry.request_needed)
+            {
+                startTimerIfNotRunning();
+            }
+        }
+
+        listeners_.forEach(
+            GenericHandlerCaller<const NodeStatusChangeEvent&>(&INodeInfoListener::handleNodeStatusChange, event));
+    }
+
+    virtual void handleNodeStatusMessage(const ReceivedDataStructure<protocol::NodeStatus>& msg)
+    {
+        Entry& entry = getEntry(msg.getSrcNodeID());
+
+        if (msg.uptime_sec < entry.uptime_sec)
+        {
+            entry.request_needed = true;
+            entry.num_attempts_made = 0;
+
+            startTimerIfNotRunning();
+        }
+        entry.uptime_sec = msg.uptime_sec;
+        entry.updated_since_last_attempt = true;
+
+        listeners_.forEach(GenericHandlerCaller<const ReceivedDataStructure<protocol::NodeStatus>&>(
+            &INodeInfoListener::handleNodeStatusMessage, msg));
+    }
+
+    void handleGetNodeInfoResponse(const ServiceCallResult<protocol::GetNodeInfo>& result)
+    {
+        Entry& entry = getEntry(result.getCallID().server_node_id);
+
+        if (result.isSuccessful())
+        {
+            /*
+             * Updating the uptime here allows to properly handle a corner case where the service response arrives
+             * after the device has restarted and published its new NodeStatus (although it's unlikely to happen).
+             */
+            entry.uptime_sec = result.getResponse().status.uptime_sec;
+            entry.request_needed = false;
+            listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id,
+                                                              result.getResponse()));
+        }
+        else
+        {
+            if (num_attempts_ != UnlimitedRequestAttempts)
+            {
+                entry.num_attempts_made++;
+                if (entry.num_attempts_made >= num_attempts_)
+                {
+                    entry.request_needed = false;
+                    listeners_.forEach(GenericHandlerCaller<NodeID>(&INodeInfoListener::handleNodeInfoUnavailable,
+                                                                    result.getCallID().server_node_id));
+                }
+            }
+        }
+    }
+
+public:
+    NodeInfoRetriever(INode& node)
+        : NodeStatusMonitor(node)
+        , TimerBase(node)
+        , listeners_(node.getAllocator())
+        , get_node_info_client_(node)
+        , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec))
+        , last_picked_node_(1)
+        , num_attempts_(DefaultNumRequestAttempts)
+    { }
+
+    /**
+     * Starts the retriever.
+     * Destroy the object to stop it.
+     * Returns negative error code.
+     */
+    int start()
+    {
+        int res = NodeStatusMonitor::start();
+        if (res < 0)
+        {
+            return res;
+        }
+
+        res = get_node_info_client_.init();
+        if (res < 0)
+        {
+            return res;
+        }
+        get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this,
+                                                                      &NodeInfoRetriever::handleGetNodeInfoResponse));
+        // Note: the timer will be started ad-hoc
+        return 0;
+    }
+
+    /**
+     * Adds one listener. Does nothing if such listener already exists.
+     * May return -ErrMemory if there's no space to add the listener.
+     */
+    int addListener(INodeInfoListener* listener)
+    {
+        if (listener != NULL)
+        {
+            removeListener(listener);
+            return (NULL == listeners_.emplace(listener)) ? -ErrMemory : 0;
+        }
+        else
+        {
+            return -ErrInvalidParam;
+        }
+    }
+
+    /**
+     * Removes the listener.
+     * If the listener was not registered, nothing will be done.
+     */
+    void removeListener(INodeInfoListener* listener)
+    {
+        if (listener != NULL)
+        {
+            listeners_.removeAll(listener);
+        }
+        else
+        {
+            UAVCAN_ASSERT(0);
+        }
+    }
+
+    unsigned getNumListeners() const { return listeners_.getSize(); }
+
+    /**
+     * Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is
+     * not implemented.
+     * Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts.
+     */
+    uint8_t getNumRequestAttempts() const { return num_attempts_; }
+    void setNumRequestAttempts(const uint8_t num)
+    {
+        num_attempts_ = min(static_cast<uint8_t>(MaxNumRequestAttempts), num);
+    }
+
+    /**
+     * Request interval also implicitly defines the maximum number of concurrent requests.
+     * Read the class documentation for details.
+     */
+    MonotonicDuration getRequestInterval() const { return request_interval_; }
+    void setRequestInterval(const MonotonicDuration interval)
+    {
+        if (interval.isPositive())
+        {
+            request_interval_ = interval;
+            if (TimerBase::isRunning())
+            {
+                TimerBase::startPeriodic(request_interval_);
+            }
+        }
+        else
+        {
+            UAVCAN_ASSERT(0);
+        }
+    }
+
+    /**
+     * These methods are needed mostly for testing.
+     */
+    bool isRetrievingInProgress() const { return TimerBase::isRunning(); }
+
+    uint8_t getNumPendingRequests() const
+    {
+        const unsigned num = get_node_info_client_.getNumPendingCalls();
+        UAVCAN_ASSERT(num <= 0xFF);
+        return static_cast<uint8_t>(num);
+    }
+};
+
+}
+
+#endif // Include guard

libuavcan/include/uavcan/protocol/node_status_monitor.hpp

@@ -19,7 +19,7 @@ namespace uavcan
  * This class implements the core functionality of a network monitor.
  * It can be extended by inheritance to add more complex logic, or used directly as is.
  */
-class UAVCAN_EXPORT NodeStatusMonitor : protected TimerBase
+class UAVCAN_EXPORT NodeStatusMonitor
 {
 public:
     typedef typename StorageType<typename protocol::NodeStatus::FieldTypes::status_code>::Type NodeStatusCode;
@@ -43,12 +43,14 @@ public:
     };
 
 private:
-    enum { TimerPeriodMs100 = 5 };
+    enum { TimerPeriodMs100 = 4 };
 
     typedef MethodBinder<NodeStatusMonitor*,
                          void (NodeStatusMonitor::*)(const ReceivedDataStructure<protocol::NodeStatus>&)>
             NodeStatusCallback;
 
+    typedef MethodBinder<NodeStatusMonitor*, void (NodeStatusMonitor::*)(const TimerEvent&)> TimerCallback;
+
     /*
      * We'll be able to handle this many nodes in the network without any dynamic memory.
      */
@@ -56,6 +58,8 @@ private:
 
     Subscriber<protocol::NodeStatus, NodeStatusCallback, NumStaticReceivers, 0> sub_;
 
+    TimerEventForwarder<TimerCallback> timer_;
+
     struct Entry
     {
         NodeStatusCode status_code;
@@ -110,11 +114,7 @@ private:
         handleNodeStatusMessage(msg);
     }
 
-protected:
-    /**
-     * This event will be invoked at 2 Hz rate. It can be used by derived classes as well.
-     */
-    virtual void handleTimerEvent(const TimerEvent&)
+    void handleTimerEvent(const TimerEvent&)
     {
         const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100;
 
@@ -126,7 +126,8 @@ protected:
             if (entry.time_since_last_update_ms100 >= 0 &&
                 entry.status_code != protocol::NodeStatus::STATUS_OFFLINE)
             {
-                entry.time_since_last_update_ms100 = int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
+                entry.time_since_last_update_ms100 =
+                    int8_t(entry.time_since_last_update_ms100 + int8_t(TimerPeriodMs100));
                 if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100)
                 {
                     Entry new_entry_value;
@@ -138,6 +139,7 @@ protected:
         }
     }
 
+protected:
     /**
      * Called when a node becomes online, changes status or goes offline.
      * Refer to uavcan.protocol.NodeStatus for the offline timeout value.
@@ -160,8 +162,8 @@ protected:
 
 public:
     explicit NodeStatusMonitor(INode& node)
-        : TimerBase(node)
-        , sub_(node)
+        : sub_(node)
+        , timer_(node)
     { }
 
     virtual ~NodeStatusMonitor() { }
@@ -176,7 +178,8 @@ public:
         const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus));
         if (res >= 0)
         {
-            TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
+            timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent));
+            timer_.startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100));
         }
         return res;
     }

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/include/uavcan/transport/outgoing_transfer_registry.hpp

@@ -165,13 +165,13 @@ TransferID* OutgoingTransferRegistry<NumStaticEntries>::accessOrCreate(const Out
 template <int NumStaticEntries>
 bool OutgoingTransferRegistry<NumStaticEntries>::exists(DataTypeID dtid, TransferType tt) const
 {
-    return NULL != map_.findFirstKey(ExistenceCheckingPredicate(dtid, tt));
+    return NULL != map_.find(ExistenceCheckingPredicate(dtid, tt));
 }
 
 template <int NumStaticEntries>
 void OutgoingTransferRegistry<NumStaticEntries>::cleanup(MonotonicTime ts)
 {
-    map_.removeWhere(DeadlineExpiredPredicate(ts));
+    map_.removeAllWhere(DeadlineExpiredPredicate(ts));
 }
 
 }

libuavcan/include/uavcan/transport/transfer_listener.hpp

@@ -178,88 +178,69 @@ public:
 
     virtual ~TransferListener()
     {
-        // Map must be cleared before bufmgr is destructed
-        receivers_.removeAll();
+        // Map must be cleared before bufmgr is destroyed
+        receivers_.clear();
     }
 };
 
+/**
+ * This class is used by transfer listener to decide if the frame should be accepted or ignored.
+ */
+class ITransferAcceptanceFilter
+{
+public:
+    /**
+     * If it returns false, the frame will be ignored, otherwise accepted.
+     */
+    virtual bool shouldAcceptFrame(const RxFrame& frame) const = 0;
+
+    virtual ~ITransferAcceptanceFilter() { }
+};
+
 /**
  * This class should be derived by callers.
  */
 template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
-class UAVCAN_EXPORT ServiceResponseTransferListener
-    : public TransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>
+class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>
 {
+    const ITransferAcceptanceFilter* filter_;
+
+    virtual void handleFrame(const RxFrame& frame);
+
 public:
     typedef TransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers> BaseType;
 
-    struct ExpectedResponseParams
-    {
-        NodeID src_node_id;
-        TransferID transfer_id;
-
-        ExpectedResponseParams()
-        {
-            UAVCAN_ASSERT(!src_node_id.isValid());
-        }
-
-        ExpectedResponseParams(NodeID arg_src_node_id, TransferID arg_transfer_id)
-            : src_node_id(arg_src_node_id)
-            , transfer_id(arg_transfer_id)
-        {
-            UAVCAN_ASSERT(src_node_id.isUnicast());
-        }
-
-        bool match(const RxFrame& frame) const
-        {
-            UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse);
-            return (frame.getSrcNodeID() == src_node_id) && (frame.getTransferID() == transfer_id);
-        }
-    };
-
-private:
-    ExpectedResponseParams response_params_;
-
-    void handleFrame(const RxFrame& frame);
-
-public:
-    ServiceResponseTransferListener(TransferPerfCounter& perf, const DataTypeDescriptor& data_type,
-                                    IPoolAllocator& allocator)
+    TransferListenerWithFilter(TransferPerfCounter& perf, const DataTypeDescriptor& data_type,
+                               IPoolAllocator& allocator)
         : BaseType(perf, data_type, allocator)
+        , filter_(NULL)
     { }
 
-    void setExpectedResponseParams(const ExpectedResponseParams& erp);
-
-    const ExpectedResponseParams& getExpectedResponseParams() const { return response_params_; }
-
-    void stopAcceptingAnything();
+    void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter)
+    {
+        filter_ = acceptance_filter;
+    }
 };
 
 // ----------------------------------------------------------------------------
 
 /*
- * ServiceResponseTransferListener<>
+ * TransferListenerWithFilter<>
  */
 template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
-void ServiceResponseTransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>::handleFrame(const RxFrame& frame)
+void TransferListenerWithFilter<MaxBufSize, NumStaticBufs, NumStaticReceivers>::handleFrame(const RxFrame& frame)
 {
-    if (response_params_.match(frame))
+    if (filter_ != NULL)
     {
-        BaseType::handleFrame(frame);
+        if (filter_->shouldAcceptFrame(frame))
+        {
+            BaseType::handleFrame(frame);
+        }
+    }
+    else
+    {
+        UAVCAN_ASSERT(0);
     }
-}
-
-template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
-void ServiceResponseTransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>::setExpectedResponseParams(
-    const ExpectedResponseParams& erp)
-{
-    response_params_ = erp;
-}
-
-template <unsigned MaxBufSize, unsigned NumStaticBufs, unsigned NumStaticReceivers>
-void ServiceResponseTransferListener<MaxBufSize, NumStaticBufs, NumStaticReceivers>::stopAcceptingAnything()
-{
-    response_params_ = ExpectedResponseParams();
 }
 
 }

libuavcan/include/uavcan/util/lazy_constructor.hpp

@@ -26,12 +26,19 @@ namespace uavcan
 template <typename T>
 class UAVCAN_EXPORT LazyConstructor
 {
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+    struct
+    {
+        alignas(T) unsigned char pool[sizeof(T)];
+    } data_;
+#else
     union
     {
         unsigned char pool[sizeof(T)];
         long double _aligner1;
         long long _aligner2;
     } data_;
+#endif
 
     T* ptr_;
 
@@ -51,12 +58,6 @@ class UAVCAN_EXPORT LazyConstructor
         }
     }
 
-    template <typename U> struct ParamType { typedef const U& Type; };
-    template <typename U> struct ParamType<U&> { typedef U& Type; };
-#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
-    template <typename U> struct ParamType<U&&> { typedef U&& Type; };
-#endif
-
 public:
     LazyConstructor()
         : ptr_(NULL)
@@ -123,50 +124,53 @@ public:
 // for nargs in range(1, MAX_ARGS + 1):
 //     nums = [(x + 1) for x in range(nargs)]
 //     l1 = ['typename P%d' % x for x in nums]
-//     l2 = ['typename ParamType<P%d>::Type p%d' % (x, x) for x in nums]
+//     l2 = ['typename ParameterType<P%d>::Type p%d' % (x, x) for x in nums]
 //     l3 = ['p%d' % x for x in nums]
 //     print(TEMPLATE % (', '.join(l1), ', '.join(l2), ', '.join(l3)))
 
     template <typename P1>
-    void construct(typename ParamType<P1>::Type p1)
+    void construct(typename ParameterType<P1>::Type p1)
     {
         ensureNotConstructed();
         ptr_ = new (static_cast<void*>(data_.pool)) T(p1);
     }
 
-    template <typename P1, typename P2>
-    void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2)
+    template<typename P1, typename P2>
+    void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2)
     {
         ensureNotConstructed();
         ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2);
     }
 
-    template <typename P1, typename P2, typename P3>
-    void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3)
+    template<typename P1, typename P2, typename P3>
+    void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+                   typename ParameterType<P3>::Type p3)
     {
         ensureNotConstructed();
         ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3);
     }
 
-    template <typename P1, typename P2, typename P3, typename P4>
-    void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3,
-                   typename ParamType<P4>::Type p4)
+    template<typename P1, typename P2, typename P3, typename P4>
+    void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+                   typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4)
     {
         ensureNotConstructed();
         ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4);
     }
 
-    template <typename P1, typename P2, typename P3, typename P4, typename P5>
-    void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3,
-                   typename ParamType<P4>::Type p4, typename ParamType<P5>::Type p5)
+    template<typename P1, typename P2, typename P3, typename P4, typename P5>
+    void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+                   typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4,
+                   typename ParameterType<P5>::Type p5)
     {
         ensureNotConstructed();
         ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4, p5);
     }
 
-    template <typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
-    void construct(typename ParamType<P1>::Type p1, typename ParamType<P2>::Type p2, typename ParamType<P3>::Type p3,
-                   typename ParamType<P4>::Type p4, typename ParamType<P5>::Type p5, typename ParamType<P6>::Type p6)
+    template<typename P1, typename P2, typename P3, typename P4, typename P5, typename P6>
+    void construct(typename ParameterType<P1>::Type p1, typename ParameterType<P2>::Type p2,
+                   typename ParameterType<P3>::Type p3, typename ParameterType<P4>::Type p4,
+                   typename ParameterType<P5>::Type p5, typename ParameterType<P6>::Type p6)
     {
         ensureNotConstructed();
         ptr_ = new (static_cast<void*>(data_.pool)) T(p1, p2, p3, p4, p5, p6);

libuavcan/include/uavcan/util/map.hpp

@@ -11,6 +11,7 @@
 #include <uavcan/build_config.hpp>
 #include <uavcan/dynamic_memory.hpp>
 #include <uavcan/util/templates.hpp>
+#include <uavcan/util/placement_new.hpp>
 
 namespace uavcan
 {
@@ -107,7 +108,7 @@ private:
     const unsigned num_static_entries_;
 #endif
 
-    KVPair* find(const Key& key);
+    KVPair* findKey(const Key& key);
 
 #if !UAVCAN_TINY
     void optimizeStorage();
@@ -116,7 +117,7 @@ private:
 
     struct YesPredicate
     {
-        bool operator()(const Key& k, const Value& v) const { (void)k; (void)v; return true; }
+        bool operator()(const Key&, const Value&) const { return true; }
     };
 
 protected:
@@ -136,8 +137,11 @@ protected:
     }
 #endif
 
-    /// Derived class destructor must call removeAll();
-    ~MapBase() { }
+    /// Derived class destructor must call clear();
+    ~MapBase()
+    {
+        UAVCAN_ASSERT(getSize() == 0);
+    }
 
 public:
     /**
@@ -158,10 +162,10 @@ public:
     /**
      * Removes entries where the predicate returns true.
      * Predicate prototype:
-     *  bool (const Key& key, const Value& value)
+     *  bool (Key& key, Value& value)
      */
     template <typename Predicate>
-    void removeWhere(Predicate predicate);
+    void removeAllWhere(Predicate predicate);
 
     /**
      * Returns first entry where the predicate returns true.
@@ -169,9 +173,12 @@ public:
      *  bool (const Key& key, const Value& value)
      */
     template <typename Predicate>
-    const Key* findFirstKey(Predicate predicate) const;
+    const Key* find(Predicate predicate) const;
 
-    void removeAll();
+    /**
+     * Removes all items.
+     */
+    void clear();
 
     /**
      * Returns a key-value pair located at the specified position from the beginning.
@@ -181,7 +188,10 @@ public:
     KVPair* getByIndex(unsigned index);
     const KVPair* getByIndex(unsigned index) const;
 
-    bool isEmpty() const;
+    /**
+     * Complexity is O(1).
+     */
+    bool isEmpty() const { return find(YesPredicate()) == NULL; }
 
     unsigned getSize() const;
 
@@ -207,7 +217,7 @@ public:
         : MapBase<Key, Value>(static_, NumStaticEntries, allocator)
     { }
 
-    ~Map() { this->removeAll(); }
+    ~Map() { this->clear(); }
 
 #endif // !UAVCAN_TINY
 };
@@ -225,7 +235,7 @@ public:
 #endif
     { }
 
-    ~Map() { this->removeAll(); }
+    ~Map() { this->clear(); }
 };
 
 // ----------------------------------------------------------------------------
@@ -234,7 +244,7 @@ public:
  * MapBase<>
  */
 template <typename Key, typename Value>
-typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::find(const Key& key)
+typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::findKey(const Key& key)
 {
 #if !UAVCAN_TINY
     for (unsigned i = 0; i < num_static_entries_; i++)
@@ -344,7 +354,7 @@ template <typename Key, typename Value>
 Value* MapBase<Key, Value>::access(const Key& key)
 {
     UAVCAN_ASSERT(!(key == Key()));
-    KVPair* const kv = find(key);
+    KVPair* const kv = findKey(key);
     return kv ? &kv->value : NULL;
 }
 
@@ -354,7 +364,7 @@ Value* MapBase<Key, Value>::insert(const Key& key, const Value& value)
     UAVCAN_ASSERT(!(key == Key()));
     remove(key);
 
-    KVPair* const kv = find(Key());
+    KVPair* const kv = findKey(Key());
     if (kv)
     {
         *kv = KVPair(key, value);
@@ -375,7 +385,7 @@ template <typename Key, typename Value>
 void MapBase<Key, Value>::remove(const Key& key)
 {
     UAVCAN_ASSERT(!(key == Key()));
-    KVPair* const kv = find(key);
+    KVPair* const kv = findKey(key);
     if (kv)
     {
         *kv = KVPair();
@@ -388,7 +398,7 @@ void MapBase<Key, Value>::remove(const Key& key)
 
 template <typename Key, typename Value>
 template <typename Predicate>
-void MapBase<Key, Value>::removeWhere(Predicate predicate)
+void MapBase<Key, Value>::removeAllWhere(Predicate predicate)
 {
     unsigned num_removed = 0;
 
@@ -407,8 +417,10 @@ void MapBase<Key, Value>::removeWhere(Predicate predicate)
 #endif
 
     KVGroup* p = list_.get();
-    while (p)
+    while (p != NULL)
     {
+        KVGroup* const next_group = p->getNextListNode();
+
         for (int i = 0; i < KVGroup::NumKV; i++)
         {
             const KVPair* const kv = p->kvs + i;
@@ -421,7 +433,8 @@ void MapBase<Key, Value>::removeWhere(Predicate predicate)
                 }
             }
         }
-        p = p->getNextListNode();
+
+        p = next_group;
     }
 
     if (num_removed > 0)
@@ -435,7 +448,7 @@ void MapBase<Key, Value>::removeWhere(Predicate predicate)
 
 template <typename Key, typename Value>
 template <typename Predicate>
-const Key* MapBase<Key, Value>::findFirstKey(Predicate predicate) const
+const Key* MapBase<Key, Value>::find(Predicate predicate) const
 {
 #if !UAVCAN_TINY
     for (unsigned i = 0; i < num_static_entries_; i++)
@@ -451,8 +464,10 @@ const Key* MapBase<Key, Value>::findFirstKey(Predicate predicate) const
 #endif
 
     KVGroup* p = list_.get();
-    while (p)
+    while (p != NULL)
     {
+        KVGroup* const next_group = p->getNextListNode();
+
         for (int i = 0; i < KVGroup::NumKV; i++)
         {
             const KVPair* const kv = p->kvs + i;
@@ -464,15 +479,16 @@ const Key* MapBase<Key, Value>::findFirstKey(Predicate predicate) const
                 }
             }
         }
-        p = p->getNextListNode();
+
+        p = next_group;
     }
     return NULL;
 }
 
 template <typename Key, typename Value>
-void MapBase<Key, Value>::removeAll()
+void MapBase<Key, Value>::clear()
 {
-    removeWhere(YesPredicate());
+    removeAllWhere(YesPredicate());
 }
 
 template <typename Key, typename Value>
@@ -495,8 +511,10 @@ typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::getByIndex(unsigned i
 
     // Slowly crawling through the dynamic storage
     KVGroup* p = list_.get();
-    while (p)
+    while (p != NULL)
     {
+        KVGroup* const next_group = p->getNextListNode();
+
         for (int i = 0; i < KVGroup::NumKV; i++)
         {
             KVPair* const kv = p->kvs + i;
@@ -509,7 +527,8 @@ typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::getByIndex(unsigned i
                 index--;
             }
         }
-        p = p->getNextListNode();
+
+        p = next_group;
     }
 
     return NULL;
@@ -521,12 +540,6 @@ const typename MapBase<Key, Value>::KVPair* MapBase<Key, Value>::getByIndex(unsi
     return const_cast<MapBase<Key, Value>*>(this)->getByIndex(index);
 }
 
-template <typename Key, typename Value>
-bool MapBase<Key, Value>::isEmpty() const
-{
-    return getSize() == 0;
-}
-
 template <typename Key, typename Value>
 unsigned MapBase<Key, Value>::getSize() const
 {

libuavcan/include/uavcan/util/multiset.hpp

@@ -0,0 +1,542 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED
+#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/util/linked_list.hpp>
+#include <uavcan/build_config.hpp>
+#include <uavcan/dynamic_memory.hpp>
+#include <uavcan/util/templates.hpp>
+#include <uavcan/util/placement_new.hpp>
+
+#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11)
+# error UAVCAN_CPP_VERSION
+#endif
+
+namespace uavcan
+{
+/**
+ * Slow but memory efficient unordered multiset. Unlike Map<>, this container does not move objects, so
+ * they don't have to be copyable.
+ *
+ * Items can be allocated in a static buffer or in the node's memory pool if the static buffer is exhausted.
+ *
+ * Number of static entries must not be less than 1.
+ */
+template <typename T, unsigned NumStaticEntries>
+class UAVCAN_EXPORT Multiset : Noncopyable
+{
+    struct Item : ::uavcan::Noncopyable
+    {
+        T* ptr;
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+        alignas(T) unsigned char pool[sizeof(T)];       ///< Memory efficient version
+#else
+        union
+        {
+            unsigned char pool[sizeof(T)];
+            /*
+             * Such alignment does not guarantee safety for all types (only for libuavcan internal ones);
+             * however, increasing it is too memory inefficient. So it is recommended to use C++11, where
+             * this issue is resolved with alignas() (see above).
+             */
+            void* _aligner1_;
+            long long _aligner2_;
+        };
+#endif
+
+        Item()
+            : ptr(NULL)
+        {
+            fill_n(pool, sizeof(pool), static_cast<unsigned char>(0));
+        }
+
+        ~Item() { destroy(); }
+
+        bool isConstructed() const { return ptr != NULL; }
+
+        void destroy()
+        {
+            if (ptr != NULL)
+            {
+                ptr->~T();
+                ptr = NULL;
+                fill_n(pool, sizeof(pool), static_cast<unsigned char>(0));
+            }
+        }
+    };
+
+private:
+    struct Chunk : LinkedListNode<Chunk>, ::uavcan::Noncopyable
+    {
+        enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode<Chunk>)) / sizeof(Item) };
+        Item items[NumItems];
+
+        Chunk()
+        {
+            StaticAssert<(static_cast<unsigned>(NumItems) > 0)>::check();
+            IsDynamicallyAllocatable<Chunk>::check();
+            UAVCAN_ASSERT(!items[0].isConstructed());
+        }
+
+        static Chunk* instantiate(IPoolAllocator& allocator)
+        {
+            void* const praw = allocator.allocate(sizeof(Chunk));
+            if (praw == NULL)
+            {
+                return NULL;
+            }
+            return new (praw) Chunk();
+        }
+
+        static void destroy(Chunk*& obj, IPoolAllocator& allocator)
+        {
+            if (obj != NULL)
+            {
+                obj->~Chunk();
+                allocator.deallocate(obj);
+                obj = NULL;
+            }
+        }
+
+        Item* findFreeSlot()
+        {
+            for (unsigned i = 0; i < static_cast<unsigned>(NumItems); i++)
+            {
+                if (!items[i].isConstructed())
+                {
+                    return items + i;
+                }
+            }
+            return NULL;
+        }
+    };
+
+    /*
+     * Data
+     */
+    LinkedListRoot<Chunk> list_;
+    IPoolAllocator& allocator_;
+    Item static_[NumStaticEntries];
+
+    /*
+     * Methods
+     */
+    Item* findOrCreateFreeSlot();
+
+    void compact();
+
+    enum RemoveStrategy { RemoveOne, RemoveAll };
+
+    template <typename Predicate>
+    void removeWhere(Predicate predicate, RemoveStrategy strategy);
+
+    struct YesPredicate
+    {
+        bool operator()(const T&) const { return true; }
+    };
+
+    struct IndexPredicate : ::uavcan::Noncopyable
+    {
+        unsigned index;
+        IndexPredicate(unsigned target_index)
+            : index(target_index)
+        { }
+
+        bool operator()(const T&)
+        {
+            return index-- == 0;
+        }
+    };
+
+    struct ComparingPredicate
+    {
+        const T& reference;
+
+        ComparingPredicate(const T& ref)
+            : reference(ref)
+        { }
+
+        bool operator()(const T& sample)
+        {
+            return reference == sample;
+        }
+    };
+
+    template<typename Operator>
+    struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable
+    {
+        Operator oper;
+
+        OperatorToFalsePredicateAdapter(Operator o)
+            : oper(o)
+        { }
+
+        bool operator()(T& item)
+        {
+            oper(item);
+            return false;
+        }
+
+        bool operator()(const T& item) const
+        {
+            oper(item);
+            return false;
+        }
+    };
+
+public:
+    Multiset(IPoolAllocator& allocator)
+        : allocator_(allocator)
+    { }
+
+    ~Multiset()
+    {
+        clear();
+    }
+
+    /**
+     * Creates one item in-place and returns a pointer to it.
+     * If creation fails due to lack of memory, NULL will be returned.
+     * Complexity is O(N).
+     */
+    T* emplace()
+    {
+        Item* const item = findOrCreateFreeSlot();
+        if (item == NULL)
+        {
+            return NULL;
+        }
+        UAVCAN_ASSERT(item->ptr == NULL);
+        item->ptr = new (item->pool) T();
+        return item->ptr;
+    }
+
+    template <typename P1>
+    T* emplace(P1 p1)
+    {
+        Item* const item = findOrCreateFreeSlot();
+        if (item == NULL)
+        {
+            return NULL;
+        }
+        UAVCAN_ASSERT(item->ptr == NULL);
+        item->ptr = new (item->pool) T(p1);
+        return item->ptr;
+    }
+
+    template <typename P1, typename P2>
+    T* emplace(P1 p1, P2 p2)
+    {
+        Item* const item = findOrCreateFreeSlot();
+        if (item == NULL)
+        {
+            return NULL;
+        }
+        UAVCAN_ASSERT(item->ptr == NULL);
+        item->ptr = new (item->pool) T(p1, p2);
+        return item->ptr;
+    }
+
+    template <typename P1, typename P2, typename P3>
+    T* emplace(P1 p1, P2 p2, P3 p3)
+    {
+        Item* const item = findOrCreateFreeSlot();
+        if (item == NULL)
+        {
+            return NULL;
+        }
+        UAVCAN_ASSERT(item->ptr == NULL);
+        item->ptr = new (item->pool) T(p1, p2, p3);
+        return item->ptr;
+    }
+
+    /**
+     * Removes entries where the predicate returns true.
+     * Predicate prototype:
+     *  bool (T& item)
+     */
+    template <typename Predicate>
+    void removeAllWhere(Predicate predicate) { removeWhere<Predicate>(predicate, RemoveAll); }
+
+    template <typename Predicate>
+    void removeFirstWhere(Predicate predicate) { removeWhere<Predicate>(predicate, RemoveOne); }
+
+    void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); }
+
+    void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); }
+
+    void clear() { removeAllWhere(YesPredicate()); }
+
+    /**
+     * Returns first entry where the predicate returns true.
+     * Predicate prototype:
+     *  bool (const T& item)
+     */
+    template <typename Predicate>
+    T* find(Predicate predicate);
+
+    template <typename Predicate>
+    const T* find(Predicate predicate) const
+    {
+        return const_cast<Multiset*>(this)->find<Predicate>(predicate);
+    }
+
+    /**
+     * Calls Operator for each item of the set.
+     * Operator prototype:
+     *  void (T& item)
+     *  void (const T& item) - const overload
+     */
+    template <typename Operator>
+    void forEach(Operator oper)
+    {
+        OperatorToFalsePredicateAdapter<Operator> adapter(oper);
+        (void)find<OperatorToFalsePredicateAdapter<Operator>&>(adapter);
+    }
+
+    template <typename Operator>
+    void forEach(Operator oper) const
+    {
+        const OperatorToFalsePredicateAdapter<Operator> adapter(oper);
+        (void)find<const OperatorToFalsePredicateAdapter<Operator>&>(adapter);
+    }
+
+    /**
+     * Returns an item located at the specified position from the beginning.
+     * Note that addition and removal operations invalidate indices.
+     * If index is greater than or equal the number of items, null pointer will be returned.
+     * Complexity is O(N).
+     */
+    T* getByIndex(unsigned index)
+    {
+        IndexPredicate predicate(index);
+        return find<IndexPredicate&>(predicate);
+    }
+
+    const T* getByIndex(unsigned index) const
+    {
+        return const_cast<Multiset*>(this)->getByIndex(index);
+    }
+
+    /**
+     * Complexity is O(1).
+     */
+    bool isEmpty() const { return find(YesPredicate()) == NULL; }
+
+    /**
+     * Counts number of items stored.
+     * Best case complexity is O(N).
+     */
+    unsigned getSize() const { return getNumStaticItems() + getNumDynamicItems(); }
+
+    /**
+     * For testing, do not use directly.
+     */
+    unsigned getNumStaticItems() const;
+    unsigned getNumDynamicItems() const;
+};
+
+// ----------------------------------------------------------------------------
+
+/*
+ * Multiset<>
+ */
+template <typename T, unsigned NumStaticEntries>
+typename Multiset<T, NumStaticEntries>::Item* Multiset<T, NumStaticEntries>::findOrCreateFreeSlot()
+{
+#if !UAVCAN_TINY
+    // Search in static pool
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        if (!static_[i].isConstructed())
+        {
+            return &static_[i];
+        }
+    }
+#endif
+
+    // Search in dynamic pool
+    {
+        Chunk* p = list_.get();
+        while (p)
+        {
+            Item* const dyn = p->findFreeSlot();
+            if (dyn != NULL)
+            {
+                return dyn;
+            }
+            p = p->getNextListNode();
+        }
+    }
+
+    // Create new dynamic chunk
+    Chunk* const chunk = Chunk::instantiate(allocator_);
+    if (chunk == NULL)
+    {
+        return NULL;
+    }
+    list_.insert(chunk);
+    return &chunk->items[0];
+}
+
+template <typename T, unsigned NumStaticEntries>
+void Multiset<T, NumStaticEntries>::compact()
+{
+    Chunk* p = list_.get();
+    while (p)
+    {
+        Chunk* const next = p->getNextListNode();
+        bool remove_this = true;
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            if (p->items[i].isConstructed())
+            {
+                remove_this = false;
+                break;
+            }
+        }
+        if (remove_this)
+        {
+            list_.remove(p);
+            Chunk::destroy(p, allocator_);
+        }
+        p = next;
+    }
+}
+
+template <typename T, unsigned NumStaticEntries>
+template <typename Predicate>
+void Multiset<T, NumStaticEntries>::removeWhere(Predicate predicate, const RemoveStrategy strategy)
+{
+    unsigned num_removed = 0;
+
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        if (static_[i].isConstructed())
+        {
+            if (predicate(*static_[i].ptr))
+            {
+                num_removed++;
+                static_[i].destroy();
+                if (strategy == RemoveOne)
+                {
+                    break;
+                }
+            }
+        }
+    }
+#endif
+
+    Chunk* p = list_.get();
+    while (p != NULL)
+    {
+        Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified
+
+        if ((num_removed > 0) && (strategy == RemoveOne))
+        {
+            break;
+        }
+
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            Item& item = p->items[i];
+            if (item.isConstructed())
+            {
+                if (predicate(*item.ptr))
+                {
+                    num_removed++;
+                    item.destroy();
+                    if (strategy == RemoveOne)
+                    {
+                        break;
+                    }
+                }
+            }
+        }
+
+        p = next_chunk;
+    }
+
+    if (num_removed > 0)
+    {
+        compact();
+    }
+}
+
+template <typename T, unsigned NumStaticEntries>
+template <typename Predicate>
+T* Multiset<T, NumStaticEntries>::find(Predicate predicate)
+{
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        if (static_[i].isConstructed())
+        {
+            if (predicate(*static_[i].ptr))
+            {
+                return static_[i].ptr;
+            }
+        }
+    }
+#endif
+
+    Chunk* p = list_.get();
+    while (p != NULL)
+    {
+        Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified
+
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            if (p->items[i].isConstructed())
+            {
+                if (predicate(*p->items[i].ptr))
+                {
+                    return p->items[i].ptr;
+                }
+            }
+        }
+
+        p = next_chunk;
+    }
+    return NULL;
+}
+
+template <typename T, unsigned NumStaticEntries>
+unsigned Multiset<T, NumStaticEntries>::getNumStaticItems() const
+{
+    unsigned num = 0;
+#if !UAVCAN_TINY
+    for (unsigned i = 0; i < NumStaticEntries; i++)
+    {
+        num += static_[i].isConstructed() ? 1U : 0U;
+    }
+#endif
+    return num;
+}
+
+template <typename T, unsigned NumStaticEntries>
+unsigned Multiset<T, NumStaticEntries>::getNumDynamicItems() const
+{
+    unsigned num = 0;
+    Chunk* p = list_.get();
+    while (p)
+    {
+        for (int i = 0; i < Chunk::NumItems; i++)
+        {
+            num += p->items[i].isConstructed() ? 1U : 0U;
+        }
+        p = p->getNextListNode();
+    }
+    return num;
+}
+
+}
+
+#endif // Include guard

libuavcan/include/uavcan/util/templates.hpp

@@ -100,6 +100,15 @@ template <typename T> struct RemoveReference<T&>  { typedef T Type; };
 template <typename T> struct RemoveReference<T&&> { typedef T Type; };
 #endif
 
+/**
+ * Parameter types
+ */
+template <typename U> struct ParameterType { typedef const U& Type; };
+template <typename U> struct ParameterType<U&> { typedef U& Type; };
+#if UAVCAN_CPP_VERSION > UAVCAN_CPP03
+template <typename U> struct ParameterType<U&&> { typedef U&& Type; };
+#endif
+
 /**
  * Value types
  */

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/node/uc_service_client.cpp

@@ -6,12 +6,33 @@
 
 namespace uavcan
 {
-
-int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID server_node_id,
-                                     TransferID& out_transfer_id)
+/*
+ * ServiceClientBase::CallState
+ */
+void ServiceClientBase::CallState::handleDeadline(MonotonicTime)
 {
-    pending_ = true;
+    UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s",
+                 int(id_.server_node_id.get()), int(id_.transfer_id.get()),
+                 (owner_.data_type_descriptor_ == NULL) ? "???" : owner_.data_type_descriptor_->getFullName());
+    /*
+     * What we're doing here is relaying execution from this call stack to a different one.
+     * We need it because call registry cannot release memory from this callback, because this will destroy the
+     * object method of which we're executing now.
+     */
+    UAVCAN_ASSERT(timed_out_ == false);
+    timed_out_ = true;
+    owner_.generateDeadlineImmediately();
+    UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback");
+}
 
+/*
+ * ServiceClientBase
+ */
+int ServiceClientBase::prepareToCall(INode& node,
+                                     const char* dtname,
+                                     NodeID server_node_id,
+                                     ServiceCallID& out_call_id)
+{
     /*
      * Making sure we're not going to get transport error because of invalid input data
      */
@@ -20,6 +41,7 @@ int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID ser
         UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID");
         return -ErrInvalidParam;
     }
+    out_call_id.server_node_id = server_node_id;
 
     /*
      * Determining the Data Type ID
@@ -50,13 +72,9 @@ int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID ser
         UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str());
         return -ErrMemory;
     }
-    out_transfer_id = *otr_tid;
+    out_call_id.transfer_id = *otr_tid;
     otr_tid->increment();
 
-    /*
-     * Registering the deadline handler
-     */
-    DeadlineHandler::startWithDelay(request_timeout_);
     return 0;
 }
 

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/src/transport/uc_transfer_listener.cpp

@@ -189,7 +189,7 @@ void TransferListenerBase::handleAnonymousTransferReception(const RxFrame& frame
 
 void TransferListenerBase::cleanup(MonotonicTime ts)
 {
-    receivers_.removeWhere(TimedOutReceiverPredicate(ts, bufmgr_));
+    receivers_.removeAllWhere(TimedOutReceiverPredicate(ts, bufmgr_));
     UAVCAN_ASSERT(receivers_.isEmpty() ? bufmgr_.isEmpty() : 1);
 }
 

libuavcan/test/node/service_client.cpp

@@ -12,6 +12,7 @@
 #include <root_ns_a/StringService.hpp>
 #include <root_ns_a/EmptyService.hpp>
 #include <queue>
+#include <sstream>
 #include "test_node.hpp"
 
 
@@ -22,21 +23,31 @@ struct ServiceCallResultHandler
     StatusType last_status;
     uavcan::NodeID last_server_node_id;
     typename DataType::Response last_response;
+    std::queue<typename DataType::Response> responses;
 
     void handleResponse(const uavcan::ServiceCallResult<DataType>& result)
     {
         std::cout << result << std::endl;
-        last_status = result.status;
-        last_response = result.response;
-        last_server_node_id = result.server_node_id;
+        last_status = result.getStatus();
+        last_response = result.getResponse();
+        last_server_node_id = result.getCallID().server_node_id;
+        responses.push(result.getResponse());
     }
 
     bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const
     {
-        return
-            status == last_status &&
+        if (status == last_status &&
             server_node_id == last_server_node_id &&
-            response == last_response;
+            response == last_response)
+        {
+            return true;
+        }
+        else
+        {
+            std::cout << "MISMATCH: status=" << last_status << ", last_server_node_id="
+                << int(last_server_node_id.get()) << ", last response:\n" << last_response << std::endl;
+            return false;
+        }
     }
 
     typedef uavcan::MethodBinder<ServiceCallResultHandler*,
@@ -94,6 +105,12 @@ TEST(ServiceClient, Basic)
         ClientType client2(nodes.b);
         ClientType client3(nodes.b);
 
+        ASSERT_EQ(0, client1.getNumPendingCalls());
+        ASSERT_EQ(0, client2.getNumPendingCalls());
+        ASSERT_EQ(0, client3.getNumPendingCalls());
+
+        ASSERT_FALSE(client1.hasPendingCallToServer(1));
+
         client1.setCallback(handler.bind());
         client2.setCallback(client1.getCallback());
         client3.setCallback(client1.getCallback());
@@ -105,23 +122,47 @@ TEST(ServiceClient, Basic)
         root_ns_a::StringService::Request request;
         request.string_request = "Hello world";
 
+        std::cout << "!!! Calling!" << std::endl;
+
         ASSERT_LT(0, client1.call(1, request)); // OK
+        ASSERT_LT(0, client1.call(1, request)); // OK - second request
         ASSERT_LT(0, client2.call(1, request)); // OK
         ASSERT_LT(0, client3.call(99, request)); // Will timeout!
+        ASSERT_LT(0, client3.call(1, request)); // OK - second request
+
+        ASSERT_TRUE(client1.hasPendingCallToServer(1));
+        ASSERT_TRUE(client2.hasPendingCallToServer(1));
+        ASSERT_TRUE(client3.hasPendingCallToServer(99));
+        ASSERT_TRUE(client3.hasPendingCallToServer(1));
+
+        std::cout << "!!! Spinning!" << std::endl;
 
         ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now!
 
-        ASSERT_TRUE(client1.isPending());
-        ASSERT_TRUE(client2.isPending());
-        ASSERT_TRUE(client3.isPending());
+        ASSERT_TRUE(client1.hasPendingCalls());
+        ASSERT_TRUE(client2.hasPendingCalls());
+        ASSERT_TRUE(client3.hasPendingCalls());
+
+        ASSERT_EQ(2, client1.getNumPendingCalls());
+        ASSERT_EQ(1, client2.getNumPendingCalls());
+        ASSERT_EQ(2, client3.getNumPendingCalls());
+
+        ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id);
+        ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id);
 
         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20));
 
+        std::cout << "!!! Spin finished!" << std::endl;
+
         ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening!
 
-        ASSERT_FALSE(client1.isPending());
-        ASSERT_FALSE(client2.isPending());
-        ASSERT_TRUE(client3.isPending());
+        ASSERT_FALSE(client1.hasPendingCalls());
+        ASSERT_FALSE(client2.hasPendingCalls());
+        ASSERT_TRUE(client3.hasPendingCalls());
+
+        ASSERT_EQ(0, client1.getNumPendingCalls());
+        ASSERT_EQ(0, client2.getNumPendingCalls());
+        ASSERT_EQ(1, client3.getNumPendingCalls());     // The one addressed to 99 is still waiting
 
         // Validating
         root_ns_a::StringService::Response expected_response;
@@ -130,18 +171,19 @@ TEST(ServiceClient, Basic)
 
         nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
 
-        ASSERT_FALSE(client1.isPending());
-        ASSERT_FALSE(client2.isPending());
-        ASSERT_FALSE(client3.isPending());
+        ASSERT_FALSE(client1.hasPendingCalls());
+        ASSERT_FALSE(client2.hasPendingCalls());
+        ASSERT_FALSE(client3.hasPendingCalls());
 
         ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :(
 
         // Validating
-        ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response()));
+        // We use expected response instead of empty response because the last valid will be reused on fauilure
+        ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, expected_response));
 
         // Stray request
         ASSERT_LT(0, client3.call(99, request)); // Will timeout!
-        ASSERT_TRUE(client3.isPending());
+        ASSERT_TRUE(client3.hasPendingCalls());
         ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());
     }
 
@@ -177,11 +219,16 @@ TEST(ServiceClient, Rejection)
 
     ASSERT_LT(0, client1.call(1, request));
 
+    ASSERT_EQ(uavcan::NodeID(1), client1.getCallIDByIndex(0).server_node_id);
+    ASSERT_EQ(uavcan::NodeID(), client1.getCallIDByIndex(1).server_node_id);
+
     ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());
-    ASSERT_TRUE(client1.isPending());
+    ASSERT_TRUE(client1.hasPendingCalls());
+    ASSERT_TRUE(client1.hasPendingCallToServer(1));
 
     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200));
-    ASSERT_FALSE(client1.isPending());
+    ASSERT_FALSE(client1.hasPendingCalls());
+    ASSERT_FALSE(client1.hasPendingCallToServer(1));
 
     ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out
 
@@ -189,6 +236,126 @@ TEST(ServiceClient, Rejection)
 }
 
 
+TEST(ServiceClient, ConcurrentCalls)
+{
+    InterlinkedTestNodesWithSysClock nodes;
+
+    // Type registration
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<root_ns_a::StringService> _registrator;
+
+    // Server
+    uavcan::ServiceServer<root_ns_a::StringService> server(nodes.a);
+    ASSERT_EQ(0, server.start(stringServiceServerCallback));
+
+    // Caller
+    typedef uavcan::ServiceCallResult<root_ns_a::StringService> ResultType;
+    typedef uavcan::ServiceClient<root_ns_a::StringService,
+                                  typename ServiceCallResultHandler<root_ns_a::StringService>::Binder > ClientType;
+    ServiceCallResultHandler<root_ns_a::StringService> handler;
+
+    /*
+     * Initializing
+     */
+    ClientType client(nodes.b);
+
+    ASSERT_EQ(0, client.getNumPendingCalls());
+
+    client.setCallback(handler.bind());
+
+    ASSERT_EQ(1, nodes.a.getDispatcher().getNumServiceRequestListeners());
+    ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // NOT listening!
+
+    ASSERT_FALSE(client.hasPendingCalls());
+    ASSERT_EQ(0, client.getNumPendingCalls());
+
+    /*
+     * Calling ten requests, the last one will be cancelled
+     * Note that there will be non-unique transfer ID values; the client must handle that correctly
+     */
+    uavcan::ServiceCallID last_call_id;
+    for (int i = 0; i < 10; i++)
+    {
+        std::ostringstream os;
+        os << i;
+        root_ns_a::StringService::Request request;
+        request.string_request = os.str().c_str();
+        ASSERT_LT(0, client.call(1, request, last_call_id));
+    }
+
+    ASSERT_LT(0, client.call(99, root_ns_a::StringService::Request()));         // Will timeout in 500 ms
+
+    client.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100));
+
+    ASSERT_LT(0, client.call(88, root_ns_a::StringService::Request()));         // Will timeout in 100 ms
+
+    ASSERT_TRUE(client.hasPendingCalls());
+    ASSERT_EQ(12, client.getNumPendingCalls());
+
+    ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());     // Listening
+
+    /*
+     * Cancelling one
+     */
+    client.cancelCall(last_call_id);
+
+    ASSERT_TRUE(client.hasPendingCalls());
+    ASSERT_EQ(11, client.getNumPendingCalls());
+
+    ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());     // Still listening
+
+    /*
+     * Spinning
+     */
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20));
+
+    ASSERT_TRUE(client.hasPendingCalls());
+    ASSERT_EQ(2, client.getNumPendingCalls());                                  // Two still pending
+    ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());     // Still listening
+
+    /*
+     * Validating the ones that didn't timeout
+     */
+    root_ns_a::StringService::Response last_response;
+    for (int i = 0; i < 9; i++)
+    {
+        std::ostringstream os;
+        os << "Request string: " << i;
+        last_response.string_response = os.str().c_str();
+
+        ASSERT_FALSE(handler.responses.empty());
+
+        ASSERT_STREQ(last_response.string_response.c_str(), handler.responses.front().string_response.c_str());
+
+        handler.responses.pop();
+    }
+
+    ASSERT_TRUE(handler.responses.empty());
+
+    /*
+     * Validating the 100 ms timeout
+     */
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100));
+
+    ASSERT_TRUE(client.hasPendingCalls());
+    ASSERT_EQ(1, client.getNumPendingCalls());                                  // One dropped
+    ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners());     // Still listening
+
+    ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, last_response));
+
+    /*
+     * Validating the 500 ms timeout
+     */
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
+
+    ASSERT_FALSE(client.hasPendingCalls());
+    ASSERT_EQ(0, client.getNumPendingCalls());                                  // All finished
+    ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners());     // Not listening
+
+    ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, last_response));
+}
+
+
 TEST(ServiceClient, Empty)
 {
     InterlinkedTestNodesWithSysClock nodes;

libuavcan/test/node/test_node.hpp

@@ -4,14 +4,21 @@
 
 #pragma once
 
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
 #include <uavcan/node/abstract_node.hpp>
 #include <memory>
+#include <set>
+#include <queue>
 #include "../transport/can/can.hpp"
 
 
 struct TestNode : public uavcan::INode
 {
-    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 8, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * 100, uavcan::MemPoolBlockSize> pool;
     uavcan::PoolManager<1> poolmgr;
     uavcan::MarshalBufferProvider<> buffer_provider;
     uavcan::OutgoingTransferRegistry<8> otr;
@@ -43,21 +50,21 @@ struct TestNode : public uavcan::INode
 struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
 {
     uavcan::ISystemClock& clock;
-    PairableCanDriver* other;
+    std::set<PairableCanDriver*> others;
     std::queue<uavcan::CanFrame> read_queue;
     std::queue<uavcan::CanFrame> loopback_queue;
     uint64_t error_count;
 
     PairableCanDriver(uavcan::ISystemClock& clock)
         : clock(clock)
-        , other(NULL)
         , error_count(0)
     { }
 
     void linkTogether(PairableCanDriver* with)
     {
-        this->other = with;
-        with->other = this;
+        this->others.insert(with);
+        with->others.insert(this);
+        others.erase(this);
     }
 
     virtual uavcan::ICanIface* getIface(uavcan::uint8_t iface_index)
@@ -73,7 +80,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
 
     virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline)
     {
-        assert(other);
         if (inout_masks.read == 1)
         {
             inout_masks.read = (!read_queue.empty() || !loopback_queue.empty()) ? 1 : 0;
@@ -91,8 +97,11 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
 
     virtual uavcan::int16_t send(const uavcan::CanFrame& frame, uavcan::MonotonicTime, uavcan::CanIOFlags flags)
     {
-        assert(other);
-        other->read_queue.push(frame);
+        assert(!others.empty());
+        for (std::set<PairableCanDriver*>::iterator it = others.begin(); it != others.end(); ++it)
+        {
+            (*it)->read_queue.push(frame);
+        }
         if (flags & uavcan::CanIOFlagLoopback)
         {
             loopback_queue.push(frame);
@@ -103,7 +112,6 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
     virtual uavcan::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
                                     uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
     {
-        assert(other);
         out_flags = 0;
         if (loopback_queue.empty())
         {
@@ -122,6 +130,11 @@ struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface
         return 1;
     }
 
+    void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
+    {
+        read_queue.push(can_frame);
+    }
+
     virtual uavcan::int16_t configureFilters(const uavcan::CanFilterConfig*, uavcan::uint16_t) { return -1; }
     virtual uavcan::uint16_t getNumFilters() const { return 0; }
     virtual uavcan::uint64_t getErrorCount() const { return error_count; }
@@ -181,3 +194,71 @@ struct InterlinkedTestNodes
 
 typedef InterlinkedTestNodes<SystemClockDriver> InterlinkedTestNodesWithSysClock;
 typedef InterlinkedTestNodes<SystemClockMock> InterlinkedTestNodesWithClockMock;
+
+
+template <unsigned NumNodes>
+struct TestNetwork
+{
+    struct NodeEnvironment
+    {
+        SystemClockDriver clock;
+        PairableCanDriver can_driver;
+        TestNode node;
+
+        NodeEnvironment(uavcan::NodeID node_id)
+            : can_driver(clock)
+            , node(can_driver, clock, node_id)
+        { }
+    };
+
+    std::auto_ptr<NodeEnvironment> nodes[NumNodes];
+
+    TestNetwork(uavcan::uint8_t first_node_id = 1)
+    {
+        for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+        {
+            nodes[i].reset(new NodeEnvironment(uint8_t(first_node_id + i)));
+        }
+
+        for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+        {
+            for (uavcan::uint8_t k = 0; k < NumNodes; k++)
+            {
+                nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver);
+            }
+        }
+
+        for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+        {
+            assert(nodes[i]->can_driver.others.size() == (NumNodes - 1));
+        }
+    }
+
+    int spinAll(uavcan::MonotonicDuration duration)
+    {
+        assert(!duration.isNegative());
+        unsigned nspins = unsigned(duration.toMSec() / NumNodes);
+        nspins = nspins ? nspins : 1;
+        while (nspins --> 0)
+        {
+            for (uavcan::uint8_t i = 0; i < NumNodes; i++)
+            {
+                int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1));
+                if (ret < 0)
+                {
+                    return ret;
+                }
+            }
+        }
+        return 0;
+    }
+
+    TestNode& operator[](unsigned index)
+    {
+        if (index >= NumNodes)
+        {
+            throw std::out_of_range("No such test node");
+        }
+        return nodes[index]->node;
+    }
+};

libuavcan/test/node/test_node_test.cpp

@@ -0,0 +1,32 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "test_node.hpp"
+
+
+TEST(TestNode, TestNetwork)
+{
+    TestNetwork<4> nwk;
+
+    uavcan::CanFrame frame;
+    for (uint8_t i = 0; i < 8; i++)
+    {
+        frame.data[i] = i;
+    }
+    frame.id = 1234U;
+
+    ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags()));
+
+    for (int i = 1; i < 4; i++)
+    {
+        uavcan::CanFrame rx;
+        uavcan::MonotonicTime ts_mono;
+        uavcan::UtcTime ts_utc;
+        uavcan::CanIOFlags flags = 0;
+        ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags));
+
+        ASSERT_TRUE(rx == frame);
+    }
+}

libuavcan/test/protocol/data_type_info_provider.cpp

@@ -31,29 +31,29 @@ static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResult<G
         std::cout << "Request was not successful" << std::endl;
         return false;
     }
-    if (resp->response.name != DataType::getDataTypeFullName())
+    if (resp->getResponse().name != DataType::getDataTypeFullName())
     {
         std::cout << "Type name mismatch: '"
-            << resp->response.name.c_str() << "' '"
+            << resp->getResponse().name.c_str() << "' '"
             << DataType::getDataTypeFullName() << "'" << std::endl;
         return false;
     }
-    if (DataType::getDataTypeSignature().get() != resp->response.signature)
+    if (DataType::getDataTypeSignature().get() != resp->getResponse().signature)
     {
         std::cout << "Signature mismatch" << std::endl;
         return false;
     }
-    if (resp->response.mask != mask)
+    if (resp->getResponse().mask != mask)
     {
         std::cout << "Mask mismatch" << std::endl;
         return false;
     }
-    if (resp->response.kind.value != DataType::DataTypeKind)
+    if (resp->getResponse().kind.value != DataType::DataTypeKind)
     {
         std::cout << "Kind mismatch" << std::endl;
         return false;
     }
-    if (resp->response.id != DataType::DefaultDataTypeID)
+    if (resp->getResponse().id != DataType::DefaultDataTypeID)
     {
         std::cout << "DTID mismatch" << std::endl;
         return false;
@@ -90,7 +90,7 @@ TEST(DataTypeInfoProvider, Basic)
     ASSERT_TRUE(validateDataTypeInfoResponse<GetDataTypeInfo>(gdti_cln.collector.result,
                                                               GetDataTypeInfo::Response::MASK_KNOWN |
                                                               GetDataTypeInfo::Response::MASK_SERVING));
-    ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
+    ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
 
     /*
      * GetDataTypeInfo request for GetDataTypeInfo by name
@@ -105,7 +105,7 @@ TEST(DataTypeInfoProvider, Basic)
     ASSERT_TRUE(validateDataTypeInfoResponse<GetDataTypeInfo>(gdti_cln.collector.result,
                                                               GetDataTypeInfo::Response::MASK_KNOWN |
                                                               GetDataTypeInfo::Response::MASK_SERVING));
-    ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
+    ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
 
     /*
      * GetDataTypeInfo request for NodeStatus - not used yet
@@ -148,11 +148,11 @@ TEST(DataTypeInfoProvider, Basic)
 
     ASSERT_TRUE(gdti_cln.collector.result.get());
     ASSERT_TRUE(gdti_cln.collector.result->isSuccessful());
-    ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
-    ASSERT_EQ(0, gdti_cln.collector.result->response.mask);
-    ASSERT_TRUE(gdti_cln.collector.result->response.name.empty());  // Empty name
-    ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->response.id);
-    ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->response.kind.value);
+    ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
+    ASSERT_EQ(0, gdti_cln.collector.result->getResponse().mask);
+    ASSERT_TRUE(gdti_cln.collector.result->getResponse().name.empty());  // Empty name
+    ASSERT_EQ(gdti_request.id, gdti_cln.collector.result->getResponse().id);
+    ASSERT_EQ(gdti_request.kind.value, gdti_cln.collector.result->getResponse().kind.value);
 
     /*
      * Requesting a non-existent type by name
@@ -166,11 +166,11 @@ TEST(DataTypeInfoProvider, Basic)
 
     ASSERT_TRUE(gdti_cln.collector.result.get());
     ASSERT_TRUE(gdti_cln.collector.result->isSuccessful());
-    ASSERT_EQ(1, gdti_cln.collector.result->server_node_id.get());
-    ASSERT_EQ(0, gdti_cln.collector.result->response.mask);
-    ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->response.name);
-    ASSERT_EQ(0, gdti_cln.collector.result->response.id);
-    ASSERT_EQ(0, gdti_cln.collector.result->response.kind.value);
+    ASSERT_EQ(1, gdti_cln.collector.result->getCallID().server_node_id.get());
+    ASSERT_EQ(0, gdti_cln.collector.result->getResponse().mask);
+    ASSERT_EQ("uavcan.equipment.gnss.Fix", gdti_cln.collector.result->getResponse().name);
+    ASSERT_EQ(0, gdti_cln.collector.result->getResponse().id);
+    ASSERT_EQ(0, gdti_cln.collector.result->getResponse().kind.value);
 
     /*
      * ComputeAggregateTypeSignature test for messages
@@ -184,12 +184,12 @@ TEST(DataTypeInfoProvider, Basic)
 
     ASSERT_TRUE(cats_cln.collector.result.get());
     ASSERT_TRUE(cats_cln.collector.result->isSuccessful());
-    ASSERT_EQ(1, cats_cln.collector.result->server_node_id.get());
-    ASSERT_EQ(NodeStatus::getDataTypeSignature().get(), cats_cln.collector.result->response.aggregate_signature);
-    ASSERT_EQ(2048, cats_cln.collector.result->response.mutually_known_ids.size());
-    ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[NodeStatus::DefaultDataTypeID]);
-    cats_cln.collector.result->response.mutually_known_ids[NodeStatus::DefaultDataTypeID] = false;
-    ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any());
+    ASSERT_EQ(1, cats_cln.collector.result->getCallID().server_node_id.get());
+    ASSERT_EQ(NodeStatus::getDataTypeSignature().get(), cats_cln.collector.result->getResponse().aggregate_signature);
+    ASSERT_EQ(2048, cats_cln.collector.result->getResponse().mutually_known_ids.size());
+    ASSERT_TRUE(cats_cln.collector.result->getResponse().mutually_known_ids[NodeStatus::DefaultDataTypeID]);
+    cats_cln.collector.result->getResponse().mutually_known_ids[NodeStatus::DefaultDataTypeID] = false;
+    ASSERT_FALSE(cats_cln.collector.result->getResponse().mutually_known_ids.any());
 
     /*
      * ComputeAggregateTypeSignature test for services
@@ -203,13 +203,13 @@ TEST(DataTypeInfoProvider, Basic)
 
     ASSERT_TRUE(cats_cln.collector.result.get());
     ASSERT_TRUE(cats_cln.collector.result->isSuccessful());
-    ASSERT_EQ(1, cats_cln.collector.result->server_node_id.get());
-    ASSERT_EQ(512, cats_cln.collector.result->response.mutually_known_ids.size());
-    ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID]);
-    ASSERT_TRUE(cats_cln.collector.result->response.mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID]);
-    cats_cln.collector.result->response.mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID] = false;
-    cats_cln.collector.result->response.mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID] = false;
-    ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any());
+    ASSERT_EQ(1, cats_cln.collector.result->getCallID().server_node_id.get());
+    ASSERT_EQ(512, cats_cln.collector.result->getResponse().mutually_known_ids.size());
+    ASSERT_TRUE(cats_cln.collector.result->getResponse().mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID]);
+    ASSERT_TRUE(cats_cln.collector.result->getResponse().mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID]);
+    cats_cln.collector.result->getResponse().mutually_known_ids[GetDataTypeInfo::DefaultDataTypeID] = false;
+    cats_cln.collector.result->getResponse().mutually_known_ids[ComputeAggregateTypeSignature::DefaultDataTypeID] = false;
+    ASSERT_FALSE(cats_cln.collector.result->getResponse().mutually_known_ids.any());
 
     /*
      * ComputeAggregateTypeSignature test for a non-existent type
@@ -221,6 +221,6 @@ TEST(DataTypeInfoProvider, Basic)
 
     ASSERT_TRUE(cats_cln.collector.result.get());
     ASSERT_TRUE(cats_cln.collector.result->isSuccessful());
-    ASSERT_EQ(0, cats_cln.collector.result->response.aggregate_signature);
-    ASSERT_FALSE(cats_cln.collector.result->response.mutually_known_ids.any());
+    ASSERT_EQ(0, cats_cln.collector.result->getResponse().aggregate_signature);
+    ASSERT_FALSE(cats_cln.collector.result->getResponse().mutually_known_ids.any());
 }

libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp

@@ -30,12 +30,6 @@ public:
         return can_followup;
     }
 
-    virtual void handleAllocationActivityDetection(
-        const uavcan::ReceivedDataStructure<uavcan::protocol::dynamic_node_id::Allocation>& msg)
-    {
-        std::cout << "ALLOCATION ACTIVITY\n" << msg << std::endl;
-    }
-
     bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id)
     {
         if (requests_.empty())

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

@@ -0,0 +1,78 @@
+/*
+ * 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;
+    std::cout << "NodeDiscoverer:       " << sizeof(NodeDiscoverer) << 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/protocol/dynamic_node_id_server/distributed/server.cpp

@@ -68,11 +68,10 @@ TEST(dynamic_node_id_server_RaftCore, Basic)
     /*
      * Running and trying not to fall
      */
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000));
 
-    // The one with lower node ID must become a leader
-    ASSERT_TRUE(raft_a->isLeader());
-    ASSERT_FALSE(raft_b->isLeader());
+    // Either must become a leader
+    ASSERT_TRUE(raft_a->isLeader() || raft_b->isLeader());
 
     ASSERT_EQ(0, raft_a->getCommitIndex());
     ASSERT_EQ(0, raft_b->getCommitIndex());
@@ -83,19 +82,19 @@ TEST(dynamic_node_id_server_RaftCore, Basic)
     Entry::FieldTypes::unique_id unique_id;
     uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA));
 
-    raft_a->appendLog(unique_id, uavcan::NodeID(1));
+    (raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1));
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000));
 
     ASSERT_EQ(1, raft_a->getCommitIndex());
     ASSERT_EQ(1, raft_b->getCommitIndex());
 
     /*
-     * Terminating the leader - the Follower will continue to sleep
+     * Terminating the leader
      */
     raft_a.reset();
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000));
 
     /*
      * Reinitializing the leader - current Follower will become the new Leader
@@ -106,7 +105,7 @@ TEST(dynamic_node_id_server_RaftCore, Basic)
     ASSERT_LE(0, raft_a->init(2));
     ASSERT_EQ(0, raft_a->getCommitIndex());
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000));
 
     ASSERT_FALSE(raft_a->isLeader());
     ASSERT_TRUE(raft_b->isLeader());
@@ -166,7 +165,7 @@ TEST(dynamic_node_id_server_Server, Basic)
     /*
      * Fire
      */
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(4000));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000));
 
     ASSERT_TRUE(client.isAllocationComplete());
     ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID());

libuavcan/test/protocol/dynamic_node_id_server/event.cpp

@@ -13,7 +13,6 @@ TEST(dynamic_node_id_server_EventTracer, EventCodeToString)
 
     // Simply checking some error codes
     ASSERT_STREQ("Error",                        IEventTracer::getEventName(TraceError));
-    ASSERT_STREQ("RaftActiveSwitch",             IEventTracer::getEventName(TraceRaftActiveSwitch));
     ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure));
     ASSERT_STREQ("RaftDiscoveryReceived",        IEventTracer::getEventName(TraceRaftDiscoveryReceived));
     ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected));

libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp

@@ -111,6 +111,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
     /*
      * Publishing NodeStatus, discovery is disabled
      */
+    std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl;
     handler.can_discover = false;
 
     uavcan::Publisher<uavcan::protocol::NodeStatus> node_status_pub(nodes.b);
@@ -132,48 +133,53 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic)
     /*
      * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving
      */
+    std::cout << "!!! Enabling discovery" << std::endl;
     handler.can_discover = true;
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(650));
 
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
-    ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
-    ASSERT_LE(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
     ASSERT_TRUE(disc.hasUnknownNodes());
 
     /*
      * Publishing NodeStatus
      */
+    std::cout << "!!! Publishing NodeStatus" << std::endl;
+
     node_status.uptime_sec += 5U;
     ASSERT_LE(0, node_status_pub.broadcast(node_status));
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(650));
 
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
-    ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
-    ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_EQ(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
     ASSERT_TRUE(disc.hasUnknownNodes());
 
     /*
      * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized
      */
+    std::cout << "!!! Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized" << std::endl;
+
     GetNodeInfoMockServer get_node_info_server(nodes.b);
     get_node_info_server.response.hardware_version.unique_id[0] = 123;  // Arbitrary data
     get_node_info_server.response.hardware_version.unique_id[6] = 213;
     get_node_info_server.response.hardware_version.unique_id[14] = 52;
     ASSERT_LE(0, get_node_info_server.start());
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500));
 
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
-    ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
-    ASSERT_LE(2, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
     ASSERT_FALSE(disc.hasUnknownNodes());
 
@@ -220,13 +226,13 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
     node_status.uptime_sec = 10;                        // Nonzero
     ASSERT_LE(0, node_status_pub.broadcast(node_status));
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600));
 
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
-    ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
-    ASSERT_LE(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_EQ(3, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
     ASSERT_TRUE(disc.hasUnknownNodes());
@@ -238,13 +244,13 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
     node_status.uptime_sec = 9;                         // Less than previous
     ASSERT_LE(0, node_status_pub.broadcast(node_status));
 
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600));
 
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop));
-    ASSERT_LE(6, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
-    ASSERT_LE(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(7, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_EQ(6, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
     ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeFinalized));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
     ASSERT_TRUE(disc.hasUnknownNodes());
@@ -252,13 +258,13 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
     /*
      * Waiting for timeout
      */
-    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400));
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600));
 
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop));
-    ASSERT_LE(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
-    ASSERT_LE(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
+    ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest));
+    ASSERT_EQ(8, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeFinalized));
     ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected));
     ASSERT_FALSE(disc.hasUnknownNodes());
@@ -269,3 +275,16 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts)
     ASSERT_TRUE(handler.findNode(UniqueID()));
     ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get());
 }
+
+
+TEST(dynamic_node_id_server_NodeDiscoverer, Sizes)
+{
+    using namespace uavcan;
+
+    std::cout << "BitSet<NodeID::Max + 1>:              " << sizeof(BitSet<NodeID::Max + 1>) << std::endl;
+    std::cout << "ServiceClient<protocol::GetNodeInfo>: " << sizeof(ServiceClient<protocol::GetNodeInfo>) << std::endl;
+    std::cout << "protocol::GetNodeInfo::Response:      " << sizeof(protocol::GetNodeInfo::Response) << std::endl;
+    std::cout << "Subscriber<protocol::NodeStatus, ~, 64, 0>: "
+              << sizeof(Subscriber<protocol::NodeStatus,
+                                   void (*)(const ReceivedDataStructure<protocol::NodeStatus>&), 64, 0>) << std::endl;
+}

libuavcan/test/protocol/file_server.cpp

@@ -0,0 +1,142 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/file_server.hpp>
+#include "helpers.hpp"
+
+
+class TestFileServerBackend : public uavcan::IFileServerBackend
+{
+public:
+    static const std::string file_name;
+    static const std::string file_data;
+
+    virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type)
+    {
+        if (path == file_name)
+        {
+            {
+                FileCRC crc;
+                crc.add(reinterpret_cast<const uavcan::uint8_t*>(file_data.c_str()), unsigned(file_data.length()));
+                out_crc64 = crc.get();
+            }
+            out_size = uint16_t(file_data.length());
+            out_type.flags |= EntryType::FLAG_FILE;
+            out_type.flags |= EntryType::FLAG_READABLE;
+            return 0;
+        }
+        else
+        {
+            return Error::NOT_FOUND;
+        }
+    }
+
+    virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size)
+    {
+        if (path == file_name)
+        {
+            if (offset < file_data.length())
+            {
+                inout_size = uint16_t(file_data.length() - offset);
+                std::memcpy(out_buffer, file_data.c_str() + offset, inout_size);
+            }
+            else
+            {
+                inout_size = 0;
+            }
+            return 0;
+        }
+        else
+        {
+            return Error::NOT_FOUND;
+        }
+    }
+};
+
+const std::string TestFileServerBackend::file_name = "test";
+const std::string TestFileServerBackend::file_data = "123456789";
+
+TEST(BasicFileServer, Basic)
+{
+    using namespace uavcan::protocol::file;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<GetInfo> _reg1;
+    uavcan::DefaultDataTypeRegistrator<Read> _reg2;
+
+    InterlinkedTestNodesWithSysClock nodes;
+
+    TestFileServerBackend backend;
+
+    uavcan::BasicFileServer serv(nodes.a, backend);
+    std::cout << "sizeof(uavcan::BasicFileServer): " << sizeof(uavcan::BasicFileServer) << std::endl;
+
+    ASSERT_LE(0, serv.start());
+
+    /*
+     * GetInfo, existing file
+     */
+    {
+        ServiceClientWithCollector<GetInfo> get_info(nodes.b);
+
+        GetInfo::Request get_info_req;
+        get_info_req.path.path = "test";
+
+        ASSERT_LE(0, get_info.call(1, get_info_req));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+        ASSERT_TRUE(get_info.collector.result.get());
+        ASSERT_TRUE(get_info.collector.result->isSuccessful());
+
+        ASSERT_EQ(0x62EC59E3F1A4F00A, get_info.collector.result->getResponse().crc64);
+        ASSERT_EQ(0, get_info.collector.result->getResponse().error.value);
+        ASSERT_EQ(9, get_info.collector.result->getResponse().size);
+        ASSERT_EQ(EntryType::FLAG_FILE | EntryType::FLAG_READABLE,
+                  get_info.collector.result->getResponse().entry_type.flags);
+    }
+
+    /*
+     * Read, existing file
+     */
+    {
+        ServiceClientWithCollector<Read> read(nodes.b);
+
+        Read::Request read_req;
+        read_req.path.path = "test";
+
+        ASSERT_LE(0, read.call(1, read_req));
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+
+        ASSERT_TRUE(read.collector.result.get());
+        ASSERT_TRUE(read.collector.result->isSuccessful());
+
+        ASSERT_EQ("123456789", read.collector.result->getResponse().data);
+        ASSERT_EQ(0, read.collector.result->getResponse().error.value);
+    }
+}
+
+
+TEST(FileServer, Basic)
+{
+    using namespace uavcan::protocol::file;
+
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<GetInfo> _reg1;
+    uavcan::DefaultDataTypeRegistrator<Read> _reg2;
+    uavcan::DefaultDataTypeRegistrator<Write> _reg3;
+    uavcan::DefaultDataTypeRegistrator<Delete> _reg4;
+    uavcan::DefaultDataTypeRegistrator<GetDirectoryEntryInfo> _reg5;
+
+    InterlinkedTestNodesWithSysClock nodes;
+
+    TestFileServerBackend backend;
+
+    uavcan::FileServer serv(nodes.a, backend);
+    std::cout << "sizeof(uavcan::FileServer): " << sizeof(uavcan::FileServer) << std::endl;
+
+    ASSERT_LE(0, serv.start());
+
+    // TODO TEST
+}

libuavcan/test/protocol/firmware_update_trigger.cpp

@@ -0,0 +1,336 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+using namespace uavcan::protocol::file;
+
+struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
+{
+    unsigned should_request_cnt;
+    unsigned should_retry_cnt;
+    unsigned confirmation_cnt;
+
+    std::string firmware_path;
+
+    int retry_quota;
+    std::string expected_node_name_to_update;
+
+    BeginFirmwareUpdate::Response last_error_response;
+
+    FirmwareVersionChecker()
+        : should_request_cnt(0)
+        , should_retry_cnt(0)
+        , confirmation_cnt(0)
+        , retry_quota(0)
+    { }
+
+    virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id,
+                                             const uavcan::protocol::GetNodeInfo::Response& node_info,
+                                             FirmwareFilePath& out_firmware_file_path)
+    {
+        should_request_cnt++;
+        std::cout << "REQUEST? " << int(node_id.get()) << "\n" << node_info << std::endl;
+        out_firmware_file_path = firmware_path.c_str();
+        return node_info.name == expected_node_name_to_update;
+    }
+
+    virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id,
+                                           const BeginFirmwareUpdate::Response& error_response,
+                                           FirmwareFilePath& out_firmware_file_path)
+    {
+        last_error_response = error_response;
+        std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl;
+        should_retry_cnt++;
+
+        EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str());
+
+        if (retry_quota > 0)
+        {
+            retry_quota--;
+            return true;
+        }
+        else
+        {
+            return false;
+        }
+    }
+
+    virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id,
+                                                  const BeginFirmwareUpdate::Response& response)
+    {
+        confirmation_cnt++;
+        std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl;
+    }
+};
+
+struct BeginFirmwareUpdateServer
+{
+    uint8_t response_error_code;
+
+    BeginFirmwareUpdateServer() : response_error_code(0) { }
+
+    void handleRequest(const uavcan::ReceivedDataStructure<typename BeginFirmwareUpdate::Request>& req,
+                       uavcan::ServiceResponseDataStructure<typename BeginFirmwareUpdate::Response>& res) const
+    {
+        std::cout << "REQUEST\n" << req << std::endl;
+        res.error = response_error_code;
+        res.optional_error_message = "foobar";
+    }
+
+    typedef uavcan::MethodBinder<BeginFirmwareUpdateServer*,
+        void (BeginFirmwareUpdateServer::*)(
+            const uavcan::ReceivedDataStructure<typename BeginFirmwareUpdate::Request>&,
+            uavcan::ServiceResponseDataStructure<typename BeginFirmwareUpdate::Response>&) const> Callback;
+
+    Callback makeCallback() { return Callback(this, &BeginFirmwareUpdateServer::handleRequest); }
+};
+
+
+TEST(FirmwareUpdateTrigger, Basic)
+{
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<BeginFirmwareUpdate> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg4;
+
+    InterlinkedTestNodesWithSysClock nodes;
+
+    FirmwareVersionChecker checker;
+
+    uavcan::NodeInfoRetriever node_info_retriever(nodes.a);            // On the same node
+
+    uavcan::FirmwareUpdateTrigger trigger(nodes.a, checker);
+    std::cout << "sizeof(uavcan::FirmwareUpdateTrigger): " << sizeof(uavcan::FirmwareUpdateTrigger) << std::endl;
+
+    std::auto_ptr<uavcan::NodeStatusProvider> provider(new uavcan::NodeStatusProvider(nodes.b));    // Other node
+
+    /*
+     * Initializing
+     */
+    ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/"));
+
+    ASSERT_LE(0, node_info_retriever.start());
+    ASSERT_EQ(1, node_info_retriever.getNumListeners());
+
+    uavcan::protocol::HardwareVersion hwver;
+    hwver.unique_id[0] = 123;
+    hwver.unique_id[4] = 213;
+    hwver.unique_id[8] = 45;
+
+    provider->setName("Ivan");
+    provider->setHardwareVersion(hwver);
+
+    ASSERT_LE(0, provider->startAndPublish());
+
+    ASSERT_FALSE(trigger.isTimerRunning());
+    ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+    /*
+     * Updating one node
+     * The server that can confirm the request is not running yet
+     */
+    checker.firmware_path = "firmware_path";
+    checker.expected_node_name_to_update = "Ivan";
+    checker.retry_quota = 1000;
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+
+    ASSERT_TRUE(trigger.isTimerRunning());
+    ASSERT_EQ(1, trigger.getNumPendingNodes());
+
+    ASSERT_EQ(1, checker.should_request_cnt);
+    ASSERT_EQ(0, checker.should_retry_cnt);
+    ASSERT_EQ(0, checker.confirmation_cnt);
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000));
+
+    // Still running!
+    ASSERT_TRUE(trigger.isTimerRunning());
+    ASSERT_EQ(1, trigger.getNumPendingNodes());
+
+    /*
+     * Starting the firmware update server that returns an error
+     * The checker will instruct the trigger to repeat
+     */
+    uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv(nodes.b);
+    BeginFirmwareUpdateServer srv_impl;
+
+    ASSERT_LE(0, srv.start(srv_impl.makeCallback()));
+
+    srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN;
+    checker.retry_quota = 1000;
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100));
+
+    ASSERT_EQ(1, checker.should_request_cnt);
+    ASSERT_EQ(1, checker.should_retry_cnt);
+    ASSERT_EQ(0, checker.confirmation_cnt);
+
+    // Still running!
+    ASSERT_TRUE(trigger.isTimerRunning());
+    ASSERT_EQ(1, trigger.getNumPendingNodes());
+
+    /*
+     * Trying again, this time with ERROR_IN_PROGRESS
+     */
+    srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS;
+    checker.retry_quota = 0;
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100));
+
+    ASSERT_EQ(1, checker.should_request_cnt);
+    ASSERT_EQ(1, checker.should_retry_cnt);
+    ASSERT_EQ(1, checker.confirmation_cnt);
+
+    // Stopped!
+    ASSERT_FALSE(trigger.isTimerRunning());
+    ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+    /*
+     * Restarting the node info provider
+     * Now it doesn't need an update
+     */
+    provider.reset(new uavcan::NodeStatusProvider(nodes.b));
+
+    provider->setName("Dmitry");
+    provider->setHardwareVersion(hwver);
+
+    ASSERT_LE(0, provider->startAndPublish());
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100));
+
+    ASSERT_EQ(2, checker.should_request_cnt);
+    ASSERT_EQ(1, checker.should_retry_cnt);
+    ASSERT_EQ(1, checker.confirmation_cnt);
+
+    // Stopped!
+    ASSERT_FALSE(trigger.isTimerRunning());
+    ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+    /*
+     * Final checks
+     */
+    ASSERT_EQ(0, nodes.a.internal_failure_count);
+    ASSERT_EQ(0, nodes.b.internal_failure_count);
+}
+
+
+TEST(FirmwareUpdateTrigger, MultiNode)
+{
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<BeginFirmwareUpdate> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg3;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg4;
+
+    TestNetwork<5> nodes;
+
+    // The trigger node
+    FirmwareVersionChecker checker;
+    uavcan::NodeInfoRetriever node_info_retriever(nodes[0]);
+    uavcan::FirmwareUpdateTrigger trigger(nodes[0], checker);
+
+    // The client nodes
+    std::auto_ptr<uavcan::NodeStatusProvider> provider_a(new uavcan::NodeStatusProvider(nodes[1]));
+    std::auto_ptr<uavcan::NodeStatusProvider> provider_b(new uavcan::NodeStatusProvider(nodes[2]));
+    std::auto_ptr<uavcan::NodeStatusProvider> provider_c(new uavcan::NodeStatusProvider(nodes[3]));
+    std::auto_ptr<uavcan::NodeStatusProvider> provider_d(new uavcan::NodeStatusProvider(nodes[4]));
+
+    uavcan::protocol::HardwareVersion hwver;
+
+    /*
+     * Initializing
+     */
+    ASSERT_LE(0, trigger.start(node_info_retriever, "/path_prefix/"));
+
+    ASSERT_LE(0, node_info_retriever.start());
+    ASSERT_EQ(1, node_info_retriever.getNumListeners());
+
+    hwver.unique_id[0] = 0xAA;
+    provider_a->setHardwareVersion(hwver);
+    provider_a->setName("Victor");
+    ASSERT_LE(0, provider_a->startAndPublish());
+
+    hwver.unique_id[0] = 0xBB;
+    provider_b->setHardwareVersion(hwver);
+    provider_b->setName("Victor");
+    ASSERT_LE(0, provider_b->startAndPublish());
+
+    hwver.unique_id[0] = 0xCC;
+    provider_c->setHardwareVersion(hwver);
+    provider_c->setName("Alexey");
+    ASSERT_LE(0, provider_c->startAndPublish());
+
+    hwver.unique_id[0] = 0xDD;
+    provider_d->setHardwareVersion(hwver);
+    provider_d->setName("Victor");
+    ASSERT_LE(0, provider_d->startAndPublish());
+
+    checker.expected_node_name_to_update = "Victor";    // Victors will get updated, others will not
+    checker.firmware_path = "abc";
+
+    /*
+     * Running - 3 will timout, 1 will be ignored
+     */
+    ASSERT_FALSE(trigger.isTimerRunning());
+    ASSERT_EQ(0, trigger.getNumPendingNodes());
+
+    nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100));
+
+    ASSERT_TRUE(trigger.isTimerRunning());
+    ASSERT_EQ(3, trigger.getNumPendingNodes());
+
+    ASSERT_EQ(4, checker.should_request_cnt);
+    ASSERT_EQ(0, checker.should_retry_cnt);
+    ASSERT_EQ(0, checker.confirmation_cnt);
+
+    /*
+     * Initializing the BeginFirmwareUpdate servers
+     */
+    uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_a(nodes[1]);
+    uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_b(nodes[2]);
+    uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_c(nodes[3]);
+    uavcan::ServiceServer<BeginFirmwareUpdate, BeginFirmwareUpdateServer::Callback> srv_d(nodes[4]);
+
+    BeginFirmwareUpdateServer srv_a_impl;
+    BeginFirmwareUpdateServer srv_b_impl;
+    BeginFirmwareUpdateServer srv_c_impl;
+    BeginFirmwareUpdateServer srv_d_impl;
+
+    ASSERT_LE(0, srv_a.start(srv_a_impl.makeCallback()));
+    ASSERT_LE(0, srv_b.start(srv_b_impl.makeCallback()));
+    ASSERT_LE(0, srv_c.start(srv_c_impl.makeCallback()));
+    ASSERT_LE(0, srv_d.start(srv_d_impl.makeCallback()));
+
+    srv_a_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry
+    srv_b_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // retry
+    srv_c_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_INVALID_MODE; // ignore (see below)
+    srv_d_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_OK;           // OK
+
+    /*
+     * Spinning, now we're getting some errors
+     * This also checks correctness of the round-robin selector
+     */
+    checker.retry_quota = 2;
+    nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200));   // Two will retry, one drop, one confirm
+
+    ASSERT_TRUE(trigger.isTimerRunning());
+    ASSERT_EQ(0, trigger.getNumPendingNodes());         // All removed
+
+    EXPECT_EQ(4, checker.should_request_cnt);
+    EXPECT_EQ(4, checker.should_retry_cnt);
+    EXPECT_EQ(1, checker.confirmation_cnt);
+
+    /*
+     * Waiting for the timer to stop
+     */
+    nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1100));
+
+    ASSERT_FALSE(trigger.isTimerRunning());
+}

libuavcan/test/protocol/global_time_sync_master.cpp

@@ -30,9 +30,9 @@ struct GlobalTimeSyncTestNetwork
         , master_low(120)
         , master_high(8)
     {
-        slave.can.other = &master_low.can;
-        master_low.can.other = &slave.can;
-        master_high.can.other = &slave.can;
+        slave.can.others.insert(&master_low.can);
+        master_low.can.others.insert(&slave.can);
+        master_high.can.others.insert(&slave.can);
     }
 
     void spinAll(uavcan::MonotonicDuration duration = uavcan::MonotonicDuration::fromMSec(9))

libuavcan/test/protocol/global_time_sync_slave.cpp

@@ -109,7 +109,7 @@ TEST(GlobalTimeSyncSlave, Basic)
     master2_clock.monotonic_auto_advance = 1000;
     master2_clock.preserve_utc = true;
     PairableCanDriver master2_can(master2_clock);
-    master2_can.other = &nodes.can_a;
+    master2_can.others.insert(&nodes.can_a);
     TestNode master2_node(master2_can, master2_clock, 8);
 
     uavcan::Publisher<uavcan::protocol::GlobalTimeSync> gts_pub2(master2_node);

libuavcan/test/protocol/helpers.hpp

@@ -103,3 +103,28 @@ struct BackgroundSpinner : uavcan::TimerBase
         spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1));
     }
 };
+
+template <typename CanDriver, typename MessageType>
+static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id,
+                                                       const MessageType& message, uavcan::TransferID tid)
+{
+    uavcan::StaticTransferBuffer<100> buffer;
+    uavcan::BitStream bitstream(buffer);
+    uavcan::ScalarCodec codec(bitstream);
+
+    // Manual message publication
+    ASSERT_LT(0, MessageType::encode(message, codec));
+    ASSERT_GE(8, buffer.getMaxWritePos());
+
+    // DataTypeID 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
+    uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
+                        node_id, uavcan::NodeID::Broadcast, 0, tid, true);
+
+    ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos()));
+
+    uavcan::CanFrame can_frame;
+    ASSERT_TRUE(frame.compile(can_frame));
+
+    can.pushRxToAllIfaces(can_frame);
+}

libuavcan/test/protocol/node_info_retriever.cpp

@@ -0,0 +1,273 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#if __GNUC__
+// We need auto_ptr for compatibility reasons
+# pragma GCC diagnostic ignored "-Wdeprecated-declarations"
+#endif
+
+#include <memory>
+#include <gtest/gtest.h>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan/protocol/node_status_provider.hpp>
+#include "helpers.hpp"
+
+static void publishNodeStatus(PairableCanDriver& can, uavcan::NodeID node_id, uavcan::uint8_t status_code,
+                              uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
+{
+    uavcan::protocol::NodeStatus msg;
+    msg.status_code = status_code;
+    msg.uptime_sec = uptime_sec;
+    emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
+}
+
+
+struct NodeInfoListener : public uavcan::INodeInfoListener
+{
+    std::auto_ptr<uavcan::protocol::GetNodeInfo::Response> last_node_info;
+    uavcan::NodeID last_node_id;
+    unsigned status_message_cnt;
+    unsigned status_change_cnt;
+    unsigned info_unavailable_cnt;
+
+    NodeInfoListener()
+        : status_message_cnt(0)
+        , status_change_cnt(0)
+        , info_unavailable_cnt(0)
+    { }
+
+    virtual void handleNodeInfoRetrieved(uavcan::NodeID node_id,
+                                         const uavcan::protocol::GetNodeInfo::Response& node_info)
+    {
+        last_node_id = node_id;
+        std::cout << node_info << std::endl;
+        last_node_info.reset(new uavcan::protocol::GetNodeInfo::Response(node_info));
+    }
+
+    virtual void handleNodeInfoUnavailable(uavcan::NodeID node_id)
+    {
+        std::cout << "NODE INFO FOR " << int(node_id.get()) << " IS UNAVAILABLE" << std::endl;
+        last_node_id = node_id;
+        info_unavailable_cnt++;
+    }
+
+    virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event)
+    {
+        std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: "
+                  << int(event.old_status.status_code) << " --> " << int(event.status.status_code) << std::endl;
+        status_change_cnt++;
+    }
+
+    virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure<uavcan::protocol::NodeStatus>& msg)
+    {
+        std::cout << msg << std::endl;
+        status_message_cnt++;
+    }
+};
+
+
+TEST(NodeInfoRetriever, Basic)
+{
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg3;
+
+    InterlinkedTestNodesWithSysClock nodes;
+
+    uavcan::NodeInfoRetriever retr(nodes.a);
+    std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
+    std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
+        << sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
+
+    std::auto_ptr<uavcan::NodeStatusProvider> provider(new uavcan::NodeStatusProvider(nodes.b));
+
+    NodeInfoListener listener;
+
+    /*
+     * Initialization
+     */
+    ASSERT_LE(0, retr.start());
+
+    retr.removeListener(&listener);     // Does nothing
+    retr.addListener(&listener);
+    retr.addListener(&listener);
+    retr.addListener(&listener);
+    ASSERT_EQ(1, retr.getNumListeners());
+
+    uavcan::protocol::HardwareVersion hwver;
+    hwver.unique_id[0] = 123;
+    hwver.unique_id[4] = 213;
+    hwver.unique_id[8] = 45;
+
+    provider->setName("Ivan");
+    provider->setHardwareVersion(hwver);
+
+    ASSERT_LE(0, provider->startAndPublish());
+
+    ASSERT_FALSE(retr.isRetrievingInProgress());
+    ASSERT_EQ(0, retr.getNumPendingRequests());
+
+    EXPECT_EQ(40, retr.getRequestInterval().toMSec());  // Default
+
+    /*
+     * Waiting for discovery
+     */
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50));
+    ASSERT_TRUE(retr.isRetrievingInProgress());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500));
+    ASSERT_FALSE(retr.isRetrievingInProgress());
+
+    ASSERT_EQ(2, listener.status_message_cnt);
+    ASSERT_EQ(1, listener.status_change_cnt);
+    ASSERT_EQ(0, listener.info_unavailable_cnt);
+    ASSERT_TRUE(listener.last_node_info.get());
+    ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id);
+    ASSERT_EQ("Ivan", listener.last_node_info->name);
+    ASSERT_TRUE(hwver == listener.last_node_info->hardware_version);
+
+    provider.reset();   // Moving the provider out of the way; its entry will timeout meanwhile
+
+    /*
+     * Declaring a bunch of different nodes that don't support GetNodeInfo
+     */
+    ASSERT_FALSE(retr.isRetrievingInProgress());
+
+    retr.setNumRequestAttempts(3);
+
+    uavcan::TransferID tid;
+
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 10, tid);
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 10, tid);
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid);
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_LE(1, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_LE(2, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_EQ(3, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+    ASSERT_TRUE(retr.isRetrievingInProgress());
+
+    tid.increment();
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 11, tid);
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 11, tid);
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid);
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_LE(1, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_LE(2, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_EQ(3, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+    ASSERT_TRUE(retr.isRetrievingInProgress());
+
+    tid.increment();
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 12, tid);
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 12, tid);
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid);     // Reset
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_LE(1, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_LE(2, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_EQ(3, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000));
+    ASSERT_TRUE(retr.isRetrievingInProgress());
+
+    EXPECT_EQ(11, listener.status_message_cnt);
+    EXPECT_EQ(5, listener.status_change_cnt);           // node 2 online/offline + 3 test nodes above
+    EXPECT_EQ(2, listener.info_unavailable_cnt);
+
+    tid.increment();
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid);
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_EQ(1, retr.getNumPendingRequests());
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40));
+    ASSERT_EQ(1, retr.getNumPendingRequests());                         // Still one because two went offline
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200));
+    ASSERT_TRUE(retr.isRetrievingInProgress());
+
+    tid.increment();
+    publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 12, tid);
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200));
+    ASSERT_FALSE(retr.isRetrievingInProgress());                // Out of attempts, stopping
+    ASSERT_EQ(0, retr.getNumPendingRequests());
+
+    EXPECT_EQ(13, listener.status_message_cnt);
+    EXPECT_EQ(7, listener.status_change_cnt);        // node 2 online/offline + 2 test nodes above online/offline + 1
+    EXPECT_EQ(3, listener.info_unavailable_cnt);
+}
+
+
+TEST(NodeInfoRetriever, MaxConcurrentRequests)
+{
+    uavcan::GlobalDataTypeRegistry::instance().reset();
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::NodeStatus> _reg1;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GetNodeInfo> _reg2;
+    uavcan::DefaultDataTypeRegistrator<uavcan::protocol::GlobalDiscoveryRequest> _reg3;
+
+    InterlinkedTestNodesWithSysClock nodes;
+
+    uavcan::NodeInfoRetriever retr(nodes.a);
+    std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl;
+    std::cout << "sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>): "
+        << sizeof(uavcan::ServiceClient<uavcan::protocol::GetNodeInfo>) << std::endl;
+
+    NodeInfoListener listener;
+
+    /*
+     * Initialization
+     */
+    ASSERT_LE(0, retr.start());
+
+    retr.addListener(&listener);
+    ASSERT_EQ(1, retr.getNumListeners());
+
+    ASSERT_FALSE(retr.isRetrievingInProgress());
+    ASSERT_EQ(0, retr.getNumPendingRequests());
+
+    ASSERT_EQ(40, retr.getRequestInterval().toMSec());
+
+    const unsigned MaxPendingRequests = 14;             // See class docs
+    const unsigned MinPendingRequestsAtFullLoad = 12;
+
+    /*
+     * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit.
+     */
+    for (uint8_t node_id = 1U; node_id <= 127U; node_id++)
+    {
+        publishNodeStatus(nodes.can_a, node_id, 0, 0, uavcan::TransferID());
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
+        ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
+        ASSERT_TRUE(retr.isRetrievingInProgress());
+    }
+
+    ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
+    ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests());
+
+    for (int i = 0; i < 8; i++)      // Approximate
+    {
+        nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35));
+        std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl;
+
+        ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests());
+        ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests());
+
+        ASSERT_TRUE(retr.isRetrievingInProgress());
+    }
+
+    ASSERT_LT(0, retr.getNumPendingRequests());
+    ASSERT_TRUE(retr.isRetrievingInProgress());
+
+    nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000));
+
+    ASSERT_EQ(0, retr.getNumPendingRequests());
+    ASSERT_FALSE(retr.isRetrievingInProgress());
+}

libuavcan/test/protocol/node_status_monitor.cpp

@@ -10,32 +10,10 @@
 static void publishNodeStatus(CanDriverMock& can, uavcan::NodeID node_id, uavcan::uint8_t status_code,
                               uavcan::uint32_t uptime_sec, uavcan::TransferID tid)
 {
-    uavcan::StaticTransferBuffer<100> buffer;
-    uavcan::BitStream bitstream(buffer);
-    uavcan::ScalarCodec codec(bitstream);
-
     uavcan::protocol::NodeStatus msg;
     msg.status_code = status_code;
     msg.uptime_sec = uptime_sec;
-
-    // Manual message publication
-    ASSERT_LT(0, uavcan::protocol::NodeStatus::encode(msg, codec));
-    ASSERT_GE(8, buffer.getMaxWritePos());
-
-    // DataTypeID 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
-    uavcan::Frame frame(uavcan::protocol::NodeStatus::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast,
-                        node_id, uavcan::NodeID::Broadcast, 0, tid, true);
-
-    ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos()));
-
-    uavcan::CanFrame can_frame;
-    ASSERT_TRUE(frame.compile(can_frame));
-
-    for (uint8_t i = 0; i < can.getNumIfaces(); i++)
-    {
-        can.ifaces.at(i).pushRx(can_frame);
-    }
+    emulateSingleFrameBroadcastTransfer(can, node_id, msg, tid);
 }
 
 

libuavcan/test/protocol/node_status_provider.cpp

@@ -102,14 +102,15 @@ TEST(NodeStatusProvider, Basic)
     ASSERT_TRUE(gni_cln.collector.result.get());   // Response must have been delivered
 
     ASSERT_TRUE(gni_cln.collector.result->isSuccessful());
-    ASSERT_EQ(1, gni_cln.collector.result->server_node_id.get());
+    ASSERT_EQ(1, gni_cln.collector.result->getCallID().server_node_id.get());
 
-    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL, gni_cln.collector.result->response.status.status_code);
+    ASSERT_EQ(uavcan::protocol::NodeStatus::STATUS_CRITICAL,
+              gni_cln.collector.result->getResponse().status.status_code);
 
-    ASSERT_TRUE(hwver == gni_cln.collector.result->response.hardware_version);
-    ASSERT_TRUE(swver == gni_cln.collector.result->response.software_version);
+    ASSERT_TRUE(hwver == gni_cln.collector.result->getResponse().hardware_version);
+    ASSERT_TRUE(swver == gni_cln.collector.result->getResponse().software_version);
 
-    ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->response.name);
+    ASSERT_EQ("superluminal_communication_unit", gni_cln.collector.result->getResponse().name);
 
     /*
      * GlobalDiscoveryRequest

libuavcan/test/protocol/param_server.cpp

@@ -113,16 +113,16 @@ TEST(ParamServer, Basic)
     save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_SAVE;
     doCall(save_erase_cln, save_erase_rq, nodes);
     ASSERT_TRUE(save_erase_cln.collector.result.get());
-    ASSERT_TRUE(save_erase_cln.collector.result->response.ok);
+    ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok);
 
     save_erase_rq.opcode = uavcan::protocol::param::ExecuteOpcode::Request::OPCODE_ERASE;
     doCall(save_erase_cln, save_erase_rq, nodes);
-    ASSERT_TRUE(save_erase_cln.collector.result->response.ok);
+    ASSERT_TRUE(save_erase_cln.collector.result->getResponse().ok);
 
     // Invalid opcode
     save_erase_rq.opcode = 0xFF;
     doCall(save_erase_cln, save_erase_rq, nodes);
-    ASSERT_FALSE(save_erase_cln.collector.result->response.ok);
+    ASSERT_FALSE(save_erase_cln.collector.result->getResponse().ok);
 
     /*
      * Get/set
@@ -131,17 +131,17 @@ TEST(ParamServer, Basic)
     get_set_rq.name = "nonexistent_parameter";
     doCall(get_set_cln, get_set_rq, nodes);
     ASSERT_TRUE(get_set_cln.collector.result.get());
-    ASSERT_TRUE(get_set_cln.collector.result->response.name.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty());
 
     // No such variable, shall return empty name/value
     get_set_rq.index = 0;
     get_set_rq.name.clear();
     get_set_rq.value.value_int.push_back(0xDEADBEEF);
     doCall(get_set_cln, get_set_rq, nodes);
-    ASSERT_TRUE(get_set_cln.collector.result->response.name.empty());
-    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_bool.empty());
-    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_int.empty());
-    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_float.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().name.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_bool.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_int.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_float.empty());
 
     mgr.kv["foobar"] = 123.456;    // New param
 
@@ -149,10 +149,10 @@ TEST(ParamServer, Basic)
     get_set_rq = uavcan::protocol::param::GetSet::Request();
     get_set_rq.name = "foobar";
     doCall(get_set_cln, get_set_rq, nodes);
-    ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str());
-    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_bool.empty());
-    ASSERT_TRUE(get_set_cln.collector.result->response.value.value_int.empty());
-    ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->response.value.value_float[0]);
+    ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_bool.empty());
+    ASSERT_TRUE(get_set_cln.collector.result->getResponse().value.value_int.empty());
+    ASSERT_FLOAT_EQ(123.456F, get_set_cln.collector.result->getResponse().value.value_float[0]);
 
     // Set by index
     get_set_rq = uavcan::protocol::param::GetSet::Request();
@@ -163,13 +163,13 @@ TEST(ParamServer, Basic)
         get_set_rq.value.value_string.push_back(str);
     }
     doCall(get_set_cln, get_set_rq, nodes);
-    ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str());
-    ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->response.value.value_float[0]);
+    ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str());
+    ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value.value_float[0]);
 
     // Get by index
     get_set_rq = uavcan::protocol::param::GetSet::Request();
     get_set_rq.index = 0;
     doCall(get_set_cln, get_set_rq, nodes);
-    ASSERT_STREQ("foobar", get_set_cln.collector.result->response.name.c_str());
-    ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->response.value.value_float[0]);
+    ASSERT_STREQ("foobar", get_set_cln.collector.result->getResponse().name.c_str());
+    ASSERT_FLOAT_EQ(424242, get_set_cln.collector.result->getResponse().value.value_float[0]);
 }

libuavcan/test/protocol/restart_request_server.cpp

@@ -44,7 +44,7 @@ TEST(RestartRequestServer, Basic)
 
     ASSERT_TRUE(rrs_cln.collector.result.get());
     ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
-    ASSERT_FALSE(rrs_cln.collector.result->response.ok);
+    ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok);
 
     /*
      * Accepted
@@ -57,7 +57,7 @@ TEST(RestartRequestServer, Basic)
     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
 
     ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
-    ASSERT_TRUE(rrs_cln.collector.result->response.ok);
+    ASSERT_TRUE(rrs_cln.collector.result->getResponse().ok);
 
     /*
      * Rejected by handler
@@ -68,7 +68,7 @@ TEST(RestartRequestServer, Basic)
     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
 
     ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
-    ASSERT_FALSE(rrs_cln.collector.result->response.ok);
+    ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok);
 
     /*
      * Rejected because of invalid magic number
@@ -79,5 +79,5 @@ TEST(RestartRequestServer, Basic)
     nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10));
 
     ASSERT_TRUE(rrs_cln.collector.result->isSuccessful());
-    ASSERT_FALSE(rrs_cln.collector.result->response.ok);
+    ASSERT_FALSE(rrs_cln.collector.result->getResponse().ok);
 }

libuavcan/test/protocol/transport_stats_provider.cpp

@@ -28,13 +28,13 @@ TEST(TransportStatsProvider, Basic)
 
     ASSERT_TRUE(tsp_cln.collector.result.get());
     ASSERT_TRUE(tsp_cln.collector.result->isSuccessful());
-    ASSERT_EQ(0, tsp_cln.collector.result->response.transfer_errors);
-    ASSERT_EQ(1, tsp_cln.collector.result->response.transfers_rx);
-    ASSERT_EQ(0, tsp_cln.collector.result->response.transfers_tx);
-    ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats.size());
-    ASSERT_EQ(0, tsp_cln.collector.result->response.can_iface_stats[0].errors);
-    ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats[0].frames_rx);
-    ASSERT_EQ(0, tsp_cln.collector.result->response.can_iface_stats[0].frames_tx);
+    ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_rx);
+    ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfers_tx);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size());
+    ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx);
+    ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx);
 
     /*
      * Second request
@@ -43,13 +43,13 @@ TEST(TransportStatsProvider, Basic)
     ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
 
     ASSERT_TRUE(tsp_cln.collector.result.get());
-    ASSERT_EQ(0, tsp_cln.collector.result->response.transfer_errors);
-    ASSERT_EQ(2, tsp_cln.collector.result->response.transfers_rx);
-    ASSERT_EQ(1, tsp_cln.collector.result->response.transfers_tx);
-    ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats.size());
-    ASSERT_EQ(0, tsp_cln.collector.result->response.can_iface_stats[0].errors);
-    ASSERT_EQ(2, tsp_cln.collector.result->response.can_iface_stats[0].frames_rx);
-    ASSERT_EQ(6, tsp_cln.collector.result->response.can_iface_stats[0].frames_tx);
+    ASSERT_EQ(0, tsp_cln.collector.result->getResponse().transfer_errors);
+    ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_rx);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfers_tx);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size());
+    ASSERT_EQ(0, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors);
+    ASSERT_EQ(2, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx);
+    ASSERT_EQ(6, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx);
 
     /*
      * Sending a malformed frame, it must be registered as tranfer error
@@ -74,11 +74,11 @@ TEST(TransportStatsProvider, Basic)
     ASSERT_LE(0, nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)));
 
     ASSERT_TRUE(tsp_cln.collector.result.get());
-    ASSERT_EQ(1, tsp_cln.collector.result->response.transfer_errors);                  // That broken frame
-    ASSERT_EQ(3, tsp_cln.collector.result->response.transfers_rx);
-    ASSERT_EQ(2, tsp_cln.collector.result->response.transfers_tx);
-    ASSERT_EQ(1, tsp_cln.collector.result->response.can_iface_stats.size());
-    ASSERT_EQ(72, tsp_cln.collector.result->response.can_iface_stats[0].errors);
-    ASSERT_EQ(4, tsp_cln.collector.result->response.can_iface_stats[0].frames_rx);     // Same here
-    ASSERT_EQ(12, tsp_cln.collector.result->response.can_iface_stats[0].frames_tx);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().transfer_errors);                  // That broken frame
+    ASSERT_EQ(3, tsp_cln.collector.result->getResponse().transfers_rx);
+    ASSERT_EQ(2, tsp_cln.collector.result->getResponse().transfers_tx);
+    ASSERT_EQ(1, tsp_cln.collector.result->getResponse().can_iface_stats.size());
+    ASSERT_EQ(72, tsp_cln.collector.result->getResponse().can_iface_stats[0].errors);
+    ASSERT_EQ(4, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_rx);     // Same here
+    ASSERT_EQ(12, tsp_cln.collector.result->getResponse().can_iface_stats[0].frames_tx);
 }

libuavcan/test/transport/can/can.hpp

@@ -169,6 +169,14 @@ public:
         , select_failure(false)
     { }
 
+    void pushRxToAllIfaces(const uavcan::CanFrame& can_frame)
+    {
+        for (uint8_t i = 0; i < getNumIfaces(); i++)
+        {
+            ifaces.at(i).pushRx(can_frame);
+        }
+    }
+
     virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime deadline)
     {
         assert(this);

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

libuavcan/test/util/map.cpp

@@ -105,18 +105,18 @@ TEST(Map, Basic)
     ASSERT_EQ("D", *map->access("4"));
 
     // Finding some keys
-    ASSERT_EQ("1", *map->findFirstKey(KeyFindPredicate("1")));
-    ASSERT_EQ("2", *map->findFirstKey(KeyFindPredicate("2")));
-    ASSERT_EQ("3", *map->findFirstKey(KeyFindPredicate("3")));
-    ASSERT_EQ("4", *map->findFirstKey(KeyFindPredicate("4")));
-    ASSERT_FALSE(map->findFirstKey(KeyFindPredicate("nonexistent_key")));
+    ASSERT_EQ("1", *map->find(KeyFindPredicate("1")));
+    ASSERT_EQ("2", *map->find(KeyFindPredicate("2")));
+    ASSERT_EQ("3", *map->find(KeyFindPredicate("3")));
+    ASSERT_EQ("4", *map->find(KeyFindPredicate("4")));
+    ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_key")));
 
     // Finding some values
-    ASSERT_EQ("1", *map->findFirstKey(ValueFindPredicate("A")));
-    ASSERT_EQ("2", *map->findFirstKey(ValueFindPredicate("B")));
-    ASSERT_EQ("3", *map->findFirstKey(ValueFindPredicate("C")));
-    ASSERT_EQ("4", *map->findFirstKey(ValueFindPredicate("D")));
-    ASSERT_FALSE(map->findFirstKey(KeyFindPredicate("nonexistent_value")));
+    ASSERT_EQ("1", *map->find(ValueFindPredicate("A")));
+    ASSERT_EQ("2", *map->find(ValueFindPredicate("B")));
+    ASSERT_EQ("3", *map->find(ValueFindPredicate("C")));
+    ASSERT_EQ("4", *map->find(ValueFindPredicate("D")));
+    ASSERT_FALSE(map->find(KeyFindPredicate("nonexistent_value")));
 
     // Removing one static
     map->remove("1");                             // One of dynamics now migrates to the static storage
@@ -176,7 +176,7 @@ TEST(Map, Basic)
     // Removing odd values - nearly half of them
     ASSERT_EQ(2, map->getNumStaticPairs());
     const unsigned num_dynamics_old = map->getNumDynamicPairs();
-    map->removeWhere(oddValuePredicate);
+    map->removeAllWhere(oddValuePredicate);
     ASSERT_EQ(2, map->getNumStaticPairs());
     const unsigned num_dynamics_new = map->getNumDynamicPairs();
     std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl;

libuavcan/test/util/multiset.cpp

@@ -0,0 +1,308 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <string>
+#include <cstdio>
+#include <memory>
+#include <gtest/gtest.h>
+#include <uavcan/util/multiset.hpp>
+
+
+static std::string toString(long x)
+{
+    char buf[80];
+    std::snprintf(buf, sizeof(buf), "%li", x);
+    return std::string(buf);
+}
+
+static bool oddValuePredicate(const std::string& value)
+{
+    EXPECT_FALSE(value.empty());
+    const int num = atoi(value.c_str());
+    return num & 1;
+}
+
+struct FindPredicate
+{
+    const std::string target;
+    FindPredicate(const std::string& target) : target(target) { }
+    bool operator()(const std::string& value) const { return value == target; }
+};
+
+struct NoncopyableWithCounter : uavcan::Noncopyable
+{
+    static int num_objects;
+    long long value;
+
+    NoncopyableWithCounter() : value(0) { num_objects++; }
+    NoncopyableWithCounter(long long x) : value(x) { num_objects++; }
+    ~NoncopyableWithCounter() { num_objects--; }
+
+    static bool isNegative(const NoncopyableWithCounter& val)
+    {
+        return val.value < 0;
+    }
+
+    bool operator==(const NoncopyableWithCounter& ref) const { return ref.value == value; }
+};
+
+int NoncopyableWithCounter::num_objects = 0;
+
+template <typename T>
+struct SummationOperator : uavcan::Noncopyable
+{
+    T accumulator;
+    SummationOperator() : accumulator() { }
+    void operator()(const T& x) { accumulator += x; }
+};
+
+struct ClearingOperator
+{
+    template <typename T>
+    void operator()(T& x) const { x = T(); }
+};
+
+
+TEST(Multiset, Basic)
+{
+    using uavcan::Multiset;
+
+    static const int POOL_BLOCKS = 3;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    typedef Multiset<std::string, 2> MultisetType;
+    std::auto_ptr<MultisetType> mset(new MultisetType(poolmgr));
+
+    typedef SummationOperator<std::string> StringConcatenationOperator;
+
+    // Empty
+    mset->removeFirst("foo");
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_FALSE(mset->getByIndex(0));
+    ASSERT_FALSE(mset->getByIndex(1));
+    ASSERT_FALSE(mset->getByIndex(10000));
+
+    // Static addion
+    ASSERT_EQ("1", *mset->emplace("1"));
+    ASSERT_EQ("2", *mset->emplace("2"));
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    ASSERT_EQ(0, mset->getNumDynamicItems());
+
+    // Ordering
+    ASSERT_TRUE(*mset->getByIndex(0) == "1");
+    ASSERT_TRUE(*mset->getByIndex(1) == "2");
+
+    {
+        StringConcatenationOperator op;
+        mset->forEach<StringConcatenationOperator&>(op);
+        ASSERT_EQ("12", op.accumulator);
+    }
+
+    // Dynamic addition
+    ASSERT_EQ("3", *mset->emplace("3"));
+    ASSERT_EQ("3", *mset->getByIndex(2));
+    ASSERT_EQ(1, pool.getNumUsedBlocks());
+
+    ASSERT_EQ("4", *mset->emplace("4"));
+    ASSERT_LE(1, pool.getNumUsedBlocks());      // One or more
+    ASSERT_EQ(2, mset->getNumStaticItems());
+    ASSERT_EQ(2, mset->getNumDynamicItems());
+
+    // Making sure everything is here
+    ASSERT_EQ("1", *mset->getByIndex(0));
+    ASSERT_EQ("2", *mset->getByIndex(1));
+    // 2 and 3 are not tested because their placement depends on number of items per dynamic block
+    ASSERT_FALSE(mset->getByIndex(100));
+    ASSERT_FALSE(mset->getByIndex(4));
+
+    const std::string data_at_pos2 = *mset->getByIndex(2);
+    const std::string data_at_pos3 = *mset->getByIndex(3);
+
+    // Finding some items
+    ASSERT_EQ("1", *mset->find(FindPredicate("1")));
+    ASSERT_EQ("2", *mset->find(FindPredicate("2")));
+    ASSERT_EQ("3", *mset->find(FindPredicate("3")));
+    ASSERT_EQ("4", *mset->find(FindPredicate("4")));
+    ASSERT_FALSE(mset->find(FindPredicate("nonexistent")));
+
+    {
+        StringConcatenationOperator op;
+        mset->forEach<StringConcatenationOperator&>(op);
+        std::cout << "Accumulator: " << op.accumulator << std::endl;
+        ASSERT_EQ(4, op.accumulator.size());
+    }
+
+    // Removing one static; ordering will be preserved
+    mset->removeFirst("1");
+    mset->removeFirst("foo");                           // There's no such thing anyway
+    ASSERT_LE(1, pool.getNumUsedBlocks());
+    ASSERT_EQ(1, mset->getNumStaticItems());
+    ASSERT_EQ(2, mset->getNumDynamicItems());           // This container does not move items
+
+    // Ordering has not changed
+    ASSERT_EQ("2", *mset->getByIndex(0));       // Entry "1" was here
+    ASSERT_EQ(data_at_pos2, *mset->getByIndex(1));
+    ASSERT_EQ(data_at_pos3, *mset->getByIndex(2));
+
+    // Removing another static
+    mset->removeFirst("2");
+    ASSERT_EQ(0, mset->getNumStaticItems());
+    ASSERT_EQ(2, mset->getNumDynamicItems());
+    ASSERT_LE(1, pool.getNumUsedBlocks());
+
+    // Adding some new items
+    unsigned max_value_integer = 0;
+    for (int i = 0; i < 100; i++)
+    {
+        const std::string value = toString(i);
+        std::string* res = mset->emplace(value);  // Will NOT override above
+        if (res == NULL)
+        {
+            ASSERT_LT(2, i);
+            break;
+        }
+        else
+        {
+            ASSERT_EQ(value, *res);
+        }
+        max_value_integer = unsigned(i);
+    }
+    std::cout << "Max value: " << max_value_integer << std::endl;
+
+    // Making sure there is true OOM
+    ASSERT_EQ(0, pool.getNumFreeBlocks());
+    ASSERT_FALSE(mset->emplace("nonexistent"));
+
+    // Removing odd values - nearly half of them
+    mset->removeAllWhere(oddValuePredicate);
+
+    // Making sure there's no odd values left
+    for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++)
+    {
+        const std::string* val = mset->find(FindPredicate(toString(kv_int)));
+        if (val)
+        {
+            ASSERT_FALSE(kv_int & 1);
+        }
+        else
+        {
+            ASSERT_TRUE(kv_int & 1);
+        }
+    }
+
+    // Clearing all strings
+    {
+        StringConcatenationOperator op;
+        mset->forEach<StringConcatenationOperator&>(op);
+        std::cout << "Accumulator before clearing: " << op.accumulator << std::endl;
+    }
+    mset->forEach(ClearingOperator());
+    {
+        StringConcatenationOperator op;
+        mset->forEach<StringConcatenationOperator&>(op);
+        std::cout << "Accumulator after clearing: " << op.accumulator << std::endl;
+        ASSERT_TRUE(op.accumulator.empty());
+    }
+
+    // Making sure the memory will be released
+    mset.reset();
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+}
+
+
+TEST(Multiset, PrimitiveKey)
+{
+    using uavcan::Multiset;
+
+    static const int POOL_BLOCKS = 3;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    typedef Multiset<int, 2> MultisetType;
+    std::auto_ptr<MultisetType> mset(new MultisetType(poolmgr));
+
+    // Empty
+    mset->removeFirst(8);
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_EQ(0, mset->getSize());
+    ASSERT_FALSE(mset->getByIndex(0));
+
+    // Insertion
+    ASSERT_EQ(1, *mset->emplace(1));
+    ASSERT_EQ(1, mset->getSize());
+    ASSERT_EQ(2, *mset->emplace(2));
+    ASSERT_EQ(2, mset->getSize());
+    ASSERT_EQ(3, *mset->emplace(3));
+    ASSERT_EQ(4, *mset->emplace(4));
+    ASSERT_EQ(4, mset->getSize());
+
+#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11
+    // Only C++11 because C++03 uses one entry per pool block which breaks ordering
+    ASSERT_EQ(1, *mset->getByIndex(0));
+    ASSERT_EQ(2, *mset->getByIndex(1));
+    ASSERT_EQ(3, *mset->getByIndex(2));
+    ASSERT_EQ(4, *mset->getByIndex(3));
+    ASSERT_FALSE(mset->getByIndex(5));
+    ASSERT_FALSE(mset->getByIndex(1000));
+#endif
+
+    // Summation and clearing
+    {
+        SummationOperator<int> summation_operator;
+        mset->forEach<SummationOperator<int>&>(summation_operator);
+        ASSERT_EQ(1 + 2 + 3 + 4, summation_operator.accumulator);
+    }
+    mset->forEach(ClearingOperator());
+    {
+        SummationOperator<int> summation_operator;
+        mset->forEach<SummationOperator<int>&>(summation_operator);
+        ASSERT_EQ(0, summation_operator.accumulator);
+    }
+}
+
+
+TEST(Multiset, NoncopyableWithCounter)
+{
+    using uavcan::Multiset;
+
+    static const int POOL_BLOCKS = 3;
+    uavcan::PoolAllocator<uavcan::MemPoolBlockSize * POOL_BLOCKS, uavcan::MemPoolBlockSize> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    typedef Multiset<NoncopyableWithCounter, 2> MultisetType;
+    std::auto_ptr<MultisetType> mset(new MultisetType(poolmgr));
+
+    ASSERT_EQ(0, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(0,    mset->emplace()->value);
+    ASSERT_EQ(1, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(123,  mset->emplace(123)->value);
+    ASSERT_EQ(2, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(-456, mset->emplace(-456)->value);
+    ASSERT_EQ(3, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(456,  mset->emplace(456)->value);
+    ASSERT_EQ(4, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(-789, mset->emplace(-789)->value);
+    ASSERT_EQ(5, NoncopyableWithCounter::num_objects);
+
+    mset->removeFirst(NoncopyableWithCounter(0));
+    ASSERT_EQ(4, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(123, mset->getByIndex(0)->value);
+
+    mset->removeFirstWhere(&NoncopyableWithCounter::isNegative);
+    ASSERT_EQ(3, NoncopyableWithCounter::num_objects);
+    ASSERT_EQ(456, mset->getByIndex(1)->value);                 // -456 is now removed
+
+    mset->removeAllWhere(&NoncopyableWithCounter::isNegative);
+    ASSERT_EQ(2, NoncopyableWithCounter::num_objects);          // Only 1 and 2 are left
+
+    mset.reset();
+
+    ASSERT_EQ(0, pool.getNumUsedBlocks());
+    ASSERT_EQ(0, NoncopyableWithCounter::num_objects);          // All destroyed
+}

libuavcan_drivers/linux/CMakeLists.txt

@@ -25,6 +25,8 @@ if (TARGET uavcan)
     set(UAVCAN_LIB uavcan)
     include_directories(${libuavcan_SOURCE_DIR}/include
                         ${libuavcan_SOURCE_DIR}/include/dsdlc_generated)
+    message(STATUS "POSIX source dir: ${libuavcan_posix_SOURCE_DIR}")
+    include_directories(${libuavcan_posix_SOURCE_DIR}/include)
 else ()
     message(STATUS "Using installed uavcan library")
     find_library(UAVCAN_LIB uavcan REQUIRED)
@@ -55,6 +57,12 @@ target_link_libraries(test_time_sync ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
 add_executable(test_system_utils apps/test_system_utils.cpp)
 target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
 
+add_executable(test_posix apps/test_posix.cpp)
+target_link_libraries(test_posix ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
+add_executable(test_dynamic_node_id_client apps/test_dynamic_node_id_client.cpp)
+target_link_libraries(test_dynamic_node_id_client ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
 #
 # Tools
 #
@@ -64,7 +72,11 @@ target_link_libraries(uavcan_status_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS
 add_executable(uavcan_nodetool apps/uavcan_nodetool.cpp)
 target_link_libraries(uavcan_nodetool ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
 
+add_executable(uavcan_dynamic_node_id_server apps/uavcan_dynamic_node_id_server.cpp)
+target_link_libraries(uavcan_dynamic_node_id_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT})
+
 install(TARGETS uavcan_status_monitor
                 uavcan_nodetool
+                uavcan_dynamic_node_id_server
         RUNTIME DESTINATION bin)
         

libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp

@@ -0,0 +1,121 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include "debug.hpp"
+#include <uavcan/protocol/dynamic_node_id_client.hpp>
+#include <uavcan_linux/uavcan_linux.hpp>
+
+namespace
+{
+
+uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector<std::string>& ifaces,
+                                            const std::uint8_t instance_id,
+                                            const uavcan::NodeID preferred_node_id,
+                                            const std::string& name)
+{
+    /*
+     * Initializing the node object
+     */
+    auto node = uavcan_linux::makeNode(ifaces);
+
+    node->setName(name.c_str());
+    node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+    {
+        const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, instance_id);
+
+        uavcan::protocol::HardwareVersion hwver;
+        std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
+        std::cout << hwver << std::endl;
+
+        node->setHardwareVersion(hwver);
+    }
+
+    /*
+     * Starting the node
+     */
+    const int start_res = node->start();
+    ENFORCE(0 == start_res);
+
+    /*
+     * Running the dynamic node ID client until it's done
+     */
+    uavcan::DynamicNodeIDClient client(*node);
+
+    ENFORCE(0 <= client.start(node->getNodeStatusProvider().getHardwareVersion(), preferred_node_id));
+
+    std::cout << "Waiting for dynamic node ID allocation..." << std::endl;
+
+    while (!client.isAllocationComplete())
+    {
+        const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+        if (res < 0)
+        {
+            std::cerr << "Spin error: " << res << std::endl;
+        }
+    }
+
+    std::cout << "Node ID " << int(client.getAllocatedNodeID().get())
+              << " allocated by " << int(client.getAllocatorNodeID().get()) << std::endl;
+
+    /*
+     * Finishing the node initialization
+     */
+    node->setNodeID(client.getAllocatedNodeID());
+
+    node->setStatusOk();
+
+    return node;
+}
+
+void runForever(const uavcan_linux::NodePtr& node)
+{
+    while (true)
+    {
+        const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100));
+        if (res < 0)
+        {
+            std::cerr << "Spin error: " << res << std::endl;
+        }
+    }
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+    try
+    {
+        if (argc < 3)
+        {
+            std::cerr << "Usage:\n\t"
+                      << argv[0] << " <instance-id> <can-iface-name-1> [can-iface-name-N...]\n"
+                      << "Where <instance-id> is used to augment the unique node ID and also indicates\n"
+                      << "the preferred node ID value. Valid range is [0, 127]."
+                      << std::endl;
+            return 1;
+        }
+
+        const int instance_id = std::stoi(argv[1]);
+        if (instance_id < 0 || instance_id > 127)
+        {
+            std::cerr << "Invalid instance ID: " << instance_id << std::endl;
+            std::exit(1);
+        }
+
+        uavcan_linux::NodePtr node = initNodeWithDynamicID(std::vector<std::string>(argv + 2, argv + argc),
+                                                           std::uint8_t(instance_id),
+                                                           std::uint8_t(instance_id),
+                                                           "org.uavcan.linux_test_dynamic_node_id_client");
+        runForever(node);
+
+        return 0;
+    }
+    catch (const std::exception& ex)
+    {
+        std::cerr << "Error: " << ex.what() << std::endl;
+        return 1;
+    }
+}

libuavcan_drivers/linux/apps/test_posix.cpp

@@ -0,0 +1,73 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_linux/uavcan_linux.hpp>
+#include <iostream>
+#include <iomanip>
+#include "debug.hpp"
+
+int main(int argc, const char** argv)
+{
+    (void)argc;
+    (void)argv;
+    try
+    {
+        ENFORCE(0 == std::system("mkdir -p /tmp/uavcan_posix/dynamic_node_id_server"));
+
+        /*
+         * Event tracer test
+         */
+        {
+            using namespace uavcan::dynamic_node_id_server;
+
+            const std::string event_log_file("/tmp/uavcan_posix/dynamic_node_id_server/event.log");
+
+            uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
+            ENFORCE(0 <= tracer.init(event_log_file.c_str()));
+
+            // Adding a line
+            static_cast<IEventTracer&>(tracer).onEvent(TraceError, 123456);
+            ENFORCE(0 == std::system(("cat " + event_log_file).c_str()));
+
+            // Removing the log file
+            ENFORCE(0 == std::system(("rm -f " + event_log_file).c_str()));
+
+            // Adding another line
+            static_cast<IEventTracer&>(tracer).onEvent(TraceError, 789123);
+            ENFORCE(0 == std::system(("cat " + event_log_file).c_str()));
+        }
+
+        /*
+         * Storage backend test
+         */
+        {
+            using namespace uavcan::dynamic_node_id_server;
+
+            uavcan_posix::dynamic_node_id_server::FileStorageBackend backend;
+            ENFORCE(0 <= backend.init("/tmp/uavcan_posix/dynamic_node_id_server/storage"));
+
+            auto print_key = [&](const char* key) {
+                std::cout << static_cast<IStorageBackend&>(backend).get(key).c_str() << std::endl;
+            };
+
+            print_key("foobar");
+
+            static_cast<IStorageBackend&>(backend).set("foobar", "0123456789abcdef0123456789abcdef");
+            static_cast<IStorageBackend&>(backend).set("the_answer", "42");
+
+            print_key("foobar");
+            print_key("the_answer");
+            print_key("nonexistent");
+        }
+
+        return 0;
+    }
+    catch (const std::exception& ex)
+    {
+        std::cerr << "Exception: " << ex.what() << std::endl;
+        return 1;
+    }
+}

libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp

@@ -0,0 +1,676 @@
+/*
+ * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <iostream>
+#include <algorithm>
+#include <sstream>
+#include <iomanip>
+#include <string>
+#include <cstdlib>
+#include <cstdio>
+#include <deque>
+#include <unordered_map>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include "debug.hpp"
+// UAVCAN
+#include <uavcan/protocol/dynamic_node_id_server/distributed.hpp>
+// UAVCAN Linux drivers
+#include <uavcan_linux/uavcan_linux.hpp>
+// UAVCAN POSIX drivers
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+
+namespace
+{
+
+constexpr int MaxNumLastEvents = 30;
+constexpr int MinUpdateInterval = 100;
+
+uavcan_linux::NodePtr initNode(const std::vector<std::string>& ifaces, uavcan::NodeID nid, const std::string& name)
+{
+    auto node = uavcan_linux::makeNode(ifaces);
+
+    node->setNodeID(nid);
+    node->setName(name.c_str());
+    node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG);
+
+    {
+        const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(),  name,  nid.get());
+
+        uavcan::protocol::HardwareVersion hwver;
+        std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin());
+        std::cout << hwver << std::endl;
+
+        node->setHardwareVersion(hwver);
+    }
+
+    const int start_res = node->start();
+    ENFORCE(0 == start_res);
+
+    uavcan::NetworkCompatibilityCheckResult check_result;
+    ENFORCE(0 == node->checkNetworkCompatibility(check_result));
+    if (!check_result.isOk())
+    {
+        throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get()));
+    }
+
+    node->setStatusOk();
+
+    return node;
+}
+
+
+class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer
+{
+public:
+    struct RecentEvent
+    {
+        const uavcan::MonotonicDuration time_since_startup;
+        const uavcan::UtcTime utc_timestamp;
+        const uavcan::dynamic_node_id_server::TraceCode code;
+        const std::int64_t argument;
+
+        RecentEvent(uavcan::MonotonicDuration arg_time_since_startup,
+                    uavcan::UtcTime arg_utc_timestamp,
+                    uavcan::dynamic_node_id_server::TraceCode arg_code,
+                    std::int64_t arg_argument)
+            : time_since_startup(arg_time_since_startup)
+            , utc_timestamp(arg_utc_timestamp)
+            , code(arg_code)
+            , argument(arg_argument)
+        { }
+
+        uavcan::MakeString<81>::Type toString() const   // Heapless return
+        {
+            char timebuf[11] = { };
+            {
+                const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6;
+                const auto tm = localtime(&rawtime);
+                std::strftime(timebuf, 10, "%H:%M:%S.", tm);
+                std::snprintf(&timebuf[9], 3, "%02u", static_cast<unsigned>((utc_timestamp.toMSec() % 1000U) / 10U));
+            }
+
+            decltype(toString()) out;
+            out.resize(out.capacity());
+            (void)std::snprintf(reinterpret_cast<char*>(out.begin()), out.size() - 1U,
+                                "%-11s  %-28s %-20lld %016llx",
+                                timebuf,
+                                getEventName(code),
+                                static_cast<long long>(argument),
+                                static_cast<long long>(argument));
+            return out;
+        }
+
+        static const char* getTableHeader()
+        {
+            // Matches the string format above
+            return "Timestamp    Event name                   Argument (dec)       Argument (hex)";
+        }
+    };
+
+    struct EventStatisticsRecord
+    {
+        std::uint64_t count;
+        uavcan::MonotonicTime last_occurence;
+
+        EventStatisticsRecord()
+            : count(0)
+        { }
+
+        void hit(uavcan::MonotonicTime ts)
+        {
+            count++;
+            last_occurence = ts;
+        }
+    };
+
+private:
+    struct EnumKeyHash
+    {
+        template <typename T>
+        std::size_t operator()(T t) const { return static_cast<std::size_t>(t); }
+    };
+
+    uavcan_linux::SystemClock clock_;
+    const uavcan::MonotonicTime started_at_ = clock_.getMonotonic();
+    const unsigned num_last_events_;
+
+    std::deque<RecentEvent> last_events_;
+    std::unordered_map<uavcan::dynamic_node_id_server::TraceCode, EventStatisticsRecord, EnumKeyHash> event_counters_;
+
+    bool had_events_ = false;
+
+    virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, std::int64_t argument)
+    {
+        uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument);
+
+        had_events_ = true;
+
+        const auto ts_m = clock_.getMonotonic();
+        const auto ts_utc = clock_.getUtc();
+        const auto time_since_startup = ts_m - started_at_;
+
+        last_events_.emplace_front(time_since_startup, ts_utc, code, argument);
+        if (last_events_.size() > num_last_events_)
+        {
+            last_events_.pop_back();
+        }
+
+        event_counters_[code].hit(ts_m);
+    }
+
+public:
+    EventTracer(unsigned num_last_events_to_keep)
+        : num_last_events_(num_last_events_to_keep)
+    { }
+
+    using uavcan_posix::dynamic_node_id_server::FileEventTracer::init;
+
+    const RecentEvent& getEventByIndex(unsigned index) const { return last_events_.at(index); }
+
+    unsigned getNumEvents() const { return last_events_.size(); }
+
+    const decltype(event_counters_)& getEventCounters() const { return event_counters_; }
+
+    bool hadEvents()
+    {
+        if (had_events_)
+        {
+            had_events_ = false;
+            return true;
+        }
+        return false;
+    }
+};
+
+
+::winsize getTerminalSize()
+{
+    auto w = ::winsize();
+    ENFORCE(0 >= ioctl(STDOUT_FILENO, TIOCGWINSZ, &w));
+    ENFORCE(w.ws_col > 0 && w.ws_row > 0);
+    return w;
+}
+
+
+std::vector<std::pair<uavcan::dynamic_node_id_server::TraceCode, EventTracer::EventStatisticsRecord>>
+collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events)
+{
+    // First, creating a vector of pairs (event code, count)
+    typedef std::pair<uavcan::dynamic_node_id_server::TraceCode, EventTracer::EventStatisticsRecord> Pair;
+    const auto counters = event_tracer.getEventCounters();
+    std::vector<Pair> pairs(counters.size());
+    std::copy(counters.begin(), counters.end(), pairs.begin());
+
+    // Now, sorting the pairs so that the most recent ones are on top of the list
+    std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) {
+        return a.second.last_occurence > b.second.last_occurence;
+    });
+
+    // Cutting the oldest events away
+    pairs.resize(std::min(num_events, unsigned(pairs.size())));
+
+    // Sorting so that the most frequent events are on top of the list
+    std::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) {
+        return a.second.count > b.second.count;
+    });
+
+    return pairs;
+}
+
+enum class CLIColor : unsigned
+{
+    Red     = 31,
+    Green   = 32,
+    Yellow  = 33,
+    Blue    = 34,
+    Magenta = 35,
+    Cyan    = 36,
+    White   = 37,
+    Default = 39
+};
+
+CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); }
+
+class CLIColorizer
+{
+    const CLIColor color_;
+public:
+    explicit CLIColorizer(CLIColor c) : color_(c)
+    {
+        std::printf("\033[%um", static_cast<unsigned>(color_));
+    }
+
+    ~CLIColorizer()
+    {
+        std::printf("\033[%um", static_cast<unsigned>(CLIColor::Default));
+    }
+};
+
+
+void redraw(const uavcan_linux::NodePtr& node,
+            const uavcan::MonotonicTime timestamp,
+            const EventTracer& event_tracer,
+            const uavcan::dynamic_node_id_server::DistributedServer& server)
+{
+    using uavcan::dynamic_node_id_server::distributed::RaftCore;
+
+    /*
+     * Constants that are permanent for the designed UI layout
+     */
+    constexpr unsigned NumRelevantEvents = 16;
+    constexpr unsigned NumRowsWithoutEvents = 3;
+
+    /*
+     * Collecting the data
+     */
+    const unsigned num_rows = getTerminalSize().ws_row;
+
+    const auto relevant_events = collectRelevantEvents(event_tracer, NumRelevantEvents);
+
+    const uavcan::dynamic_node_id_server::distributed::StateReport report(server);
+
+    const auto time_since_last_activity = timestamp - report.last_activity_timestamp;
+
+    /*
+     * Basic rendering functions
+     */
+    unsigned next_relevant_event_index = 0;
+
+    const auto render_next_event_counter = [&]()
+    {
+        const char* event_name = "";
+        char event_count_str[10] = { };
+        CLIColor event_color = CLIColor::Default;
+
+        if (next_relevant_event_index < relevant_events.size())
+        {
+            const auto e = relevant_events[next_relevant_event_index];
+            event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first);
+            std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu",
+                          static_cast<unsigned long long>(e.second.count));
+            event_color = getColorHash(static_cast<unsigned>(e.first));
+        }
+        next_relevant_event_index++;
+
+        std::printf(" | ");
+        CLIColorizer izer(event_color);
+        std::printf("%-29s %-9s\n", event_name, event_count_str);
+    };
+
+    const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color)
+    {
+        {
+            CLIColorizer izer(color);
+            std::printf("%-20s %-16s", local_state_name, local_state_value);
+        }
+        render_next_event_counter();
+    };
+
+    const auto render_top_int = [&](const char* local_state_name, long long local_state_value, CLIColor color)
+    {
+        char buf[21];
+        std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value);
+        render_top_str(local_state_name, buf, color);
+    };
+
+    const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s)
+    {
+        switch (s)
+        {
+        case RaftCore::ServerStateFollower:  return "Follower";
+        case RaftCore::ServerStateCandidate: return "Candidate";
+        case RaftCore::ServerStateLeader:    return "Leader";
+        default:                             return "BADSTATE";
+        }
+    };
+
+    const auto duration_to_string = [](uavcan::MonotonicDuration dur)
+    {
+        uavcan::MakeString<16>::Type str;                       // This is much faster than std::string
+        str.appendFormatted("%.1f", dur.toUSec() / 1e6);
+        return str;
+    };
+
+    const auto colorize_if = [](bool condition, CLIColor color)
+    {
+        return condition ? color : CLIColor::Default;
+    };
+
+    /*
+     * Rendering the data to the CLI
+     */
+    std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning
+    std::printf("\x1b[H");  // Move cursor to the coordinates 1,1
+
+    // Local state and relevant event counters - two columns
+    std::printf("        Local state                   |         Event counters\n");
+
+    render_top_int("Node ID",
+                   node->getNodeID().get(),
+                   CLIColor::Default);
+
+    render_top_str("State",
+                   raft_state_to_string(report.state),
+                   (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta :
+                       (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green :
+                           CLIColor::Default);
+
+    render_top_int("Last log index",
+                   report.last_log_index,
+                   CLIColor::Default);
+
+    render_top_int("Commit index",
+                   report.commit_index,
+                   colorize_if(report.commit_index != report.last_log_index, CLIColor::Magenta));
+
+    render_top_int("Last log term",
+                   report.last_log_term,
+                   CLIColor::Default);
+
+    render_top_int("Current term",
+                   report.current_term,
+                   CLIColor::Default);
+
+    render_top_int("Voted for",
+                   report.voted_for.get(),
+                   CLIColor::Default);
+
+    render_top_str("Since activity",
+                   duration_to_string(time_since_last_activity).c_str(),
+                   CLIColor::Default);
+
+    render_top_str("Random timeout",
+                   duration_to_string(report.randomized_timeout).c_str(),
+                   CLIColor::Default);
+
+    render_top_int("Unknown nodes",
+                   report.num_unknown_nodes,
+                   colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta));
+
+    render_top_int("Node failures",
+                   node->getInternalFailureCount(),
+                   colorize_if(node->getInternalFailureCount() != 0, CLIColor::Magenta));
+
+    // Empty line before the next block
+    std::printf("                                     ");
+    render_next_event_counter();
+
+    // Followers block
+    std::printf("        Followers                    ");
+    render_next_event_counter();
+
+    const auto render_followers_state = [&](const char* name,
+                                            const std::function<int (std::uint8_t)> value_getter,
+                                            const std::function<CLIColor (std::uint8_t)> color_getter)
+    {
+        std::printf("%-17s", name);
+        for (std::uint8_t i = 0; i < 4; i++)
+        {
+            if (i < (report.cluster_size - 1))
+            {
+                CLIColorizer colorizer(color_getter(i));
+                const auto value = value_getter(i);
+                if (value >= 0)
+                {
+                    std::printf("%-5d", value);
+                }
+                else
+                {
+                    std::printf("N/A  ");
+                }
+            }
+            else
+            {
+                std::printf("     ");
+            }
+        }
+        render_next_event_counter();
+    };
+
+    const auto follower_color_getter = [&](std::uint8_t i)
+    {
+        if (report.state != RaftCore::ServerStateLeader)              { return CLIColor::Default; }
+        if (!report.followers[i].node_id.isValid())                   { return CLIColor::Red; }
+        if (report.followers[i].match_index != report.last_log_index ||
+            report.followers[i].next_index <= report.last_log_index)
+        {
+            return CLIColor::Magenta;
+        }
+        return CLIColor::Default;
+    };
+
+    render_followers_state("Node ID", [&](std::uint8_t i)
+                           {
+                               const auto nid = report.followers[i].node_id;
+                               return nid.isValid() ? nid.get() : -1;
+                           },
+                           follower_color_getter);
+
+    render_followers_state("Next index",
+                           [&](std::uint8_t i) { return report.followers[i].next_index; },
+                           follower_color_getter);
+
+    render_followers_state("Match index",
+                           [&](std::uint8_t i) { return report.followers[i].match_index; },
+                           follower_color_getter);
+
+    assert(next_relevant_event_index == NumRelevantEvents);     // Ensuring that all events can be printed
+
+    // Separator
+    std::printf("--------------------------------------+----------------------------------------\n");
+
+    // Event log
+    std::printf("%s\n", EventTracer::RecentEvent::getTableHeader());
+    const int num_events_to_render = static_cast<int>(num_rows) -
+                                     static_cast<int>(next_relevant_event_index) -
+                                     static_cast<int>(NumRowsWithoutEvents) -
+                                     1; // This allows to keep the last line empty for stdout or UAVCAN_TRACE()
+    for (int i = 0;
+         i < num_events_to_render && i < static_cast<int>(event_tracer.getNumEvents());
+         i++)
+    {
+        const auto e = event_tracer.getEventByIndex(i);
+        CLIColorizer colorizer(getColorHash(static_cast<unsigned>(e.code)));
+        std::printf("%s\n", e.toString().c_str());
+    }
+
+    std::fflush(stdout);
+}
+
+
+void runForever(const uavcan_linux::NodePtr& node,
+                const std::uint8_t cluster_size,
+                const std::string& event_log_file,
+                const std::string& persistent_storage_path)
+{
+    /*
+     * Event tracer
+     */
+    EventTracer event_tracer(MaxNumLastEvents);
+    ENFORCE(0 <= event_tracer.init(event_log_file.c_str()));
+
+    /*
+     * Storage backend
+     */
+    uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
+    ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str()));
+
+    /*
+     * Server
+     */
+    uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer);
+
+    const int server_init_res = server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size);
+    if (server_init_res < 0)
+    {
+        throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res));
+    }
+
+    /*
+     * Preparing the CLI
+     */
+    std::printf("\x1b[2J"); // Clear entire screen; this will preserve initialization output in the scrollback
+
+    /*
+     * Spinning the node
+     */
+    uavcan::MonotonicTime last_redraw_at;
+
+    while (true)
+    {
+        const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval));
+        if (res < 0)
+        {
+            std::cerr << "Spin error: " << res << std::endl;
+        }
+
+        const auto ts = node->getMonotonicTime();
+
+        if (event_tracer.hadEvents() || (ts - last_redraw_at).toMSec() > 1000)
+        {
+            last_redraw_at = ts;
+            redraw(node, ts, event_tracer, server);
+        }
+    }
+}
+
+struct Options
+{
+    uavcan::NodeID node_id;
+    std::vector<std::string> ifaces;
+    std::uint8_t cluster_size = 0;
+    std::string storage_path;
+};
+
+Options parseOptions(int argc, const char** argv)
+{
+    const char* const executable_name = *argv++;
+    argc--;
+
+    const auto enforce = [executable_name](bool condition, const char* error_text) {
+        if (!condition)
+        {
+            std::cerr << error_text << "\n"
+                      << "Usage:\n\t"
+                      << executable_name
+                      << " <node-id> <can-iface-name-1> [can-iface-name-N...] [-c <cluster-size>] -s <storage-path>]"
+                      << std::endl;
+            std::exit(1);
+        }
+    };
+
+    enforce(argc >= 3, "Not enough arguments");
+
+    /*
+     * Node ID is always at the first position
+     */
+    argc--;
+    const int node_id = std::stoi(*argv++);
+    enforce(node_id >= 1 && node_id <= 127, "Invalid node ID");
+
+    Options out;
+    out.node_id = uavcan::NodeID(std::uint8_t(node_id));
+
+    while (argc --> 0)
+    {
+        const std::string token(*argv++);
+
+        if (token[0] != '-')
+        {
+            out.ifaces.push_back(token);
+        }
+        else if (token[1] == 'c')
+        {
+            int cluster_size = 0;
+            if (token.length() > 2)     // -c2
+            {
+                cluster_size = std::stoi(token.c_str() + 2);
+            }
+            else                        // -c 2
+            {
+                enforce(argc --> 0, "Expected cluster size");
+                cluster_size = std::stoi(*argv++);
+            }
+            enforce(cluster_size >= 1 &&
+                    cluster_size <= uavcan::dynamic_node_id_server::distributed::ClusterManager::MaxClusterSize,
+                    "Invalid cluster size");
+            out.cluster_size = std::uint8_t(cluster_size);
+        }
+        else if (token[1] == 's')
+        {
+            if (token.length() > 2)     // -s/foo/bar
+            {
+                out.storage_path = token.c_str() + 2;
+            }
+            else                        // -s /foo/bar
+            {
+                enforce(argc --> 0, "Expected storage path");
+                out.storage_path = *argv++;
+            }
+        }
+        else
+        {
+            enforce(false, "Unexpected argument");
+        }
+    }
+
+    enforce(!out.storage_path.empty(), "Invalid storage path");
+
+    return out;
+}
+
+}
+
+int main(int argc, const char** argv)
+{
+    try
+    {
+        std::srand(std::time(nullptr));
+
+        if (isatty(STDOUT_FILENO) != 1)
+        {
+            std::cerr << "This application cannot run if stdout is not associated with a terminal" << std::endl;
+            std::exit(1);
+        }
+
+        auto options = parseOptions(argc, argv);
+
+        std::cout << "Self node ID: " << int(options.node_id.get()) << "\n"
+                     "Cluster size: " << int(options.cluster_size) << "\n"
+                     "Storage path: " << options.storage_path << "\n"
+                     "Num ifaces:   " << options.ifaces.size() << "\n"
+#ifdef NDEBUG
+                     "Build mode:   Release"
+#else
+                     "Build mode:   Debug"
+#endif
+                     << std::endl;
+
+        /*
+         * Preparing the storage directory
+         */
+        options.storage_path += "/node_" + std::to_string(options.node_id.get());
+
+        (void)std::system(("mkdir -p '" + options.storage_path + "' &>/dev/null").c_str());
+
+        const auto event_log_file = options.storage_path + "/events.log";
+        const auto storage_path   = options.storage_path + "/storage/";
+
+        /*
+         * Starting the node
+         */
+        auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_app.dynamic_node_id_server");
+        runForever(node, options.cluster_size, event_log_file, storage_path);
+        return 0;
+    }
+    catch (const std::exception& ex)
+    {
+        std::cerr << "Error: " << ex.what() << std::endl;
+        return 1;
+    }
+}

libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp

@@ -94,7 +94,8 @@ class Monitor : public uavcan::NodeStatusMonitor
 
     void redraw(const uavcan::TimerEvent&)
     {
-        std::cout << "\x1b\x5b\x48" << "\x1b\x5b\x32\x4a"
+        std::cout << "\x1b[1J"   // Clear screen from the current cursor position to the beginning
+                  << "\x1b[H"    // Move cursor to the coordinates 1,1
                   << " NID | Status        | Uptime (sec) | Vendor-specific status (hex/bin/dec)\n"
                   << "-----+---------------+--------------+--------------------------------------\n";
         for (unsigned i = 1; i <= uavcan::NodeID::Max; i++)

libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp

@@ -42,7 +42,7 @@ class BlockingServiceClient : public uavcan::ServiceClient<DataType>
 
     void callback(const uavcan::ServiceCallResult<DataType>& res)
     {
-        response_ = res.response;
+        response_ = res.getResponse();
         call_was_successful_ = res.isSuccessful();
     }
 
@@ -85,7 +85,7 @@ public:
         const int call_res = Super::call(server_node_id, request);
         if (call_res >= 0)
         {
-            while (Super::isPending())
+            while (Super::hasPendingCalls())
             {
                 const int spin_res = Super::getNode().spin(SpinDuration);
                 if (spin_res < 0)

libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp

@@ -14,6 +14,7 @@
 #include <algorithm>
 #include <utility>
 #include <uavcan_linux/exception.hpp>
+#include <uavcan/data_type.hpp>
 
 namespace uavcan_linux
 {
@@ -132,5 +133,45 @@ public:
     }
 };
 
+/**
+ * This class computes unique ID for a UAVCAN node in a Linux application.
+ * It takes the following inputs:
+ *  - Unique machine ID
+ *  - Node name string (e.g. "org.uavcan.linux_app.dynamic_node_id_server")
+ *  - Instance ID byte, e.g. node ID
+ */
+std::array<std::uint8_t, 16> makeApplicationID(const MachineIDReader::MachineID& machine_id,
+                                               const std::string& node_name,
+                                               const std::uint8_t instance_id)
+{
+    union HalfID
+    {
+        std::uint64_t num;
+        std::uint8_t bytes[8];
+
+        HalfID(std::uint64_t arg_num) : num(arg_num) { }
+    };
+
+    std::array<std::uint8_t, 16> out;
+
+    // First 8 bytes of the application ID are CRC64 of the machine ID in native byte order
+    {
+        uavcan::DataTypeSignatureCRC crc;
+        crc.add(machine_id.data(), static_cast<unsigned>(machine_id.size()));
+        HalfID half(crc.get());
+        std::copy_n(half.bytes, 8, out.begin());
+    }
+
+    // Last 8 bytes of the application ID are CRC64 of the node name and optionally node ID
+    {
+        uavcan::DataTypeSignatureCRC crc;
+        crc.add(reinterpret_cast<const std::uint8_t*>(node_name.c_str()), static_cast<unsigned>(node_name.length()));
+        crc.add(instance_id);
+        HalfID half(crc.get());
+        std::copy_n(half.bytes, 8, out.begin() + 8);
+    }
+
+    return out;
+}
 
 }

libuavcan_drivers/posix/CMakeLists.txt

@@ -0,0 +1,12 @@
+#
+# Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+
+cmake_minimum_required(VERSION 2.8)
+
+project(libuavcan_posix)
+
+#
+# Library (header only)
+#
+install(DIRECTORY include/uavcan_posix DESTINATION include)

libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp

@@ -0,0 +1,154 @@
+/****************************************************************************
+*
+*   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+*      Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+*              David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED
+#define UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED
+
+#include <sys/stat.h>
+#include <cstdio>
+#include <cstddef>
+#include <cstdlib>
+#include <cstring>
+#include <cerrno>
+#include <unistd.h>
+#include <fcntl.h>
+
+#include <uavcan/data_type.hpp>
+#include <uavcan/protocol/file/Error.hpp>
+#include <uavcan/protocol/file/EntryType.hpp>
+#include <uavcan/protocol/file/Read.hpp>
+#include <uavcan/protocol/file_server.hpp>
+#include <uavcan/data_type.hpp>
+
+namespace uavcan_posix
+{
+/**
+ * This interface implements a POSIX compliant IFileServerBackend interface
+ */
+class BasicFileSeverBackend : public uavcan::IFileServerBackend
+{
+    enum { FilePermissions = 438 };   ///< 0o666
+
+protected:
+    /**
+     * Back-end for uavcan.protocol.file.GetInfo.
+     * Implementation of this method is required.
+     * On success the method must return zero.
+     */
+    virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type)
+    {
+        int rv = uavcan::protocol::file::Error::INVALID_VALUE;
+        FileCRC crc;
+        if (path.size() > 0)
+        {
+            using namespace std;
+            out_size = 0;
+            out_crc64 = 0;
+
+            rv = -ENOENT;
+            uint8_t buffer[512];
+
+            int fd = ::open(path.c_str(), O_RDONLY);
+
+            if (fd >= 0)
+            {
+                int len = 0;
+
+                do
+                {
+
+                    len = ::read(fd, buffer, sizeof(buffer));
+
+                    if (len > 0)
+                    {
+
+                        out_size += len;
+                        crc.add(buffer, len);
+
+                    }
+                    else if (len < 0)
+                    {
+                        rv = EIO;
+                        goto out_close;
+                    }
+
+                }
+                while(len > 0);
+
+                out_crc64 = crc.get();
+
+                // We can assume the path is to a file and the file is readable.
+                out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE |
+                                 uavcan::protocol::file::EntryType::FLAG_FILE;
+
+                // TODO Using fixed flag FLAG_READABLE until we add file permission checks to return actual value.
+                // TODO Check whether the object pointed by path is a file or a directory
+                // On could ad call to stat() to determine if the path is to a file or a directory but the
+                // what are the return parameters in this case?
+
+                rv = 0;
+            out_close:
+                close(fd);
+            }
+        }
+        return rv;
+    }
+
+    /**
+     * Back-end for uavcan.protocol.file.Read.
+     * Implementation of this method is required.
+     * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except
+     * if the end of file is reached.
+     * On success the method must return zero.
+     */
+
+    virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size)
+    {
+        int rv = uavcan::protocol::file::Error::INVALID_VALUE;
+
+        if (path.size() > 0)
+        {
+            int fd = open(path.c_str(), O_RDONLY);
+
+            if (fd < 0)
+            {
+                rv = errno;
+            }
+            else
+            {
+                if (::lseek(fd, offset, SEEK_SET) < 0)
+                {
+                    rv = errno;
+                }
+                else
+                {
+                    // TODO use a read at offset to fill on EAGAIN
+                    ssize_t len = ::read(fd, out_buffer, inout_size);
+
+                    if (len < 0)
+                    {
+                        rv = errno;
+                    }
+                    else
+                    {
+
+                        inout_size = len;
+                        rv = 0;
+                    }
+                }
+                (void)close(fd);
+            }
+        }
+        return rv;
+    }
+
+};
+
+}
+
+#endif // Include guard

libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp

@@ -0,0 +1,89 @@
+/****************************************************************************
+*
+*   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+*      Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+*              David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED
+#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED
+
+#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
+#include <cstdio>
+#include <time.h>
+#include <fcntl.h>
+#include <unistd.h>
+
+namespace uavcan_posix
+{
+namespace dynamic_node_id_server
+{
+/**
+ * This interface implements a POSIX compliant file based IEventTracer interface
+ */
+class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer
+{
+    /**
+     * Maximum length of full path to log file
+     */
+    enum { MaxPathLength = 128 };
+
+    enum { FilePermissions = 438 };     ///< 0o666
+
+    /**
+     * This type is used for the path
+     */
+    typedef uavcan::MakeString<MaxPathLength>::Type PathString;
+
+    PathString path_;
+
+protected:
+    virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument)
+    {
+        using namespace std;
+
+        timespec ts = timespec();               // If clock_gettime() fails, zero time will be used
+        (void)clock_gettime(CLOCK_REALTIME, &ts);
+
+        int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions);
+        if (fd >= 0)
+        {
+            const int FormatBufferLength = 63;
+            char buffer[FormatBufferLength + 1];
+            int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n",
+                             static_cast<long>(ts.tv_sec), static_cast<long>(ts.tv_nsec / 1000L),
+                             static_cast<int>(code), static_cast<long long>(argument));
+            (void)write(fd, buffer, n); // TODO FIXME Write loop
+            (void)close(fd);
+        }
+    }
+
+public:
+    /**
+     * Initializes the file based event tracer.
+     */
+    int init(const PathString & path)
+    {
+        using namespace std;
+
+        int rv = -uavcan::ErrInvalidParam;
+
+        if (path.size() > 0)
+        {
+            rv = 0;
+            path_ = path.c_str();
+            int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions);
+            if (fd >= 0)
+            {
+                (void)close(fd);
+            }
+        }
+        return rv;
+    }
+};
+
+}
+}
+
+#endif // Include guard

libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp

@@ -6,15 +6,17 @@
 *
 ****************************************************************************/
 
-#pragma once
+#ifndef UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED
+#define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_STORAGE_BACKEND_HPP_INCLUDED
 
 #include <sys/stat.h>
-#include <stdio.h>
+#include <cstdio>
 #include <cstddef>
 #include <cstdlib>
-#include <cfcntl>
 #include <cstring>
 #include <cerrno>
+#include <unistd.h>
+#include <fcntl.h>
 
 #include <uavcan/protocol/dynamic_node_id_server/storage_backend.hpp>
 
@@ -30,24 +32,18 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken
     /**
      * Maximum length of full path including / and key max
      */
-
     enum { MaxPathLength = 128 };
 
+    enum { FilePermissions = 438 };     ///< 0o666
 
     /**
      * This type is used for the path
      */
-    typedef uavcan::Array<uavcan::IntegerSpec<8, uavcan::SignednessUnsigned, uavcan::CastModeTruncate>,
-                          uavcan::ArrayModeDynamic, MaxPathLength> PathString;
-
+    typedef uavcan::MakeString<MaxPathLength>::Type PathString;
 
     PathString base_path;
 
-public:
-
-    FileStorageBackend() { }
-
-
+protected:
     virtual String get(const String& key) const
     {
         using namespace std;
@@ -58,9 +54,9 @@ public:
         if (fd >= 0)
         {
             char buffer[MaxStringLength + 1];
-            memset(buffer, 0, sizeof(buffer));
+            (void)memset(buffer, 0, sizeof(buffer));
             int len = read(fd, buffer, MaxStringLength);
-            close(fd);
+            (void)close(fd);
             if (len > 0)
             {
                 for (int i = 0; i < len; i++)
@@ -75,7 +71,6 @@ public:
             }
         }
         return value;
-
     }
 
     virtual void set(const String& key, const String& value)
@@ -83,24 +78,24 @@ public:
         using namespace std;
         PathString path = base_path.c_str();
         path += key;
-        int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC);
+        int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions);
         if (fd >= 0)
         {
-            write(fd, value.c_str(), value.size());
-            close(fd);
+            (void)write(fd, value.c_str(), value.size());       // TODO FIXME Write loop
+            (void)fsync(fd);
+            (void)close(fd);
         }
     }
 
+public:
     /**
-     * Initializes the File based back end storage by passing to a path to
+     * Initializes the file based backend storage by passing a path to
      * the directory where the key named files will be stored.
-     * This the return result should be 0 on success.
+     * The return value should be 0 on success.
      * If it is -ErrInvalidConfiguration then the the path name is too long to
-     * Accommodate the trailing slash and max key length;
-     *
+     * accommodate the trailing slash and max key length.
      */
-
-    int init(const PathString & path)
+    int init(const PathString& path)
     {
         using namespace std;
 
@@ -108,7 +103,6 @@ public:
 
         if (path.size() > 0)
         {
-
             base_path = path.c_str();
 
             if (base_path.back() == '/')
@@ -132,10 +126,10 @@ public:
             }
         }
         return rv;
-
     }
-
 };
 
 }
 }
+
+#endif // Include guard

libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp

@@ -0,0 +1,424 @@
+/****************************************************************************
+*
+*   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+*      Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+*              David Sidrane <david_s5@usa.net>
+*
+****************************************************************************/
+
+#ifndef UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED
+#define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED
+
+#include <cstdint>
+#include <cstdio>
+#include <cstring>
+#include <fcntl.h>
+#include <cerrno>
+#include <dirent.h>
+
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+
+#if !defined(DIRENT_ISFILE) && defined(DT_REG)
+# define DIRENT_ISFILE(dtype)  ((dtype) == DT_REG)
+#endif
+
+namespace uavcan_posix
+{
+/**
+ * Firmware version checking logic.
+ * Refer to @ref FirmwareUpdateTrigger for details.
+ */
+class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
+{
+    enum { FilePermissions = 438 }; ///< 0o666
+
+    enum { MaxBasePathLength = 128 };
+
+    /**
+     * This type is used for the base path
+     */
+    typedef uavcan::MakeString<MaxBasePathLength>::Type BasePathString;
+
+    /**
+     * Maximum length of full path including / the file name
+     */
+    enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength };
+
+    /**
+     * This type is used internally for the full path to file
+     */
+    typedef uavcan::MakeString<MaxPathLength>::Type PathString;
+
+
+    BasePathString base_path_;
+    BasePathString cache_path_;
+
+    /**
+     * The folder where the files will be copied and read from
+     */
+    static const char* getCacheDir() { return "c"; }
+
+    static void addSlash(BasePathString& path)
+    {
+        if (path.back() != getPathSeparator())
+        {
+            path.push_back(getPathSeparator());
+        }
+    }
+
+    static void removeSlash(BasePathString& path)
+    {
+        if (path.back() == getPathSeparator())
+        {
+            path.pop_back();
+        }
+    }
+
+    void setFirmwareBasePath(const char* path)
+    {
+        base_path_ = path;
+    }
+
+    void setFirmwareCachePath(const char* path)
+    {
+        cache_path_ = path;
+    }
+
+    int copyIfNot(const char* srcpath, const char* destpath)
+    {
+        // Does the file exist
+        int rv = 0;
+        int dfd = open(destpath, O_RDONLY, 0);
+
+        if (dfd >= 0)
+        {
+            // Close it and exit 0
+            (void)close(dfd);
+        }
+        else
+        {
+            uint8_t buffer[512];
+
+            dfd = open(destpath, O_WRONLY | O_CREAT, FilePermissions);
+            if (dfd < 0)
+            {
+                rv = -errno;
+            }
+            else
+            {
+                int sfd = open(srcpath, O_RDONLY, 0);
+                if (sfd < 0)
+                {
+                    rv = -errno;
+                }
+                else
+                {
+                    ssize_t size;
+                    do
+                    {
+                        size = ::read(sfd, buffer, sizeof(buffer));
+                        if (size != 0)
+                        {
+                            if (size < 0)
+                            {
+
+                                rv = -errno;
+
+                            }
+                            else
+                            {
+
+                                if (size != write(dfd, buffer, size))
+                                {
+                                    rv = -errno;
+                                }
+                            }
+                        }
+                    }
+                    while (rv == 0 && size);
+
+                    (void)close(sfd);
+                }
+                (void)close(dfd);
+            }
+        }
+        return rv;
+    }
+
+    struct AppDescriptor
+    {
+        uint8_t signature[sizeof(uavcan::uint64_t)];
+        uint64_t image_crc;
+        uint32_t image_size;
+        uint32_t vcs_commit;
+        uint8_t major_version;
+        uint8_t minor_version;
+        uint8_t reserved[6];
+    };
+
+
+    static int getFileInfo(const char* path, AppDescriptor & descriptor)
+    {
+        using namespace std;
+
+        const unsigned MaxChunk = 512 / sizeof(uint64_t);
+
+        uint64_t signature = 0;
+        std::memcpy(&signature, "APDesc00", 8);
+
+
+        int rv = -ENOENT;
+        uint64_t chunk[MaxChunk];
+        int fd = open(path, O_RDONLY);
+
+        if (fd >= 0)
+        {
+            AppDescriptor* pdescriptor = NULL;
+
+            while (pdescriptor == NULL)
+            {
+                int len = read(fd, chunk, sizeof(chunk));
+
+                if (len == 0)
+                {
+                    break;
+                }
+
+                if (len < 0)
+                {
+                    rv = -errno;
+                    goto out_close;
+                }
+
+                uint64_t* p = &chunk[0];
+
+                do
+                {
+                    if (*p == signature)
+                    {
+                        pdescriptor = (AppDescriptor*) p;
+                        descriptor = *pdescriptor;
+                        rv = 0;
+                        break;
+                    }
+                }
+                while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]);
+            }
+
+        out_close:
+            (void)close(fd);
+        }
+        return rv;
+    }
+
+public:
+
+    const BasePathString& getFirmwareBasePath() const { return base_path_; }
+
+    const BasePathString& getFirmwareCachePath() const { return cache_path_; }
+
+    static char getPathSeparator()
+    {
+        return static_cast<char>(uavcan::protocol::file::Path::SEPARATOR);
+    }
+
+    /**
+     * Creates the Directories were the files will be stored
+     *
+     * This is directory structure is in support of a workaround
+     * for the issues that FirmwareFilePath is 40
+     *
+     *  It creates a path structure:
+     *    +---(base_path)
+     *        +-c                           <----------- Files are cached here.
+     */
+    int createFwPaths(const char* base_path)
+    {
+        using namespace std;
+        int rv = -uavcan::ErrInvalidParam;
+
+        if (base_path)
+        {
+            const int len = strlen(base_path);
+
+            if (len > 0 && len < base_path_.MaxSize)
+            {
+                setFirmwareBasePath(base_path);
+                removeSlash(base_path_);
+                const char* path = getFirmwareBasePath().c_str();
+
+                setFirmwareCachePath(path);
+                addSlash(cache_path_);
+                cache_path_ += getCacheDir();
+
+                rv = 0;
+                struct stat sb;
+                if (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))
+                {
+                    rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO);
+                }
+
+                path = getFirmwareCachePath().c_str();
+
+                if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode)))
+                {
+                    rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO);
+                }
+
+                addSlash(base_path_);
+                addSlash(cache_path_);
+
+                if (rv >= 0)
+                {
+                    if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) >
+                        MaxPathLength)
+                    {
+                        rv = -uavcan::ErrInvalidConfiguration;
+                    }
+                }
+            }
+        }
+        return rv;
+    }
+
+
+
+protected:
+    /**
+     * This method will be invoked when the class obtains a response to GetNodeInfo request.
+     *
+     * @param node_id                   Node ID that this GetNodeInfo response was received from.
+     *
+     * @param node_info                 Actual node info structure; refer to uavcan.protocol.GetNodeInfo for details.
+     *
+     * @param out_firmware_file_path    The implementation should return the firmware image path via this argument.
+     *                                  Note that this path must be reachable via uavcan.protocol.file.Read service.
+     *                                  Refer to @ref FileServer and @ref BasicFileServer for details.
+     *
+     * @return                          True - the class will begin sending update requests.
+     *                                  False - the node will be ignored, no request will be sent.
+     */
+
+    virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id,
+                                             const uavcan::protocol::GetNodeInfo::Response& node_info,
+                                             FirmwareFilePath& out_firmware_file_path)
+    {
+        using namespace std;
+
+        /* This is a work  around for two issues.
+         *  1) FirmwareFilePath is 40
+         *  2) OK using is using 32 for max file names.
+         *
+         *  So for the file:
+         *    org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin
+         *    +---fw
+         *        +-c                           <----------- Files are cashed here.
+         *          +--- 59efc137.bin           <----------  A unknown Firmware file
+         *        +---org.pixhawk.px4cannode-v1 <---------- node_info.name
+         *        +---1.0 <-------------------------------- node_info.name's hardware_version.major,minor
+         *            + - 59efc137.bin          <----------- A well known file must match the name
+         *                                                   in the root fw folder, so if it does not exist
+         *                                                   it is copied up
+         */
+        bool rv = false;
+
+        char fname_root[MaxBasePathLength + 1];
+        int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d",
+                         getFirmwareBasePath().c_str(),
+                         node_info.name.c_str(),
+                         node_info.hardware_version.major,
+                         node_info.hardware_version.minor);
+
+        if (n > 0 && n < (int) sizeof(fname_root) - 2)
+        {
+            DIR* const fwdir = opendir(fname_root);
+
+            fname_root[n++] = getPathSeparator();
+            fname_root[n++] = '\0';
+
+            if (fwdir != NULL)
+            {
+                struct dirent* pfile = NULL;
+                while ((pfile = readdir(fwdir)) != NULL)
+                {
+                    // TODO: This is not POSIX compliant
+                    if (DIRENT_ISFILE(pfile->d_type))
+                    {
+                        // Open any bin file in there.
+                        if (strstr(pfile->d_name, ".bin") != NULL)
+                        {
+                            PathString full_src_path = fname_root;
+                            full_src_path += pfile->d_name;
+
+                            PathString full_dst_path = getFirmwareCachePath().c_str();
+                            full_dst_path += pfile->d_name;
+
+                            // ease the burden on the user
+                            int cr = copyIfNot(full_src_path.c_str(), full_dst_path.c_str());
+
+                            // We have a file, is it a valid image
+                            AppDescriptor descriptor;
+
+                            std::memset(&descriptor, 0, sizeof(descriptor));
+
+                            if (cr == 0 && getFileInfo(full_dst_path.c_str(), descriptor) == 0)
+                            {
+                                volatile AppDescriptor descriptorC = descriptor;
+                                descriptorC.reserved[1]++;
+
+                                if (node_info.software_version.image_crc == 0 ||
+                                    (node_info.software_version.major == 0 && node_info.software_version.minor == 0) ||
+                                    descriptor.image_crc != node_info.software_version.image_crc)
+                                {
+                                    rv = true;
+                                    out_firmware_file_path = pfile->d_name;
+                                }
+                                break;
+                            }
+                        }
+                    }
+                }
+                (void)closedir(fwdir);
+            }
+        }
+        return rv;
+    }
+
+    /**
+     * This method will be invoked when a node responds to the update request with an error. If the request simply
+     * times out, this method will not be invoked.
+     * Note that if by the time of arrival of the response the node is already removed, this method will not be called.
+     *
+     * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting
+     *               is not needed anymore. This method will not be invoked.
+     *
+     * @param node_id                   Node ID that returned this error.
+     *
+     * @param error_response            Contents of the error response. It contains error code and text.
+     *
+     * @param out_firmware_file_path    New firmware path if a retry is needed. Note that this argument will be
+     *                                  initialized with old path, so if the same path needs to be reused, this
+     *                                  argument should be left unchanged.
+     *
+     * @return                          True - the class will continue sending update requests with new firmware path.
+     *                                  False - the node will be forgotten, new requests will not be sent.
+     */
+    virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID,
+                                           const uavcan::protocol::file::BeginFirmwareUpdate::Response&,
+                                           FirmwareFilePath&)
+    {
+        // TODO: Limit the number of attempts per node
+        return true;
+    }
+
+public:
+
+    const char* getFirmwarePath() const
+    {
+        return getFirmwareCachePath().c_str();
+    }
+};
+
+}
+
+#endif // Include guard

libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp

@@ -1,80 +0,0 @@
-/*
- * Copyright (C) 2015 Pavel Kirienko <pavel.kirienko@gmail.com>
- */
-
-#pragma once
-
-#include <uavcan/protocol/dynamic_node_id_server/event.hpp>
-#include <time.h>
-
-namespace uavcan_posix
-{
-namespace dynamic_node_id_server
-{
-/**
- * This interface implements a POSIX compliant file based IEventTracer interface
- */
-class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer
-{
-    /**
-     * Maximum length of full path to log file
-     */
-
-    enum { MaxPathLength = 128, FormatBufferLength = 64 };
-
-
-    /**
-     * This type is used for the path
-     */
-    typedef uavcan::Array<uavcan::IntegerSpec<8, uavcan::SignednessUnsigned, uavcan::CastModeTruncate>,
-                          uavcan::ArrayModeDynamic, MaxPathLength> PathString;
-
-
-    PathString path_;
-
-
-public:
-
-    FileEventTracer() { }
-
-    virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument)
-    {
-        struct timespec ts;
-        clock_gettime(CLOCK_REALTIME, &ts);
-        int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND);
-        if (fd >= 0 )
-        {
-            char buffer[FormatBufferLength + 1];
-            int n =   snprintf(buffer, FormatBufferLength, "%d.%ld,%d,%lld\n", ts.tv_sec, ts.tv_nsec, code, argument);
-            write(fd, buffer, n);
-            close(fd);
-        }
-    }
-    /**
-     * Initializes the File based event trace
-     *
-     */
-
-    int init(const PathString & path)
-    {
-        using namespace std;
-
-        int rv = -uavcan::ErrInvalidParam;
-
-        if (path.size() > 0)
-        {
-            rv = 0;
-            path_ = path.c_str();
-            int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC);
-            if ( fd >= 0)
-            {
-                close(fd);
-            }
-        }
-        return rv;
-    }
-
-};
-
-}
-}