From 1017935ba9c13d810f1bd83181bc42a36d7c1562 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 May 2015 10:11:31 +0300 Subject: [PATCH 001/268] Dynamic allocation master - added definitions of Raft types --- .../server/1011.Discovery.uavcan | 20 +++++++++++++++++++ .../server/220.AppendEntries.uavcan | 16 +++++++++++++++ .../server/221.RequestVote.uavcan | 13 ++++++++++++ .../dynamic_node_id/server/Entry.uavcan | 10 ++++++++++ 4 files changed, 59 insertions(+) create mode 100644 dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan create mode 100644 dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan create mode 100644 dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan create mode 100644 dsdl/uavcan/protocol/dynamic_node_id/server/Entry.uavcan diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan new file mode 100644 index 0000000000..33f15dd1fe --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan @@ -0,0 +1,20 @@ +# +# This message is used by allocation servers to find each other's node ID. +# Please refer to the specification for details. +# + +# +# This message should be broadcasted by the server at this interval until all other servers are discovered. +# +uint16 BROADCASTING_INTERVAL_MS = 1000 + +# +# Number of servers in the cluster as configured on the sender. +# +uint8 configured_cluster_size + +# +# Node ID of servers that are known to the publishing server, including the publishing server itself. +# Capacity of this array defines maximum size of the server cluster. +# +uint8[<=7] known_nodes diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan new file mode 100644 index 0000000000..b6d6253bf7 --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -0,0 +1,16 @@ +# +# This type is a part of the Raft consensus algorithm. +# Please refer to the specification for details. +# + +uint32 term +uint8 prev_log_index +uint8 prev_log_term +uint8 leader_commit +# Request message fits one CAN frame when there's no entries to send. +Entry[<=5] entries + +--- + +uint32 term +bool success diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan new file mode 100644 index 0000000000..958be2612e --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan @@ -0,0 +1,13 @@ +# +# This type is a part of the Raft consensus algorithm. +# Please refer to the specification for details. +# + +uint32 term +uint8 last_log_index +uint8 last_log_term + +--- + +uint32 term +bool vote_granted diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/Entry.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/Entry.uavcan new file mode 100644 index 0000000000..34165c4b88 --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/Entry.uavcan @@ -0,0 +1,10 @@ +# +# One dynamic node ID allocation entry. +# This type is a part of the Raft consensus algorithm. +# Please refer to the specification for details. +# + +uint32 term +uint8[16] unique_id +uint7 node_id +bool alignment From 3dcb3c0b573765911aab8ec09fc4830cccd5d3a9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 May 2015 18:06:46 +0300 Subject: [PATCH 002/268] Basic interface of allocation server --- .../dynamic_node_id_allocation_server.hpp | 365 ++++++++++++++++++ 1 file changed, 365 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp new file mode 100644 index 0000000000..bd5d311152 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -0,0 +1,365 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +// Types used by the server +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * + */ +class DynamicNodeIDAllocationServer +{ + /* + * Public type definitions + */ +public: + /** + * This interface is used by the server to read and write stable storage. + * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 + * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward + * and efficient implementation of storage backends, e.g. based on text files. + */ + class IStorageBackend + { + public: + /** + * Maximum length of keys and values. One pair takes twice as much space. + */ + enum { MaxStringLength = 32 }; + + /** + * It is guaranteed that the server will never require more than this number of key/value pairs. + * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. + */ + enum { MaxKeyValuePairs = 400 }; + + /** + * 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, ArrayModeDynamic, MaxStringLength> String; + + /** + * Read one value from the storage. + * If such key does not exist, or if read failed, an empty string will be returned. + * This method should not block for more than 50 ms. + */ + virtual String get(const String& key) const = 0; + + /** + * Create or update value for the given key. + * This method should not block for more than 50 ms. + * Failures will be ignored. + */ + virtual void set(const String& key, const String& value) = 0; + + virtual ~IStorageBackend() { } + }; + +private: + /* + * Callback type definitions + */ + typedef MethodBinder&)> + AllocationCallback; + + typedef MethodBinder&)> NodeStatusCallback; + + typedef MethodBinder AppendEntriesCallback; + + typedef MethodBinder RequestVoteCallback; + + /* + * Internal type definitions + */ + typedef Map PendingGetNodeInfoAttemptsMap; + + typedef StorageType::Type Term; + + /** + * This class extends the storage backend interface with serialization/deserialization functionality. + */ + class StorageMarshallingDecorator + { + IStorageBackend& storage_; + + public: + StorageMarshallingDecorator(IStorageBackend& storage) + : storage_(storage) + { + // Making sure that there will be no data loss during serialization. + StaticAssert<(sizeof(Term) <= sizeof(uint32_t))>::check(); + } + + /** + * Setters do the following: + * 1. Serialize the value. + * 2. Update the value on the backend. + * 3. Read the value back from the backend; return false if read fails. + * 4. Deserealize the newly read value. + * 5. Update the argument with deserialized value. + * 6. Return true. + * The caller then is supposed to check whether the argument has the desired value. + */ + bool setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value); + bool setAndGetBack(const IStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value); + + /** + * Getters simply read and deserialize the value. + */ + bool get(const IStorageBackend::String& key, uint32_t& out_value) const; + bool get(const IStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const; + }; + + /** + * Raft log. + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ + class Log + { + public: + typedef uint8_t Index; + + private: + enum { Capacity = NodeID::Max + 1 }; + + IStorageBackend& storage_; + protocol::dynamic_node_id::server::Entry entries_[Capacity]; + Index max_index_; // Index zero always contains an empty entry + + public: + Log(IStorageBackend& storage) + : storage_(storage) + , max_index_(0) + { } + + /** + * This method invokes storage IO. + */ + int init(); + + /** + * This method invokes storage IO. + */ + void append(const protocol::dynamic_node_id::server::Entry& entry); + + /** + * This method invokes storage IO. + */ + void removeEntriesWhereIndexGreaterOrEqual(Index index); + + /** + * Returns nullptr if there's no such index. + * This method does not use storage IO. + */ + const protocol::dynamic_node_id::server::Entry* getEntryAtIndex(Index index) const; + + Index getMaxIndex() const { return max_index_; } + + bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const; + }; + + /** + * This class is a convenient container for persistent state variables defined by Raft. + * Writes are slow, reads are instantaneous. + */ + class PersistentState + { + IStorageBackend& storage_; + + Term current_term_; + NodeID voted_for_; + Log log_; + + public: + PersistentState(IStorageBackend& storage) + : storage_(storage) + , current_term_(0) + { } + + int init(); + + Term getCurrentTerm() const { return current_term_; } + + NodeID getVotedFor() const { return voted_for_; } + + Log& getLog() { return log_; } + const Log& getLog() const { return log_; } + + /** + * Invokes storage IO. + */ + void setCurrentTerm(Term term); + + /** + * Invokes storage IO. + */ + void setVotedFor(NodeID node_id); + }; + + /** + * This class maintains the cluster state. + */ + class ClusterManager : private TimerBase + { + typedef MethodBinder&)> + DiscoveryCallback; + + struct Server + { + const NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + Server() + : next_index(0) + , match_index(0) + { } + }; + + enum { MaxServers = protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize }; + + const IStorageBackend& storage_; + const Log& log_; + + Subscriber discovery_sub_; + mutable Publisher discovery_pub_; + + Server servers_[MaxServers - 1]; ///< Minus one because the local server is not listed there. + + uint8_t cluster_size_; + uint8_t num_known_servers_; + + virtual void handleTimerEvent(const TimerEvent& event); + + void handleDiscovery(const ReceivedDataStructure& msg); + + void publishDiscovery() const; + + public: + enum { ClusterSizeUnknown = 0 }; + + /** + * @param node Needed to publish and subscribe to Discovery message + * @param storage Needed to read the cluster size parameter from the storage + * @param log Needed to initialize nextIndex[] values after elections + */ + ClusterManager(INode& node, const IStorageBackend& storage, const Log& log) + : storage_(storage) + , log_(log) + , discovery_sub_(node) + , discovery_pub_(node) + , cluster_size_(0) + , num_known_servers_(0) + { } + + /** + * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the + * storage backend using key 'cluster_size'. + * Returns negative error code. + */ + int init(uint8_t cluster_size = ClusterSizeUnknown); + + /** + * An invalid node ID will be returned if there's no such server. + * The local server is not listed there. + */ + NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const; + + /** + * See next_index[] in Raft paper. + */ + Log::Index getServerNextIndex(NodeID server_node_id) const; + void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment); + void decrementServerNextIndex(NodeID server_node_id); + + /** + * See match_index[] in Raft paper. + */ + Log::Index getServerMatchIndex(NodeID server_node_id) const; + void setServerMatchIndex(NodeID server_node_id, Log::Index match_index); + + /** + * This method must be called when the current server becomes leader. + */ + void resetAllServerIndices(); + + uint8_t getNumKnownServers() const { return num_known_servers_; } + uint8_t getConfiguredClusterSize() const { return cluster_size_; } + uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } + }; + + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + + /* + * Raft-related states + */ + PersistentState persistent_state_; ///< Modifications of this state are slow as they involve storage IO. + + Log::Index commit_index_; + + ClusterManager cluster_; + + /* + * Implementation-specific states + */ + PendingGetNodeInfoAttemptsMap pending_get_node_info_attempts_; + + MonotonicTime last_activity_timestamp_; + bool active_mode_; + + ServerState server_state_; + + /* + * Transport + */ + Subscriber allocation_sub_; + Publisher allocation_pub_; + + Subscriber node_status_sub_; + + ServiceServer append_entries_srv_; + ServiceServer request_vote_srv_; +}; + +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED From 5527faca5decb0037820f74c10f4d3d552dd4f09 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 1 May 2015 18:19:11 +0300 Subject: [PATCH 003/268] Raft server: more internals --- .../dynamic_node_id_allocation_server.hpp | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index bd5d311152..3cc151c7f2 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -97,6 +97,16 @@ private: (const protocol::dynamic_node_id::server::RequestVote::Request&, protocol::dynamic_node_id::server::RequestVote::Response&)> RequestVoteCallback; + typedef MethodBinder&)> + AppendEntriesResponseCallback; + + typedef MethodBinder&)> + RequestVoteResponseCallback; + /* * Internal type definitions */ @@ -358,6 +368,11 @@ private: ServiceServer append_entries_srv_; ServiceServer request_vote_srv_; + + ServiceClient append_entries_client_; + ServiceClient request_vote_client_; }; } From 86675455fb988d4c71ebdcbad51c2f81cfc09188 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 1 May 2015 15:28:06 -1000 Subject: [PATCH 004/268] Added uavcan/debug.hpp to compile --- libuavcan/src/transport/uc_frame.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/src/transport/uc_frame.cpp b/libuavcan/src/transport/uc_frame.cpp index a732adf84d..087893cb9f 100644 --- a/libuavcan/src/transport/uc_frame.cpp +++ b/libuavcan/src/transport/uc_frame.cpp @@ -4,6 +4,7 @@ #include #include +#include #include namespace uavcan From 3e7026ad1912f4866b335ffe3557b321680fa386 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 2 May 2015 11:41:54 +0300 Subject: [PATCH 005/268] Dynamic Node ID allocation server - internal types --- .../dynamic_node_id_allocation_server.hpp | 716 ++++++++++-------- .../dynamic_node_id_allocation_server.cpp | 12 + 2 files changed, 399 insertions(+), 329 deletions(-) create mode 100644 libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 3cc151c7f2..803a6d4894 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -6,8 +6,11 @@ #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED #include +#include #include #include +#include +#include #include #include #include @@ -22,62 +25,394 @@ namespace uavcan { +/** + * This interface is used by the server to read and write stable storage. + * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 + * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward + * and efficient implementation of storage backends, e.g. based on text files. + */ +class IDynamicNodeIDStorageBackend +{ +public: + /** + * Maximum length of keys and values. One pair takes twice as much space. + */ + enum { MaxStringLength = 32 }; + + /** + * It is guaranteed that the server will never require more than this number of key/value pairs. + * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. + */ + enum { MaxKeyValuePairs = 400 }; + + /** + * 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, ArrayModeDynamic, MaxStringLength> String; + + /** + * Read one value from the storage. + * If such key does not exist, or if read failed, an empty string will be returned. + * This method should not block for more than 50 ms. + */ + virtual String get(const String& key) const = 0; + + /** + * Create or update value for the given key. + * This method should not block for more than 50 ms. + * Failures will be ignored. + */ + virtual void set(const String& key, const String& value) = 0; + + virtual ~IDynamicNodeIDStorageBackend() { } +}; + +/** + * Internals, do not use anything from this namespace directly. + */ +namespace dynamic_node_id_server_impl +{ +/** + * Raft term + */ +typedef StorageType::Type Term; + +/** + * This class extends the storage backend interface with serialization/deserialization functionality. + */ +class MarshallingStorageDecorator +{ + IDynamicNodeIDStorageBackend& storage_; + +public: + MarshallingStorageDecorator(IDynamicNodeIDStorageBackend& storage) + : storage_(storage) + { + // Making sure that there will be no data loss during serialization. + StaticAssert<(sizeof(Term) <= sizeof(uint32_t))>::check(); + } + + /** + * Setters do the following: + * 1. Serialize the value. + * 2. Update the value on the backend. + * 3. Read the value back from the backend; return false if read fails. + * 4. Deserealize the newly read value; return false if deserialization fails. + * 5. Update the argument with deserialized value. + * 6. Return true. + * The caller then is supposed to check whether the argument has the desired value. + */ + bool setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value); + bool setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value); + + /** + * Getters simply read and deserialize the value. + */ + bool get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const; + bool get(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const; +}; + +/** + * Raft log. + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ +class Log +{ +public: + typedef uint8_t Index; + +private: + enum { Capacity = NodeID::Max + 1 }; + + IDynamicNodeIDStorageBackend& storage_; + protocol::dynamic_node_id::server::Entry entries_[Capacity]; + Index max_index_; // Index zero always contains an empty entry + +public: + Log(IDynamicNodeIDStorageBackend& storage) + : storage_(storage) + , max_index_(0) + { } + + /** + * This method invokes storage IO. + */ + int init(); + + /** + * This method invokes storage IO. + */ + void append(const protocol::dynamic_node_id::server::Entry& entry); + + /** + * This method invokes storage IO. + */ + void removeEntriesWhereIndexGreaterOrEqual(Index index); + + /** + * Returns nullptr if there's no such index. + * This method does not use storage IO. + */ + const protocol::dynamic_node_id::server::Entry* getEntryAtIndex(Index index) const; + + Index getMaxIndex() const { return max_index_; } + + bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const; +}; + + +/** + * This class is a convenient container for persistent state variables defined by Raft. + * Writes are slow, reads are instantaneous. + */ +class PersistentState +{ + IDynamicNodeIDStorageBackend& storage_; + + Term current_term_; + NodeID voted_for_; + Log log_; + +public: + PersistentState(IDynamicNodeIDStorageBackend& storage) + : storage_(storage) + , current_term_(0) + , log_(storage) + { } + + int init(); + + Term getCurrentTerm() const { return current_term_; } + + NodeID getVotedFor() const { return voted_for_; } + + Log& getLog() { return log_; } + const Log& getLog() const { return log_; } + + /** + * Invokes storage IO. + */ + void setCurrentTerm(Term term); + + /** + * Invokes storage IO. + */ + void setVotedFor(NodeID node_id); +}; + +/** + * This class maintains the cluster state. + */ +class ClusterManager : private TimerBase +{ + typedef MethodBinder&)> + DiscoveryCallback; + + struct Server + { + const NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + Server() + : next_index(0) + , match_index(0) + { } + }; + + enum { MaxServers = protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize }; + + const IDynamicNodeIDStorageBackend& storage_; + const Log& log_; + + Subscriber discovery_sub_; + mutable Publisher discovery_pub_; + + Server servers_[MaxServers - 1]; ///< Minus one because the local server is not listed there. + + uint8_t cluster_size_; + uint8_t num_known_servers_; + + virtual void handleTimerEvent(const TimerEvent&); + + void handleDiscovery(const ReceivedDataStructure& msg); + + void publishDiscovery() const; + +public: + enum { ClusterSizeUnknown = 0 }; + + /** + * @param node Needed to publish and subscribe to Discovery message + * @param storage Needed to read the cluster size parameter from the storage + * @param log Needed to initialize nextIndex[] values after elections + */ + ClusterManager(INode& node, const IDynamicNodeIDStorageBackend& storage, const Log& log) + : TimerBase(node) + , storage_(storage) + , log_(log) + , discovery_sub_(node) + , discovery_pub_(node) + , cluster_size_(0) + , num_known_servers_(0) + { } + + /** + * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the + * storage backend using key 'cluster_size'. + * Returns negative error code. + */ + int init(uint8_t cluster_size = ClusterSizeUnknown); + + /** + * An invalid node ID will be returned if there's no such server. + * The local server is not listed there. + */ + NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const; + + /** + * See next_index[] in Raft paper. + */ + Log::Index getServerNextIndex(NodeID server_node_id) const; + void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment); + void decrementServerNextIndex(NodeID server_node_id); + + /** + * See match_index[] in Raft paper. + */ + Log::Index getServerMatchIndex(NodeID server_node_id) const; + void setServerMatchIndex(NodeID server_node_id, Log::Index match_index); + + /** + * This method must be called when the current server becomes leader. + */ + void resetAllServerIndices(); + + uint8_t getNumKnownServers() const { return num_known_servers_; } + uint8_t getConfiguredClusterSize() const { return cluster_size_; } + uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } +}; + +/** + * This class implements log replication and voting. + * It does not implement client-server interaction at all; instead it just exposes a public method for adding + * allocation entries. + */ +class RaftCore : private TimerBase +{ + typedef MethodBinder AppendEntriesCallback; + + typedef MethodBinder RequestVoteCallback; + + typedef MethodBinder&)> + AppendEntriesResponseCallback; + + typedef MethodBinder&)> + RequestVoteResponseCallback; + + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + + dynamic_node_id_server_impl::PersistentState persistent_state_; + + dynamic_node_id_server_impl::Log::Index commit_index_; + + dynamic_node_id_server_impl::ClusterManager cluster_; + + MonotonicTime last_activity_timestamp_; + bool active_mode_; + + ServerState server_state_; + + ServiceServer append_entries_srv_; + ServiceServer request_vote_srv_; + + ServiceClient append_entries_client_; + ServiceClient request_vote_client_; + + virtual void handleTimerEvent(const TimerEvent&); + +public: + RaftCore(INode& node, IDynamicNodeIDStorageBackend& storage) + : TimerBase(node) + , persistent_state_(storage) + , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero + , cluster_(node, storage, persistent_state_.getLog()) + , last_activity_timestamp_(node.getMonotonicTime()) + , active_mode_(true) + , server_state_(ServerStateFollower) + , append_entries_srv_(node) + , request_vote_srv_(node) + , append_entries_client_(node) + , request_vote_client_(node) + { } + + /** + * Once started, the logic runs in the background until destructor is called. + */ + int init(); + + /** + * Inserts one entry into log. This operation may fail, which will not be reported. + * Failures are tolerble because all operations are idempotent. + */ + void appendLog(const protocol::dynamic_node_id::server::Entry& entry); + + /** + * This method is used by the allocator to query existence of certain entries in the Raft log. + * Predicate is a callable of the following prototype: + * bool (const protocol::dynamic_node_id::server::Entry&) + * Once the predicate returns true, the loop will be terminated and the method will return a pointer to the last + * visited entry; otherwise nullptr will be returned. + * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. + */ + template + const protocol::dynamic_node_id::server::Entry* traverseLogFromEndUntil(const Predicate& predicate) const + { + UAVCAN_ASSERT(try_implicit_cast(predicate, true)); + for (int index = static_cast(persistent_state_.getLog().getMaxIndex()); index--; index >= 0) + { + const protocol::dynamic_node_id::server::Entry* const entry = + persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); + UAVCAN_ASSERT(entry != NULL); + if (predicate(*entry)) + { + return entry; + } + } + return NULL; + } +}; + +} // namespace dynamic_node_id_impl + /** * */ class DynamicNodeIDAllocationServer { - /* - * Public type definitions - */ -public: - /** - * This interface is used by the server to read and write stable storage. - * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 - * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward - * and efficient implementation of storage backends, e.g. based on text files. - */ - class IStorageBackend - { - public: - /** - * Maximum length of keys and values. One pair takes twice as much space. - */ - enum { MaxStringLength = 32 }; - - /** - * It is guaranteed that the server will never require more than this number of key/value pairs. - * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. - */ - enum { MaxKeyValuePairs = 400 }; - - /** - * 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, ArrayModeDynamic, MaxStringLength> String; - - /** - * Read one value from the storage. - * If such key does not exist, or if read failed, an empty string will be returned. - * This method should not block for more than 50 ms. - */ - virtual String get(const String& key) const = 0; - - /** - * Create or update value for the given key. - * This method should not block for more than 50 ms. - * Failures will be ignored. - */ - virtual void set(const String& key, const String& value) = 0; - - virtual ~IStorageBackend() { } - }; - -private: - /* - * Callback type definitions - */ typedef MethodBinder&)> @@ -87,292 +422,15 @@ private: void (DynamicNodeIDAllocationServer::*) (const ReceivedDataStructure&)> NodeStatusCallback; - typedef MethodBinder AppendEntriesCallback; + typedef Map PendingGetNodeInfoAttemptsMap; - typedef MethodBinder RequestVoteCallback; - - typedef MethodBinder&)> - AppendEntriesResponseCallback; - - typedef MethodBinder&)> - RequestVoteResponseCallback; - - /* - * Internal type definitions - */ - typedef Map PendingGetNodeInfoAttemptsMap; - - typedef StorageType::Type Term; - - /** - * This class extends the storage backend interface with serialization/deserialization functionality. - */ - class StorageMarshallingDecorator - { - IStorageBackend& storage_; - - public: - StorageMarshallingDecorator(IStorageBackend& storage) - : storage_(storage) - { - // Making sure that there will be no data loss during serialization. - StaticAssert<(sizeof(Term) <= sizeof(uint32_t))>::check(); - } - - /** - * Setters do the following: - * 1. Serialize the value. - * 2. Update the value on the backend. - * 3. Read the value back from the backend; return false if read fails. - * 4. Deserealize the newly read value. - * 5. Update the argument with deserialized value. - * 6. Return true. - * The caller then is supposed to check whether the argument has the desired value. - */ - bool setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value); - bool setAndGetBack(const IStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value); - - /** - * Getters simply read and deserialize the value. - */ - bool get(const IStorageBackend::String& key, uint32_t& out_value) const; - bool get(const IStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const; - }; - - /** - * Raft log. - * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. - * Writes are slow, reads are instantaneous. - */ - class Log - { - public: - typedef uint8_t Index; - - private: - enum { Capacity = NodeID::Max + 1 }; - - IStorageBackend& storage_; - protocol::dynamic_node_id::server::Entry entries_[Capacity]; - Index max_index_; // Index zero always contains an empty entry - - public: - Log(IStorageBackend& storage) - : storage_(storage) - , max_index_(0) - { } - - /** - * This method invokes storage IO. - */ - int init(); - - /** - * This method invokes storage IO. - */ - void append(const protocol::dynamic_node_id::server::Entry& entry); - - /** - * This method invokes storage IO. - */ - void removeEntriesWhereIndexGreaterOrEqual(Index index); - - /** - * Returns nullptr if there's no such index. - * This method does not use storage IO. - */ - const protocol::dynamic_node_id::server::Entry* getEntryAtIndex(Index index) const; - - Index getMaxIndex() const { return max_index_; } - - bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const; - }; - - /** - * This class is a convenient container for persistent state variables defined by Raft. - * Writes are slow, reads are instantaneous. - */ - class PersistentState - { - IStorageBackend& storage_; - - Term current_term_; - NodeID voted_for_; - Log log_; - - public: - PersistentState(IStorageBackend& storage) - : storage_(storage) - , current_term_(0) - { } - - int init(); - - Term getCurrentTerm() const { return current_term_; } - - NodeID getVotedFor() const { return voted_for_; } - - Log& getLog() { return log_; } - const Log& getLog() const { return log_; } - - /** - * Invokes storage IO. - */ - void setCurrentTerm(Term term); - - /** - * Invokes storage IO. - */ - void setVotedFor(NodeID node_id); - }; - - /** - * This class maintains the cluster state. - */ - class ClusterManager : private TimerBase - { - typedef MethodBinder&)> - DiscoveryCallback; - - struct Server - { - const NodeID node_id; - Log::Index next_index; - Log::Index match_index; - - Server() - : next_index(0) - , match_index(0) - { } - }; - - enum { MaxServers = protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize }; - - const IStorageBackend& storage_; - const Log& log_; - - Subscriber discovery_sub_; - mutable Publisher discovery_pub_; - - Server servers_[MaxServers - 1]; ///< Minus one because the local server is not listed there. - - uint8_t cluster_size_; - uint8_t num_known_servers_; - - virtual void handleTimerEvent(const TimerEvent& event); - - void handleDiscovery(const ReceivedDataStructure& msg); - - void publishDiscovery() const; - - public: - enum { ClusterSizeUnknown = 0 }; - - /** - * @param node Needed to publish and subscribe to Discovery message - * @param storage Needed to read the cluster size parameter from the storage - * @param log Needed to initialize nextIndex[] values after elections - */ - ClusterManager(INode& node, const IStorageBackend& storage, const Log& log) - : storage_(storage) - , log_(log) - , discovery_sub_(node) - , discovery_pub_(node) - , cluster_size_(0) - , num_known_servers_(0) - { } - - /** - * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the - * storage backend using key 'cluster_size'. - * Returns negative error code. - */ - int init(uint8_t cluster_size = ClusterSizeUnknown); - - /** - * An invalid node ID will be returned if there's no such server. - * The local server is not listed there. - */ - NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const; - - /** - * See next_index[] in Raft paper. - */ - Log::Index getServerNextIndex(NodeID server_node_id) const; - void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment); - void decrementServerNextIndex(NodeID server_node_id); - - /** - * See match_index[] in Raft paper. - */ - Log::Index getServerMatchIndex(NodeID server_node_id) const; - void setServerMatchIndex(NodeID server_node_id, Log::Index match_index); - - /** - * This method must be called when the current server becomes leader. - */ - void resetAllServerIndices(); - - uint8_t getNumKnownServers() const { return num_known_servers_; } - uint8_t getConfiguredClusterSize() const { return cluster_size_; } - uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } - }; - - enum ServerState - { - ServerStateFollower, - ServerStateCandidate, - ServerStateLeader - }; - - /* - * Raft-related states - */ - PersistentState persistent_state_; ///< Modifications of this state are slow as they involve storage IO. - - Log::Index commit_index_; - - ClusterManager cluster_; - - /* - * Implementation-specific states - */ PendingGetNodeInfoAttemptsMap pending_get_node_info_attempts_; - MonotonicTime last_activity_timestamp_; - bool active_mode_; - - ServerState server_state_; - - /* - * Transport - */ Subscriber allocation_sub_; Publisher allocation_pub_; Subscriber node_status_sub_; - ServiceServer append_entries_srv_; - ServiceServer request_vote_srv_; - - ServiceClient append_entries_client_; - ServiceClient request_vote_client_; }; } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp new file mode 100644 index 0000000000..ed9b785c8d --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -0,0 +1,12 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + +TEST(DynamicNodeIDAllocationClient, MarshallingStorageDecorator) +{ + +} From 80169f9a1c87b02be2086e402f52a7913a50d2d7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 2 May 2015 16:59:22 +0300 Subject: [PATCH 006/268] Marshalling storage decorator --- .../dynamic_node_id_allocation_server.hpp | 16 +- .../uc_dynamic_node_id_allocation_server.cpp | 156 ++++++++++++++++++ .../dynamic_node_id_allocation_server.cpp | 109 +++++++++++- 3 files changed, 274 insertions(+), 7 deletions(-) create mode 100644 libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 803a6d4894..7e3f6abed9 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -30,6 +30,7 @@ namespace uavcan * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward * and efficient implementation of storage backends, e.g. based on text files. + * Keys and values may contain only alphanumeric characters and underscores. */ class IDynamicNodeIDStorageBackend { @@ -59,7 +60,7 @@ public: virtual String get(const String& key) const = 0; /** - * Create or update value for the given key. + * Create or update value for the given key. Empty value should be regarded as a request to delete the key. * This method should not block for more than 50 ms. * Failures will be ignored. */ @@ -85,6 +86,8 @@ class MarshallingStorageDecorator { IDynamicNodeIDStorageBackend& storage_; + static uint8_t convertLowerCaseHexCharToNibble(char ch); + public: MarshallingStorageDecorator(IDynamicNodeIDStorageBackend& storage) : storage_(storage) @@ -94,13 +97,10 @@ public: } /** - * Setters do the following: + * These methods set the value and then immediately read it back. * 1. Serialize the value. * 2. Update the value on the backend. - * 3. Read the value back from the backend; return false if read fails. - * 4. Deserealize the newly read value; return false if deserialization fails. - * 5. Update the argument with deserialized value. - * 6. Return true. + * 3. Call get() with the same value argument. * The caller then is supposed to check whether the argument has the desired value. */ bool setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value); @@ -109,6 +109,10 @@ public: /** * Getters simply read and deserialize the value. + * 1. Read the value back from the backend; return false if read fails. + * 2. Deserealize the newly read value; return false if deserialization fails. + * 3. Update the argument with deserialized value. + * 4. Return true. */ bool get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const; bool get(const IDynamicNodeIDStorageBackend::String& key, diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp new file mode 100644 index 0000000000..0dc68bbe06 --- /dev/null +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +#ifndef UAVCAN_CPP_VERSION +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +namespace dynamic_node_id_server_impl +{ +/* + * MarshallingStorageDecorator + */ +uint8_t MarshallingStorageDecorator::convertLowerCaseHexCharToNibble(char ch) +{ + const uint8_t ret = (ch > '9') ? static_cast(ch - 'a' + 10) : static_cast(ch - '0'); + UAVCAN_ASSERT(ret < 16); + return ret; +} + +bool MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value) +{ + IDynamicNodeIDStorageBackend::String serialized; + serialized.appendFormatted("%llu", static_cast(inout_value)); + + UAVCAN_TRACE("MarshallingStorageDecorator", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); +} + +bool MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value) +{ + IDynamicNodeIDStorageBackend::String serialized; + for (uint8_t i = 0; i < protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize; i++) + { + serialized.appendFormatted("%02x", inout_value.at(i)); + } + UAVCAN_ASSERT(serialized.size() == protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize * 2); + + UAVCAN_TRACE("MarshallingStorageDecorator", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); +} + +bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const +{ + /* + * Reading the storage + */ + const IDynamicNodeIDStorageBackend::String val = storage_.get(key); + if (val.empty()) + { + return false; + } + + /* + * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. + * The value must contain only numeric characters. + */ + for (IDynamicNodeIDStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if (static_cast(*it) < '0' || static_cast(*it) > '9') + { + return false; + } + } + + if (val.size() > 10) // len(str(0xFFFFFFFF)) + { + return false; + } + + /* + * Conversion is carried out here + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + errno = 0; +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + const unsigned long long x = std::strtoull(val.c_str(), NULL, 10); +#else + // There was no strtoull() before C++11, so we need to resort to strtoul() + StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check(); + const unsigned long x = std::strtoul(val.c_str(), NULL, 10); +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + if (errno != 0) + { + return false; + } +#endif + + out_value = static_cast(x); + return true; +} + +bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const +{ + static const uint8_t NumBytes = protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize; + + /* + * Reading the storage + */ + IDynamicNodeIDStorageBackend::String val = storage_.get(key); + if (val.size() != NumBytes * 2) + { + return false; + } + + /* + * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. + * The value must contain only hexadecimal numbers. + */ + val.convertToLowerCaseASCII(); + for (IDynamicNodeIDStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if ((static_cast(*it) < '0' || static_cast(*it) > '9') && + (static_cast(*it) < 'a' || static_cast(*it) > 'f')) + { + return false; + } + } + + /* + * Conversion is carried out here + */ + IDynamicNodeIDStorageBackend::String::const_iterator it = val.begin(); + + for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++) + { + out_value[byte_index] = static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) << 4); + out_value[byte_index] = static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) | + out_value[byte_index]); + } + + return true; +} + +} // dynamic_node_id_server_impl + +} diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index ed9b785c8d..39af887c1a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -3,10 +3,117 @@ */ #include +#include #include #include "helpers.hpp" -TEST(DynamicNodeIDAllocationClient, MarshallingStorageDecorator) +class StorageBackend : public uavcan::IDynamicNodeIDStorageBackend { +public: + typedef std::map Container; + Container container_; + virtual String get(const String& key) const + { + const Container::const_iterator it = container_.find(key); + if (it == container_.end()) + { + return String(); + } + return it->second; + } + + virtual void set(const String& key, const String& value) + { + container_[key] = value; + } + + void printContainer() const + { + for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it) + { + std::cout << it->first.c_str() << "\t" << it->second.c_str() << std::endl; + } + } +}; + +TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) +{ + StorageBackend st; + + uavcan::dynamic_node_id_server_impl::MarshallingStorageDecorator marshaler(st); + + uavcan::IDynamicNodeIDStorageBackend::String key; + + /* + * uint32 + */ + uint32_t u32 = 0; + + key = "foo"; + u32 = 0; + ASSERT_TRUE(marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0, u32); + + key = "bar"; + u32 = 0xFFFFFFFF; + ASSERT_TRUE(marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + ASSERT_TRUE(marshaler.get(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + + key = "foo"; + ASSERT_TRUE(marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + key = "the_cake_is_a_lie"; + ASSERT_FALSE(marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + /* + * uint8[16] + */ + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id array; + + key = "a"; + // Set zero + ASSERT_TRUE(marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(0, array[i]); + } + + // Make sure this will not be interpreted as uint32 + ASSERT_FALSE(marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + // Set pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i + 1); + } + ASSERT_TRUE(marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(i + 1, array[i]); + } + + // Set another pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i | (i << 4)); + } + ASSERT_TRUE(marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(uint8_t(i | (i << 4)), array[i]); + } + + // Make sure uint32 cannot be interpreted as an array + key = "foo"; + ASSERT_FALSE(marshaler.get(key, array)); + + // Nonexistent key + key = "the_cake_is_a_lie"; + ASSERT_FALSE(marshaler.get(key, array)); } From a60d5c812a26fcdcd0aa3af4fbee9f3b09243bfc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 11:28:59 +0300 Subject: [PATCH 007/268] Fixed RaftCore API --- .../dynamic_node_id_allocation_server.hpp | 33 +++++++++++++++---- 1 file changed, 26 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 7e3f6abed9..c310fb46bf 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -13,6 +13,7 @@ #include #include #include +#include #include // Types used by the server #include @@ -384,16 +385,31 @@ public: */ void appendLog(const protocol::dynamic_node_id::server::Entry& entry); + /** + * This class is used to perform log searches. + */ + struct LogEntryInfo + { + protocol::dynamic_node_id::server::Entry entry; + bool committed; + + LogEntryInfo(const protocol::dynamic_node_id::server::Entry& arg_entry, bool arg_committed) + : entry(arg_entry) + , committed(arg_committed) + { } + }; + /** * This method is used by the allocator to query existence of certain entries in the Raft log. * Predicate is a callable of the following prototype: - * bool (const protocol::dynamic_node_id::server::Entry&) - * Once the predicate returns true, the loop will be terminated and the method will return a pointer to the last - * visited entry; otherwise nullptr will be returned. + * bool (const LogEntryInfo& entry) + * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy + * contructor to the last visited entry; otherwise the constructor will not be initialized. In this case, lazy + * constructor is used as boost::optional. * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. */ template - const protocol::dynamic_node_id::server::Entry* traverseLogFromEndUntil(const Predicate& predicate) const + inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const { UAVCAN_ASSERT(try_implicit_cast(predicate, true)); for (int index = static_cast(persistent_state_.getLog().getMaxIndex()); index--; index >= 0) @@ -401,12 +417,15 @@ public: const protocol::dynamic_node_id::server::Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); UAVCAN_ASSERT(entry != NULL); - if (predicate(*entry)) + const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_); + if (predicate(info)) { - return entry; + LazyConstructor ret; + ret.template construct(info); + return ret; } } - return NULL; + return LazyConstructor(); } }; From 5633cb8bdb813312ca804635832a7563009c29d5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 12:28:26 +0300 Subject: [PATCH 008/268] Raft: Basic log methods --- .../dynamic_node_id_allocation_server.hpp | 33 ++-- .../uc_dynamic_node_id_allocation_server.cpp | 157 ++++++++++++++++-- .../dynamic_node_id_allocation_server.cpp | 22 +-- 3 files changed, 175 insertions(+), 37 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index c310fb46bf..f3ea8eb833 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -31,7 +31,7 @@ namespace uavcan * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward * and efficient implementation of storage backends, e.g. based on text files. - * Keys and values may contain only alphanumeric characters and underscores. + * Keys and values may contain only non-whitespace, non-formatting printable characters. */ class IDynamicNodeIDStorageBackend { @@ -104,9 +104,9 @@ public: * 3. Call get() with the same value argument. * The caller then is supposed to check whether the argument has the desired value. */ - bool setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value); - bool setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value); + int setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value); + int setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value); /** * Getters simply read and deserialize the value. @@ -115,9 +115,9 @@ public: * 3. Update the argument with deserialized value. * 4. Return true. */ - bool get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const; - bool get(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const; + int get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const; + int get(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const; }; /** @@ -135,12 +135,17 @@ private: IDynamicNodeIDStorageBackend& storage_; protocol::dynamic_node_id::server::Entry entries_[Capacity]; - Index max_index_; // Index zero always contains an empty entry + Index last_index_; // Index zero always contains an empty entry + + static IDynamicNodeIDStorageBackend::String makeEntryKey(Index index, const char* postfix); + + int readEntryFromStorage(Index index, protocol::dynamic_node_id::server::Entry& out_entry); + int writeEntryToStorage(Index index, const protocol::dynamic_node_id::server::Entry& entry); public: Log(IDynamicNodeIDStorageBackend& storage) : storage_(storage) - , max_index_(0) + , last_index_(0) { } /** @@ -150,13 +155,15 @@ public: /** * This method invokes storage IO. + * Returned value indicates whether the entry was successfully appended. */ - void append(const protocol::dynamic_node_id::server::Entry& entry); + int append(const protocol::dynamic_node_id::server::Entry& entry); /** * This method invokes storage IO. + * Returned value indicates whether the requested operation has been carried out successfully. */ - void removeEntriesWhereIndexGreaterOrEqual(Index index); + int removeEntriesWhereIndexGreaterOrEqual(Index index); /** * Returns nullptr if there's no such index. @@ -164,7 +171,7 @@ public: */ const protocol::dynamic_node_id::server::Entry* getEntryAtIndex(Index index) const; - Index getMaxIndex() const { return max_index_; } + Index getLastIndex() const { return last_index_; } bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const; }; @@ -412,7 +419,7 @@ public: inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const { UAVCAN_ASSERT(try_implicit_cast(predicate, true)); - for (int index = static_cast(persistent_state_.getLog().getMaxIndex()); index--; index >= 0) + for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index--; index >= 0) { const protocol::dynamic_node_id::server::Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 0dc68bbe06..a611bd5b6a 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -27,7 +27,7 @@ uint8_t MarshallingStorageDecorator::convertLowerCaseHexCharToNibble(char ch) return ret; } -bool MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value) +int MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value) { IDynamicNodeIDStorageBackend::String serialized; serialized.appendFormatted("%llu", static_cast(inout_value)); @@ -38,7 +38,7 @@ bool MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBacke return get(key, inout_value); } -bool MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, +int MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value) { IDynamicNodeIDStorageBackend::String serialized; @@ -54,7 +54,7 @@ bool MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBacke return get(key, inout_value); } -bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const +int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const { /* * Reading the storage @@ -62,7 +62,7 @@ bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String const IDynamicNodeIDStorageBackend::String val = storage_.get(key); if (val.empty()) { - return false; + return -ErrFailure; } /* @@ -73,13 +73,13 @@ bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String { if (static_cast(*it) < '0' || static_cast(*it) > '9') { - return false; + return -ErrFailure; } } if (val.size() > 10) // len(str(0xFFFFFFFF)) { - return false; + return -ErrFailure; } /* @@ -100,16 +100,16 @@ bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 if (errno != 0) { - return false; + return -ErrFailure; } #endif out_value = static_cast(x); - return true; + return 0; } -bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const +int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, + protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const { static const uint8_t NumBytes = protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize; @@ -119,7 +119,7 @@ bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String IDynamicNodeIDStorageBackend::String val = storage_.get(key); if (val.size() != NumBytes * 2) { - return false; + return -ErrFailure; } /* @@ -132,7 +132,7 @@ bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String if ((static_cast(*it) < '0' || static_cast(*it) > '9') && (static_cast(*it) < 'a' || static_cast(*it) > 'f')) { - return false; + return -ErrFailure; } } @@ -148,7 +148,138 @@ bool MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String out_value[byte_index]); } - return true; + return 0; +} + +/* + * Log + */ +IDynamicNodeIDStorageBackend::String Log::makeEntryKey(Index index, const char* postfix) +{ + IDynamicNodeIDStorageBackend::String str; + // "log0.foobar" + str += "log"; + str.appendFormatted("%d", int(index)); + str += "."; + str += postfix; + return str; +} + +int Log::readEntryFromStorage(Index index, protocol::dynamic_node_id::server::Entry& out_entry) +{ + const MarshallingStorageDecorator io(storage_); + + // Term + if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0) + { + return -ErrFailure; + } + + // 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; + } + out_entry.node_id = static_cast(node_id); + + return 0; +} + +int Log::writeEntryToStorage(Index index, const protocol::dynamic_node_id::server::Entry& entry) +{ + protocol::dynamic_node_id::server::Entry temp = entry; + + MarshallingStorageDecorator io(storage_); + + // Term + if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0) + { + return -ErrFailure; + } + + // Unique ID + if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = entry.node_id; + if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + temp.node_id = static_cast(node_id); + + return (temp == entry) ? 0 : -ErrFailure; +} + +int Log::init() +{ + const MarshallingStorageDecorator io(storage_); + + // Reading max index + { + IDynamicNodeIDStorageBackend::String key; + key = "log_last_index"; + uint32_t value = 0; + if (io.get(key, value) < 0) + { + return -ErrFailure; + } + if (value >= Capacity) + { + return -ErrFailure; + } + last_index_ = Index(value); + } + + // Restoring log entries + for (Index index = 0; index <= last_index_; index++) + { + const int result = readEntryFromStorage(index, entries_[index]); + if (result < 0) + { + return result; + } + } + + return 0; +} + +int Log::append(const protocol::dynamic_node_id::server::Entry& entry) +{ + (void)entry; + return -1; +} + +int Log::removeEntriesWhereIndexGreaterOrEqual(Index index) +{ + (void)index; + return -1; +} + +const protocol::dynamic_node_id::server::Entry* Log::getEntryAtIndex(Index index) const +{ + return (index <= last_index_) ? &entries_[index] : NULL; +} + +bool Log::isOtherLogUpToDate(Log::Index other_last_index, Term other_last_term) const +{ + // Terms are different - the one with higher term is more up-to-date + if (other_last_term != entries_[last_index_].term) + { + return other_last_term > entries_[last_index_].term; + } + // Terms are equal - longer log wins + return other_last_index >= last_index_; } } // dynamic_node_id_server_impl diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 39af887c1a..e509bbba7d 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -52,22 +52,22 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) key = "foo"; u32 = 0; - ASSERT_TRUE(marshaler.setAndGetBack(key, u32)); + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); ASSERT_EQ(0, u32); key = "bar"; u32 = 0xFFFFFFFF; - ASSERT_TRUE(marshaler.setAndGetBack(key, u32)); + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); ASSERT_EQ(0xFFFFFFFF, u32); - ASSERT_TRUE(marshaler.get(key, u32)); + ASSERT_LE(0, marshaler.get(key, u32)); ASSERT_EQ(0xFFFFFFFF, u32); key = "foo"; - ASSERT_TRUE(marshaler.get(key, u32)); + ASSERT_LE(0, marshaler.get(key, u32)); ASSERT_EQ(0, u32); key = "the_cake_is_a_lie"; - ASSERT_FALSE(marshaler.get(key, u32)); + ASSERT_GT(0, marshaler.get(key, u32)); ASSERT_EQ(0, u32); /* @@ -77,14 +77,14 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) key = "a"; // Set zero - ASSERT_TRUE(marshaler.setAndGetBack(key, array)); + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); for (uint8_t i = 0; i < 16; i++) { ASSERT_EQ(0, array[i]); } // Make sure this will not be interpreted as uint32 - ASSERT_FALSE(marshaler.get(key, u32)); + ASSERT_GT(0, marshaler.get(key, u32)); ASSERT_EQ(0, u32); // Set pattern @@ -92,7 +92,7 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) { array[i] = uint8_t(i + 1); } - ASSERT_TRUE(marshaler.setAndGetBack(key, array)); + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); for (uint8_t i = 0; i < 16; i++) { ASSERT_EQ(i + 1, array[i]); @@ -103,7 +103,7 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) { array[i] = uint8_t(i | (i << 4)); } - ASSERT_TRUE(marshaler.setAndGetBack(key, array)); + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); for (uint8_t i = 0; i < 16; i++) { ASSERT_EQ(uint8_t(i | (i << 4)), array[i]); @@ -111,9 +111,9 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) // Make sure uint32 cannot be interpreted as an array key = "foo"; - ASSERT_FALSE(marshaler.get(key, array)); + ASSERT_GT(0, marshaler.get(key, array)); // Nonexistent key key = "the_cake_is_a_lie"; - ASSERT_FALSE(marshaler.get(key, array)); + ASSERT_GT(0, marshaler.get(key, array)); } From a439c62532590c412fdedc569108690acf1db29f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 13:29:53 +0300 Subject: [PATCH 009/268] String constructor for Array<> --- libuavcan/include/uavcan/marshal/array.hpp | 14 ++++++++++++++ libuavcan/test/marshal/array.cpp | 10 ++++++++++ 2 files changed, 24 insertions(+) diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index fb17b68dea..73277f4c1b 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -667,6 +667,20 @@ public: static_cast(RawValueType::MaxBitLen) * static_cast(MaxSize) }; + /** + * Default constructor zero-initializes the storage even if it consists of primitive types. + */ + Array() { } + + /** + * String constructor - only for string-like arrays. + * Refer to @ref operator+=(const char*) for details. + */ + Array(const char* str) // Implicit + { + operator+=(str); + } + static int encode(const SelfType& array, ScalarCodec& codec, const TailArrayOptimizationMode tao_mode) { return array.encodeImpl(codec, tao_mode, BooleanType()); diff --git a/libuavcan/test/marshal/array.cpp b/libuavcan/test/marshal/array.cpp index d213c78b2a..9bcc6a461d 100644 --- a/libuavcan/test/marshal/array.cpp +++ b/libuavcan/test/marshal/array.cpp @@ -768,6 +768,16 @@ TEST(Array, Strings) ASSERT_STREQ("Our sun is dying. 123456-789", a8_2.c_str()); ASSERT_STREQ("Our sun is dying. 123456-789", a8.c_str()); ASSERT_STREQ("456", a7.c_str()); + + /* + * String constructor + */ + A8 a8_3("123"); + A7 a7_3 = "456"; + ASSERT_EQ(3, a8_3.size()); + ASSERT_EQ(3, a7_3.size()); + ASSERT_STREQ("123", a8_3.c_str()); + ASSERT_STREQ("456", a7_3.c_str()); } From b1f5313fcb33fed5d7ffef9a65bdf0b1a198073d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 14:21:41 +0300 Subject: [PATCH 010/268] Raft log - basic tests of state recovery --- .../dynamic_node_id_allocation_server.hpp | 1 + .../uc_dynamic_node_id_allocation_server.cpp | 87 +++++++++++++++--- .../dynamic_node_id_allocation_server.cpp | 89 +++++++++++++++++++ 3 files changed, 166 insertions(+), 11 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index f3ea8eb833..c8fde33897 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -137,6 +137,7 @@ private: protocol::dynamic_node_id::server::Entry entries_[Capacity]; Index last_index_; // Index zero always contains an empty entry + static IDynamicNodeIDStorageBackend::String getLastIndexKey() { return "log_last_index"; } static IDynamicNodeIDStorageBackend::String makeEntryKey(Index index, const char* postfix); int readEntryFromStorage(Index index, protocol::dynamic_node_id::server::Entry& out_entry); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index a611bd5b6a..b7fac5fb34 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -123,7 +123,6 @@ int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& } /* - * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. * The value must contain only hexadecimal numbers. */ val.convertToLowerCaseASCII(); @@ -157,10 +156,10 @@ int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& IDynamicNodeIDStorageBackend::String Log::makeEntryKey(Index index, const char* postfix) { IDynamicNodeIDStorageBackend::String str; - // "log0.foobar" + // "log0_foobar" str += "log"; str.appendFormatted("%d", int(index)); - str += "."; + str += "_"; str += postfix; return str; } @@ -187,6 +186,10 @@ int Log::readEntryFromStorage(Index index, protocol::dynamic_node_id::server::En { return -ErrFailure; } + if (node_id > NodeID::Max) + { + return -ErrFailure; + } out_entry.node_id = static_cast(node_id); return 0; @@ -227,12 +230,21 @@ int Log::init() // Reading max index { - IDynamicNodeIDStorageBackend::String key; - key = "log_last_index"; uint32_t value = 0; - if (io.get(key, value) < 0) + if (io.get(getLastIndexKey(), value) < 0) { - return -ErrFailure; + if (storage_.get(getLastIndexKey()).empty()) + { + // It appears like there's no log in the storage - initializing empty log then + last_index_ = 0; + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Initializing empty log"); + return 0; + } + else + { + // There's some data in the storage, but it cannot be parsed - reporting an error + return -ErrFailure; + } } if (value >= Capacity) { @@ -251,28 +263,81 @@ int Log::init() } } + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Restored %u log entries", unsigned(last_index_)); return 0; } int Log::append(const protocol::dynamic_node_id::server::Entry& entry) { - (void)entry; - return -1; + if ((last_index_ + 1) >= Capacity) + { + return -ErrLogic; + } + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + int res = writeEntryToStorage(Index(last_index_ + 1), entry); + if (res < 0) + { + return res; + } + + // Updating the last index + MarshallingStorageDecorator io(storage_); + uint32_t new_last_index = last_index_ + 1U; + res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != last_index_ + 1U) + { + return -ErrFailure; + } + entries_[new_last_index] = entry; + last_index_ = Index(new_last_index); + + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "New entry, index %u, node ID %u, term %u", + unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term)); + return 0; } int Log::removeEntriesWhereIndexGreaterOrEqual(Index index) { - (void)index; - return -1; + UAVCAN_ASSERT(last_index_ < Capacity); + + if (((index) >= Capacity) || (index == 0)) + { + return -ErrLogic; + } + + MarshallingStorageDecorator io(storage_); + uint32_t new_last_index = index - 1U; + int res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != index - 1U) + { + return -ErrFailure; + } + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Entries removed, last index %u --> %u", + unsigned(last_index_), unsigned(new_last_index)); + last_index_ = Index(new_last_index); + + // Removal operation leaves dangling entries in storage, it's OK + return 0; } const protocol::dynamic_node_id::server::Entry* Log::getEntryAtIndex(Index index) const { + UAVCAN_ASSERT(last_index_ < Capacity); return (index <= last_index_) ? &entries_[index] : NULL; } bool Log::isOtherLogUpToDate(Log::Index other_last_index, Term other_last_term) const { + UAVCAN_ASSERT(last_index_ < Capacity); // Terms are different - the one with higher term is more up-to-date if (other_last_term != entries_[last_index_].term) { diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index e509bbba7d..22c7d71a89 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -37,6 +37,7 @@ public: } }; + TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) { StorageBackend st; @@ -117,3 +118,91 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) key = "the_cake_is_a_lie"; ASSERT_GT(0, marshaler.get(key, array)); } + + +TEST(DynamicNodeIDAllocationServer, LogInitialization) +{ + // No log data in the storage - initializing empty log + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + + ASSERT_LE(0, log.init()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + } + // Nonempty storage, one item + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + } + // Nonempty storage, broken data + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + + storage.set("log_last_index", "foobar"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "128"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // OK + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "128"); // Bad value (127 max) + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + } + // Nonempty storage, many items + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + + storage.set("log_last_index", "1"); // 2 items - 0, 1 + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + storage.set("log1_term", "1"); + storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef"); + storage.set("log1_node_id", "127"); + + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(1, log.getLastIndex()); + + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + + ASSERT_EQ(1, log.getEntryAtIndex(1)->term); + ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id); + 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_EQ(uid, log.getEntryAtIndex(1)->unique_id); + } +} From a7dd5f9aea05ef22b4c92b523dd5f37f11a58aa5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 20:10:25 +0300 Subject: [PATCH 011/268] Proper log initialization --- .../dynamic_node_id_allocation_server.hpp | 2 + .../uc_dynamic_node_id_allocation_server.cpp | 38 ++++++++++++++++--- .../dynamic_node_id_allocation_server.cpp | 27 +++++++++++-- 3 files changed, 58 insertions(+), 9 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index c8fde33897..3fa03b5c50 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -143,6 +143,8 @@ private: int readEntryFromStorage(Index index, protocol::dynamic_node_id::server::Entry& out_entry); int writeEntryToStorage(Index index, const protocol::dynamic_node_id::server::Entry& entry); + int initEmptyLogStorage(); + public: Log(IDynamicNodeIDStorageBackend& storage) : storage_(storage) diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index b7fac5fb34..8f179b4476 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -224,9 +224,37 @@ int Log::writeEntryToStorage(Index index, const protocol::dynamic_node_id::serve return (temp == entry) ? 0 : -ErrFailure; } +int Log::initEmptyLogStorage() +{ + MarshallingStorageDecorator io(storage_); + + // Initializing last index + last_index_ = 0; + uint32_t stored_index = 0; + int res = io.setAndGetBack(getLastIndexKey(), stored_index); + if (res < 0) + { + return res; + } + if (stored_index != 0) + { + return -ErrFailure; + } + + // Writing the zero entry - it must always be default-initialized + entries_[0] = protocol::dynamic_node_id::server::Entry(); + res = writeEntryToStorage(0, entries_[0]); + if (res < 0) + { + return res; + } + + return 0; +} + int Log::init() { - const MarshallingStorageDecorator io(storage_); + MarshallingStorageDecorator io(storage_); // Reading max index { @@ -235,10 +263,8 @@ int Log::init() { if (storage_.get(getLastIndexKey()).empty()) { - // It appears like there's no log in the storage - initializing empty log then - last_index_ = 0; - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Initializing empty log"); - return 0; + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Initializing empty storage"); + return initEmptyLogStorage(); } else { @@ -253,7 +279,7 @@ int Log::init() last_index_ = Index(value); } - // Restoring log entries + // Restoring log entries - note that index 0 always exists for (Index index = 0; index <= last_index_; index++) { const int result = readEntryFromStorage(index, entries_[index]); diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 22c7d71a89..6e2c239fe4 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -9,10 +9,16 @@ class StorageBackend : public uavcan::IDynamicNodeIDStorageBackend { -public: typedef std::map Container; Container container_; + bool fail_; + +public: + StorageBackend() + : fail_(false) + { } + virtual String get(const String& key) const { const Container::const_iterator it = container_.find(key); @@ -28,7 +34,13 @@ public: container_[key] = value; } - void printContainer() const + void failOnSetCalls(bool really) { fail_ = really; } + + void reset() { container_.clear(); } + + unsigned getNumKeys() const { return unsigned(container_.size()); } + + void print() const { for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it) { @@ -122,12 +134,16 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) TEST(DynamicNodeIDAllocationServer, LogInitialization) { + const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry + // No log data in the storage - initializing empty log { StorageBackend storage; uavcan::dynamic_node_id_server_impl::Log log(storage); + ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, log.init()); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); ASSERT_EQ(0, log.getLastIndex()); ASSERT_EQ(0, log.getEntryAtIndex(0)->term); ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); @@ -141,11 +157,13 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) storage.set("log_last_index", "0"); ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found + ASSERT_EQ(1, storage.getNumKeys()); storage.set("log0_term", "0"); storage.set("log0_unique_id", "00000000000000000000000000000000"); storage.set("log0_node_id", "0"); ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); ASSERT_EQ(0, log.getLastIndex()); ASSERT_EQ(0, log.getEntryAtIndex(0)->term); } @@ -161,7 +179,8 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value storage.set("log_last_index", "0"); - ASSERT_LE(-uavcan::ErrFailure, log.init()); // OK + ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items + ASSERT_EQ(1, storage.getNumKeys()); storage.set("log0_term", "0"); storage.set("log0_unique_id", "00000000000000000000000000000000"); @@ -169,6 +188,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed ASSERT_EQ(0, log.getLastIndex()); ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(4, storage.getNumKeys()); } // Nonempty storage, many items { @@ -184,6 +204,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) storage.set("log1_node_id", "127"); ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(7, storage.getNumKeys()); ASSERT_EQ(1, log.getLastIndex()); ASSERT_EQ(0, log.getEntryAtIndex(0)->term); From 54fcfe4e06d9a8a13b31c90af01a94e9a89544a3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 20:28:39 +0300 Subject: [PATCH 012/268] Log::append() tests --- .../dynamic_node_id_allocation_server.hpp | 2 +- .../dynamic_node_id_allocation_server.cpp | 82 ++++++++++++++++++- 2 files changed, 80 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 3fa03b5c50..bd9cd19ad4 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -130,9 +130,9 @@ class Log public: typedef uint8_t Index; -private: enum { Capacity = NodeID::Max + 1 }; +private: IDynamicNodeIDStorageBackend& storage_; protocol::dynamic_node_id::server::Entry entries_[Capacity]; Index last_index_; // Index zero always contains an empty entry diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 6e2c239fe4..46d4d3be66 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -31,7 +31,10 @@ public: virtual void set(const String& key, const String& value) { - container_[key] = value; + if (!fail_) + { + container_[key] = value; + } } void failOnSetCalls(bool really) { fail_ = really; } @@ -50,6 +53,9 @@ public: }; +static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry + + TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) { StorageBackend st; @@ -134,8 +140,6 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) TEST(DynamicNodeIDAllocationServer, LogInitialization) { - const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry - // No log data in the storage - initializing empty log { StorageBackend storage; @@ -227,3 +231,75 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) ASSERT_EQ(uid, log.getEntryAtIndex(1)->unique_id); } } + + +TEST(DynamicNodeIDAllocationServer, LogAppend) +{ + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + storage.print(); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + + /* + * Entry at the index 0 always exists, and it's always zero-initialized. + */ + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("log0_term")); + ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id")); + ASSERT_EQ("0", storage.get("log0_node_id")); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("log1_term")); + ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id")); + ASSERT_EQ("1", storage.get("log1_node_id")); + + ASSERT_EQ(1, log.getLastIndex()); + ASSERT_TRUE(entry == *log.getEntryAtIndex(1)); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(7, storage.getNumKeys()); + + entry.term = 2; + entry.node_id = 2; + entry.unique_id[0] = 2; + ASSERT_GT(0, log.append(entry)); + + ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, log.getLastIndex()); + + /* + * Making sure append() fails when the log is full + */ + storage.failOnSetCalls(false); + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, log.append(entry)); // Failing because full + + storage.print(); +} From c9b41330b2705c2520c4e7558e0d21e11ac3fcf4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 3 May 2015 20:37:07 +0300 Subject: [PATCH 013/268] Tests for log removal --- .../dynamic_node_id_allocation_server.cpp | 52 +++++++++++++++++++ 1 file changed, 52 insertions(+) diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 46d4d3be66..bc6bd0b6ef 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -303,3 +303,55 @@ TEST(DynamicNodeIDAllocationServer, LogAppend) storage.print(); } + + +TEST(DynamicNodeIDAllocationServer, LogRemove) +{ + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + + /* + * Filling the log fully + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + /* + * Removal will fail as the storage is failing to update + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + /* + * Now removal must work + */ + storage.failOnSetCalls(false); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); + ASSERT_EQ(59, log.getLastIndex()); + + ASSERT_EQ("59", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1)); + ASSERT_EQ(0, log.getLastIndex()); + + ASSERT_EQ("0", storage.get("log_last_index")); + + storage.print(); +} From 954d6899b295025a9adc23cc16714d2691b4d717 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 4 May 2015 08:02:55 +0300 Subject: [PATCH 014/268] Persistent storage implementation --- .../dynamic_node_id_allocation_server.hpp | 7 +- .../uc_dynamic_node_id_allocation_server.cpp | 93 +++++++++++++++++++ 2 files changed, 98 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index bd9cd19ad4..48e4be3709 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -192,6 +192,9 @@ class PersistentState NodeID voted_for_; Log log_; + static IDynamicNodeIDStorageBackend::String getCurrentTermKey() { return "current_term"; } + static IDynamicNodeIDStorageBackend::String getVotedForKey() { return "voted_for"; } + public: PersistentState(IDynamicNodeIDStorageBackend& storage) : storage_(storage) @@ -211,12 +214,12 @@ public: /** * Invokes storage IO. */ - void setCurrentTerm(Term term); + int setCurrentTerm(Term term); /** * Invokes storage IO. */ - void setVotedFor(NodeID node_id); + int setVotedFor(NodeID node_id); }; /** diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 8f179b4476..bf6bb13b11 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -373,6 +373,99 @@ bool Log::isOtherLogUpToDate(Log::Index other_last_index, Term other_last_term) return other_last_index >= last_index_; } +/* + * PersistentState + */ +int PersistentState::init() +{ + // Reading log + int res = log_.init(); + if (res < 0) + { + return res; + } + + const protocol::dynamic_node_id::server::Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); + if (last_entry == NULL) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + MarshallingStorageDecorator io(storage_); + + // Reading current term + res = io.get(getCurrentTermKey(), current_term_); + if (res < 0) + { + return res; + } + + if (current_term_ < last_entry->term) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", + "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)", + unsigned(current_term_), unsigned(last_entry->term)); + return -ErrLogic; + } + + // Reading voted for + uint32_t stored_voted_for = 0; + res = io.get(getVotedForKey(), stored_voted_for); + if ((res < 0) || (stored_voted_for > NodeID::Max)) + { + return -ErrFailure; + } + else + { + voted_for_ = NodeID(uint8_t(stored_voted_for)); + } + + return 0; +} + +int PersistentState::setCurrentTerm(const Term term) +{ + UAVCAN_ASSERT(current_term_ <= term); + MarshallingStorageDecorator io(storage_); + + Term tmp = term; + int res = io.setAndGetBack(getCurrentTermKey(), tmp); + if (res < 0) + { + return res; + } + + if (tmp != term) + { + return -ErrFailure; + } + + current_term_ = term; + return 0; +} + +int PersistentState::setVotedFor(const NodeID node_id) +{ + voted_for_ = node_id; + MarshallingStorageDecorator io(storage_); + + uint32_t tmp = node_id.get(); + int res = io.setAndGetBack(getVotedForKey(), tmp); + if (res < 0) + { + return res; + } + + if (node_id.get() != tmp) + { + return -ErrFailure; + } + + voted_for_ = node_id; + return 0; +} + } // dynamic_node_id_server_impl } From 6a8135fedf531a629862adec522ed17a7f1ab98c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 4 May 2015 13:41:51 +0300 Subject: [PATCH 015/268] Service server feature - response suppression --- .../include/uavcan/node/service_server.hpp | 73 ++++++++++++++++--- libuavcan/test/node/service_client.cpp | 54 +++++++++++++- 2 files changed, 114 insertions(+), 13 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index 6d5695451c..1602e6b70f 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -19,6 +19,48 @@ namespace uavcan { +/** + * This type can be used in place of the response type in a service server callback to get more advanced control + * of service request processing. + * + * PLEASE NOTE that since this class inherits the response type, service server callbacks can accept either + * object of this class or the response type directly if the extra options are not needed. + * + * For example, both of these callbacks can be used with the same service type 'Foo': + * + * void first(const ReceivedDataStructure& request, + * ServiceResponseDataStructure& response); + * + * void second(const Foo::Request& request, + * Foo::Response& response); + * + * In the latter case, an implicit cast will happen before the callback is invoked. + */ +template +class ServiceResponseDataStructure : public ResponseDataType_ +{ + // Fields are weirdly named to avoid name clashing with the inherited data type + bool _enabled_; + +public: + typedef ResponseDataType_ ResponseDataType; + + ServiceResponseDataStructure() + : _enabled_(true) + { } + + /** + * When disabled, the server will not transmit the response transfer. + * By default it is enabled, i.e. response will be sent. + */ + void setResponseEnabled(bool x) { _enabled_ = x; } + + /** + * Whether the response will be sent. By default it will. + */ + bool isResponseEnabled() const { return _enabled_; } +}; + /** * Use this class to implement UAVCAN service servers. * @@ -28,7 +70,7 @@ namespace uavcan * response will be returned via the output parameter of the callback. Note that * the reference to service response data struct passed to the callback always points * to a default initialized response object. - * Please also refer to @ref ReceivedDataStructure<>. + * Please also refer to @ref ReceivedDataStructure<> and @ref ServiceResponseDataStructure<>. * 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. @@ -42,10 +84,10 @@ namespace uavcan template = UAVCAN_CPP11 typename Callback_ = std::function&, - typename DataType_::Response&)>, + ServiceResponseDataStructure&)>, #else typename Callback_ = void (*)(const ReceivedDataStructure&, - typename DataType_::Response&), + ServiceResponseDataStructure&), #endif #if UAVCAN_TINY unsigned NumStaticReceivers = 0, @@ -75,14 +117,16 @@ private: PublisherType publisher_; Callback callback_; uint32_t response_failure_count_; - ResponseType response_; + ServiceResponseDataStructure response_; virtual void handleReceivedDataStruct(ReceivedDataStructure& request) { UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); if (try_implicit_cast(callback_, true)) { - response_ = ResponseType(); // The application needs newly initialized structure + // The application needs newly initialized structure + response_ = ServiceResponseDataStructure(); + UAVCAN_ASSERT(response_.isResponseEnabled()); // Enabled by default callback_(request, response_); } else @@ -90,13 +134,20 @@ private: handleFatalError("Srv serv clbk"); } - const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(), - request.getTransferID()); - if (res < 0) + if (response_.isResponseEnabled()) { - UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res); - publisher_.getNode().getDispatcher().getTransferPerfCounter().addError(); - response_failure_count_++; + const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(), + request.getTransferID()); + if (res < 0) + { + UAVCAN_TRACE("ServiceServer", "Response publication failure: %i", res); + publisher_.getNode().getDispatcher().getTransferPerfCounter().addError(); + response_failure_count_++; + } + } + else + { + UAVCAN_TRACE("ServiceServer", "Response was suppressed by the application"); } } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 4192bc890c..f668f791d5 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -47,14 +47,25 @@ struct ServiceCallResultHandler static void stringServiceServerCallback(const uavcan::ReceivedDataStructure& req, - root_ns_a::StringService::Response& rsp) + uavcan::ServiceResponseDataStructure& rsp) { rsp.string_response = "Request string: "; rsp.string_response += req.string_request; } +static void rejectingStringServiceServerCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& rsp) +{ + rsp.string_response = "Request string: "; + rsp.string_response += req.string_request; + ASSERT_TRUE(rsp.isResponseEnabled()); + rsp.setResponseEnabled(false); + ASSERT_FALSE(rsp.isResponseEnabled()); +} + static void emptyServiceServerCallback(const uavcan::ReceivedDataStructure&, - root_ns_a::EmptyService::Response&) + uavcan::ServiceResponseDataStructure&) { // Nothing to do - the service is empty } @@ -139,6 +150,45 @@ TEST(ServiceClient, Basic) } +TEST(ServiceClient, Rejection) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(rejectingStringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler handler; + + ClientType client1(nodes.b); + client1.setRequestTimeout(uavcan::MonotonicDuration::fromMSec(100)); + client1.setCallback(handler.bind()); + + root_ns_a::StringService::Request request; + request.string_request = "Hello world"; + + ASSERT_LT(0, client1.call(1, request)); + + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); + ASSERT_TRUE(client1.isPending()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + ASSERT_FALSE(client1.isPending()); + + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 1, root_ns_a::StringService::Response())); +} + + TEST(ServiceClient, Empty) { InterlinkedTestNodesWithSysClock nodes; From 1994260a2ce7660e88974629ff83d4b682667fb5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 4 May 2015 16:44:04 +0300 Subject: [PATCH 016/268] Persistent storage implementation and tests --- .../dynamic_node_id_allocation_server.hpp | 7 +- .../uc_dynamic_node_id_allocation_server.cpp | 88 +++++++-- .../dynamic_node_id_allocation_server.cpp | 184 ++++++++++++++++++ 3 files changed, 266 insertions(+), 13 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 48e4be3709..cdd9d6c1bb 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -152,7 +152,12 @@ public: { } /** - * This method invokes storage IO. + * 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(); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index bf6bb13b11..68be3636f4 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -269,6 +269,7 @@ int Log::init() else { // There's some data in the storage, but it cannot be parsed - reporting an error + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read last index"); return -ErrFailure; } } @@ -285,6 +286,8 @@ int Log::init() const int result = readEntryFromStorage(index, entries_[index]); if (result < 0) { + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read entry at index %u: %d", + unsigned(index), result); return result; } } @@ -378,10 +381,13 @@ bool Log::isOtherLogUpToDate(Log::Index other_last_index, Term other_last_term) */ int PersistentState::init() { - // Reading log + /* + * Reading log + */ int res = log_.init(); if (res < 0) { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Log init failed: %d", res); return res; } @@ -392,13 +398,37 @@ int PersistentState::init() return -ErrLogic; } + const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0); + MarshallingStorageDecorator io(storage_); - // Reading current term - res = io.get(getCurrentTermKey(), current_term_); - if (res < 0) + /* + * Reading currentTerm + */ + if (storage_.get(getCurrentTermKey()).empty() && log_is_empty) { - return res; + // First initialization + current_term_ = 0; + res = io.setAndGetBack(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init current term: %d", res); + return res; + } + if (current_term_ != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + res = io.get(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read current term: %d", res); + return res; + } } if (current_term_ < last_entry->term) @@ -409,15 +439,39 @@ int PersistentState::init() return -ErrLogic; } - // Reading voted for - uint32_t stored_voted_for = 0; - res = io.get(getVotedForKey(), stored_voted_for); - if ((res < 0) || (stored_voted_for > NodeID::Max)) + /* + * Reading votedFor + */ + if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0)) { - return -ErrFailure; + // First initialization + voted_for_ = NodeID(0); + uint32_t stored_voted_for = 0; + res = io.setAndGetBack(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init votedFor: %d", res); + return res; + } + if (stored_voted_for != 0) + { + return -ErrFailure; + } } else { + // Restoring + uint32_t stored_voted_for = 0; + res = io.get(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read votedFor: %d", res); + return res; + } + if (stored_voted_for > NodeID::Max) + { + return -ErrFailure; + } voted_for_ = NodeID(uint8_t(stored_voted_for)); } @@ -426,7 +480,12 @@ int PersistentState::init() int PersistentState::setCurrentTerm(const Term term) { - UAVCAN_ASSERT(current_term_ <= term); + if (term < current_term_) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + MarshallingStorageDecorator io(storage_); Term tmp = term; @@ -447,7 +506,12 @@ int PersistentState::setCurrentTerm(const Term term) int PersistentState::setVotedFor(const NodeID node_id) { - voted_for_ = node_id; + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + MarshallingStorageDecorator io(storage_); uint32_t tmp = node_id.get(); diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index bc6bd0b6ef..4dfffa1ddf 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -355,3 +355,187 @@ TEST(DynamicNodeIDAllocationServer, LogRemove) storage.print(); } + + +TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) +{ + /* + * First initialization + */ + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - only empty log is recovered + */ + { + StorageBackend storage; + + { + // This log is used to initialize the storage + uavcan::dynamic_node_id_server_impl::Log log(storage); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - log and current term are recovered + */ + { + StorageBackend storage; + + { + // This log is used to initialize the storage + uavcan::dynamic_node_id_server_impl::Log log(storage); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + storage.set("current_term", "1"); + + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + + ASSERT_GT(0, pers.init()); // Fails because current term is not zero + + storage.set("current_term", "0"); + + ASSERT_LE(0, pers.init()); // OK now + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Full recovery + */ + { + StorageBackend storage; + + { + // This log is used to initialize the storage + uavcan::dynamic_node_id_server_impl::Log log(storage); + ASSERT_LE(0, log.init()); + + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + } + ASSERT_LE(4, storage.getNumKeys()); + + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + + ASSERT_GT(0, pers.init()); // Fails because log is not empty + + storage.set("current_term", "0"); + storage.set("voted_for", "0"); + ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm + + storage.set("current_term", "1"); // OK + storage.set("voted_for", "128"); // Invalid value + ASSERT_GT(0, pers.init()); // Fails because of bad votedFor + + storage.set("voted_for", "0"); // OK now + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } +} + + +TEST(DynamicNodeIDAllocationServer, PersistentStorage) +{ + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + + /* + * Initializing + */ + ASSERT_LE(0, pers.init()); + + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Inserting some log entries + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, pers.getLog().append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing current term + */ + ASSERT_EQ(0, pers.getCurrentTerm()); + ASSERT_LE(0, pers.setCurrentTerm(2)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing votedFor + */ + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(0)); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_EQ(45, pers.getVotedFor().get()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Handling errors + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(2, pers.getCurrentTerm()); + ASSERT_GT(0, pers.setCurrentTerm(7893)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_GT(0, pers.setVotedFor(78)); + ASSERT_EQ(45, pers.getVotedFor().get()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Final checks + */ + ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage +} From 51cd8404b133fe34f09d1f5516389e73aba08a9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 4 May 2015 19:00:39 +0300 Subject: [PATCH 017/268] Cluster manager implementation, no tests yet --- .../dynamic_node_id_allocation_server.hpp | 38 ++- .../uc_dynamic_node_id_allocation_server.cpp | 285 ++++++++++++++++++ 2 files changed, 317 insertions(+), 6 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index cdd9d6c1bb..e37d3a5c87 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -239,7 +239,7 @@ class ClusterManager : private TimerBase struct Server { - const NodeID node_id; + NodeID node_id; Log::Index next_index; Log::Index match_index; @@ -251,7 +251,7 @@ class ClusterManager : private TimerBase enum { MaxServers = protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize }; - const IDynamicNodeIDStorageBackend& storage_; + IDynamicNodeIDStorageBackend& storage_; const Log& log_; Subscriber discovery_sub_; @@ -262,11 +262,23 @@ class ClusterManager : private TimerBase uint8_t cluster_size_; uint8_t num_known_servers_; + bool had_discovery_activity_; + + static IDynamicNodeIDStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; } + + INode& getNode() { return discovery_sub_.getNode(); } + const INode& getNode() const { return discovery_sub_.getNode(); } + + Server* findServer(NodeID node_id); + const Server* findServer(NodeID node_id) const; + bool isKnownServer(NodeID node_id) const; + void addServer(NodeID node_id); + virtual void handleTimerEvent(const TimerEvent&); void handleDiscovery(const ReceivedDataStructure& msg); - void publishDiscovery() const; + void publishDiscovery(); public: enum { ClusterSizeUnknown = 0 }; @@ -276,7 +288,7 @@ public: * @param storage Needed to read the cluster size parameter from the storage * @param log Needed to initialize nextIndex[] values after elections */ - ClusterManager(INode& node, const IDynamicNodeIDStorageBackend& storage, const Log& log) + ClusterManager(INode& node, IDynamicNodeIDStorageBackend& storage, const Log& log) : TimerBase(node) , storage_(storage) , log_(log) @@ -284,6 +296,7 @@ public: , discovery_pub_(node) , cluster_size_(0) , num_known_servers_(0) + , had_discovery_activity_(false) { } /** @@ -291,7 +304,7 @@ public: * storage backend using key 'cluster_size'. * Returns negative error code. */ - int init(uint8_t cluster_size = ClusterSizeUnknown); + int init(uint8_t init_cluster_size = ClusterSizeUnknown); /** * An invalid node ID will be returned if there's no such server. @@ -317,8 +330,21 @@ public: */ void resetAllServerIndices(); + /** + * 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; + } + uint8_t getNumKnownServers() const { return num_known_servers_; } - uint8_t getConfiguredClusterSize() const { return cluster_size_; } + uint8_t getClusterSize() const { return cluster_size_; } uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } }; diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 68be3636f4..a69ca1bfb7 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -530,6 +530,291 @@ int PersistentState::setVotedFor(const NodeID node_id) return 0; } +/* + * ClusterManager + */ +ClusterManager::Server* ClusterManager::findServer(NodeID node_id) +{ + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + if (servers_[i].node_id == node_id) + { + return &servers_[i]; + } + } + return NULL; +} + +const ClusterManager::Server* ClusterManager::findServer(NodeID node_id) const +{ + return const_cast(this)->findServer(node_id); +} + +bool ClusterManager::isKnownServer(NodeID node_id) const +{ + if (node_id == getNode().getNodeID()) + { + return true; + } + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); + if (servers_[i].node_id == node_id) + { + return true; + } + } + return false; +} + +void ClusterManager::addServer(NodeID node_id) +{ + UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxServers - 2)); + if (!isKnownServer(node_id) && node_id.isUnicast()) + { + servers_[num_known_servers_].node_id = node_id; + num_known_servers_ = static_cast(num_known_servers_ + 1U); + } + else + { + UAVCAN_ASSERT(0); + } +} + +void ClusterManager::handleTimerEvent(const TimerEvent&) +{ + UAVCAN_ASSERT(num_known_servers_ < cluster_size_); + if (num_known_servers_ < (cluster_size_ - 1)) + { + publishDiscovery(); + } + else + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster is fully discovered, no more broadcasts"); + stop(); + } +} + +void ClusterManager::handleDiscovery(const ReceivedDataStructure& msg) +{ + /* + * Validating cluster configuration + * If there's a case of misconfiguration, the message will be ignored. + */ + if (msg.configured_cluster_size != cluster_size_) + { + getNode().registerInternalFailure("Bad Raft cluster size"); + return; + } + + had_discovery_activity_ = true; + + /* + * Updating the set of known servers + */ + for (uint8_t i = 0; i < msg.known_nodes.size(); i++) + { + if (num_known_servers_ >= (cluster_size_ - 1)) + { + break; + } + + const NodeID node_id(msg.known_nodes[i]); + if (node_id.isUnicast() && !isKnownServer(node_id)) + { + addServer(node_id); + } + } + + /* + * Publishing a new Discovery request if the timer is stopped already and the publishing server needs to + * learn about more servers. + */ + if ((msg.configured_cluster_size > msg.known_nodes.size()) && !isRunning()) + { + publishDiscovery(); + } +} + +void ClusterManager::publishDiscovery() +{ + protocol::dynamic_node_id::server::Discovery msg; + + msg.configured_cluster_size = cluster_size_; + + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + msg.known_nodes.push_back(servers_[i].node_id.get()); + } + + UAVCAN_ASSERT(msg.known_nodes.size() == num_known_servers_); + + msg.known_nodes.push_back(getNode().getNodeID().get()); + + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Broadcasting Discovery message; known nodes: %d of %d", + int(msg.known_nodes.size()), int(cluster_size_)); + + const int res = discovery_pub_.broadcast(msg); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcst failed: %d", res); + getNode().registerInternalFailure("Raft discovery broadcast"); + } +} + +int ClusterManager::init(const uint8_t init_cluster_size) +{ + /* + * Figuring out the cluster size + */ + if (init_cluster_size == ClusterSizeUnknown) + { + // Reading from the storage + MarshallingStorageDecorator io(storage_); + uint32_t value = 0; + int res = io.get(getStorageKeyForClusterSize(), value); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", + "Cluster size is neither configured nor stored in the storage"); + return res; + } + if ((value == 0) || (value > MaxServers)) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster size is invalid"); + return -ErrFailure; + } + cluster_size_ = static_cast(value); + } + else + { + if ((init_cluster_size == 0) || (init_cluster_size > MaxServers)) + { + return -ErrInvalidParam; + } + cluster_size_ = init_cluster_size; + + // Writing the storage + MarshallingStorageDecorator io(storage_); + uint32_t value = init_cluster_size; + int res = io.setAndGetBack(getStorageKeyForClusterSize(), value); + if ((res < 0) || (value != init_cluster_size)) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Failed to store cluster size"); + return -ErrFailure; + } + } + + UAVCAN_ASSERT(cluster_size_ > 0); + UAVCAN_ASSERT(cluster_size_ <= MaxServers); + + /* + * Initializing pub/sub and timer + */ + int res = discovery_pub_.init(); + if (res < 0) + { + return res; + } + + res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery)); + if (res < 0) + { + return res; + } + + startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::server::Discovery::BROADCASTING_INTERVAL_MS)); + + /* + * Misc + */ + resetAllServerIndices(); + return 0; +} + +NodeID ClusterManager::getRemoteServerNodeIDAtIndex(uint8_t index) const +{ + if (index < num_known_servers_) + { + return servers_[index].node_id; + } + return NodeID(); +} + +Log::Index ClusterManager::getServerNextIndex(NodeID server_node_id) const +{ + const Server* const s = findServer(server_node_id); + if (s != NULL) + { + return s->next_index; + } + UAVCAN_ASSERT(0); + return 0; +} + +void ClusterManager::incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment) +{ + Server* const s = findServer(server_node_id); + if (s != NULL) + { + s->next_index = Log::Index(s->next_index + increment); + } + else + { + UAVCAN_ASSERT(0); + } +} + +void ClusterManager::decrementServerNextIndex(NodeID server_node_id) +{ + Server* const s = findServer(server_node_id); + if (s != NULL) + { + s->next_index--; + } + else + { + UAVCAN_ASSERT(0); + } +} + +Log::Index ClusterManager::getServerMatchIndex(NodeID server_node_id) const +{ + const Server* const s = findServer(server_node_id); + if (s != NULL) + { + return s->match_index; + } + UAVCAN_ASSERT(0); + return 0; +} + +void ClusterManager::setServerMatchIndex(NodeID server_node_id, Log::Index match_index) +{ + Server* const s = findServer(server_node_id); + if (s != NULL) + { + s->match_index = match_index; + } + else + { + UAVCAN_ASSERT(0); + } +} + +void ClusterManager::resetAllServerIndices() +{ + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + servers_[i].next_index = Log::Index(log_.getLastIndex() + 1U); + servers_[i].match_index = 0; + } +} + } // dynamic_node_id_server_impl } From 92d74d35ea15965fe5880682b9a720b40c769bdf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 4 May 2015 22:33:18 +0300 Subject: [PATCH 018/268] ClusterManager initialization test --- .../dynamic_node_id_allocation_server.cpp | 55 +++++++++++++++++++ 1 file changed, 55 insertions(+) diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 4dfffa1ddf..b663034922 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -539,3 +539,58 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorage) */ ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage } + + +TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) +{ + const unsigned MaxClusterSize = + uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + /* + * Simple initialization + */ + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + + // Too big + ASSERT_GT(0, mgr.init(MaxClusterSize + 1)); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + ASSERT_LE(0, mgr.init(7)); + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ("7", storage.get("cluster_size")); + + // Testing other states + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_EQ(7, mgr.getClusterSize()); + ASSERT_EQ(4, mgr.getQuorumSize()); + ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid()); + } + /* + * Recovery from the storage + */ + { + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + + // Not configured + ASSERT_GT(0, mgr.init()); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + storage.set("cluster_size", "7"); + ASSERT_LE(0, mgr.init()); + ASSERT_EQ(1, storage.getNumKeys()); + } +} From f11f49a7d2da865c8f13d653492d71a554ee26ad Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 5 May 2015 11:29:51 +0300 Subject: [PATCH 019/268] ClusterManager test for the case of one server --- .../dynamic_node_id_allocation_server.hpp | 4 +- .../uc_dynamic_node_id_allocation_server.cpp | 74 +++++++++++-------- libuavcan/test/node/test_node.hpp | 3 + .../dynamic_node_id_allocation_server.cpp | 74 +++++++++++++++++++ 4 files changed, 122 insertions(+), 33 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index e37d3a5c87..b6c02b537c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -278,7 +278,7 @@ class ClusterManager : private TimerBase void handleDiscovery(const ReceivedDataStructure& msg); - void publishDiscovery(); + void startDiscoveryPublishingTimerIfNotRunning(); public: enum { ClusterSizeUnknown = 0 }; @@ -346,6 +346,8 @@ public: uint8_t getNumKnownServers() const { return num_known_servers_; } uint8_t getClusterSize() const { return cluster_size_; } uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } + + bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); } }; /** diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index a69ca1bfb7..c358b80f88 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -586,13 +586,42 @@ void ClusterManager::addServer(NodeID node_id) void ClusterManager::handleTimerEvent(const TimerEvent&) { UAVCAN_ASSERT(num_known_servers_ < cluster_size_); - if (num_known_servers_ < (cluster_size_ - 1)) + + /* + * Filling the message + */ + protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = cluster_size_; + + for (uint8_t i = 0; i < num_known_servers_; i++) { - publishDiscovery(); + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + msg.known_nodes.push_back(servers_[i].node_id.get()); } - else + + msg.known_nodes.push_back(getNode().getNodeID().get()); + + UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1)); + + /* + * Broadcasting + */ + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Broadcasting Discovery message; known nodes: %d of %d", + int(msg.known_nodes.size()), int(cluster_size_)); + + const int res = discovery_pub_.broadcast(msg); + if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster is fully discovered, no more broadcasts"); + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcst failed: %d", res); + getNode().registerInternalFailure("Raft discovery broadcast"); + } + + /* + * Termination condition + */ + if (isClusterDiscovered()) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcasting timer stopped"); stop(); } } @@ -616,7 +645,7 @@ void ClusterManager::handleDiscovery(const ReceivedDataStructure= (cluster_size_ - 1)) + if (isClusterDiscovered()) { break; } @@ -629,39 +658,20 @@ void ClusterManager::handleDiscovery(const ReceivedDataStructure msg.known_nodes.size()) && !isRunning()) + if (msg.configured_cluster_size > msg.known_nodes.size()) { - publishDiscovery(); + startDiscoveryPublishingTimerIfNotRunning(); } } -void ClusterManager::publishDiscovery() +void ClusterManager::startDiscoveryPublishingTimerIfNotRunning() { - protocol::dynamic_node_id::server::Discovery msg; - - msg.configured_cluster_size = cluster_size_; - - for (uint8_t i = 0; i < num_known_servers_; i++) + if (!isRunning()) { - UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - msg.known_nodes.push_back(servers_[i].node_id.get()); - } - - UAVCAN_ASSERT(msg.known_nodes.size() == num_known_servers_); - - msg.known_nodes.push_back(getNode().getNodeID().get()); - - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Broadcasting Discovery message; known nodes: %d of %d", - int(msg.known_nodes.size()), int(cluster_size_)); - - const int res = discovery_pub_.broadcast(msg); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcst failed: %d", res); - getNode().registerInternalFailure("Raft discovery broadcast"); + startPeriodic( + MonotonicDuration::fromMSec(protocol::dynamic_node_id::server::Discovery::BROADCASTING_INTERVAL_MS)); } } @@ -726,7 +736,7 @@ int ClusterManager::init(const uint8_t init_cluster_size) return res; } - startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::server::Discovery::BROADCASTING_INTERVAL_MS)); + startDiscoveryPublishingTimerIfNotRunning(); /* * Misc diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 6fcdc60080..c8b2055834 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -16,10 +16,12 @@ struct TestNode : public uavcan::INode uavcan::MarshalBufferProvider<> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; uavcan::Scheduler scheduler; + uint64_t internal_failure_count; TestNode(uavcan::ICanDriver& can_driver, uavcan::ISystemClock& clock_driver, uavcan::NodeID self_node_id) : otr(poolmgr) , scheduler(can_driver, poolmgr, clock_driver, otr) + , internal_failure_count(0) { poolmgr.addPool(&pool); setNodeID(self_node_id); @@ -28,6 +30,7 @@ struct TestNode : public uavcan::INode virtual void registerInternalFailure(const char* msg) { std::cout << "TestNode internal failure: " << msg << std::endl; + internal_failure_count++; } virtual uavcan::PoolManager<1>& getAllocator() { return poolmgr; } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index b663034922..af117bceb6 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -594,3 +594,77 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) ASSERT_EQ(1, storage.getNumKeys()); } } + + +TEST(DynamicNodeIDAllocationServer, ClusterManagerOneServer) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(1)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_TRUE(mgr.isClusterDiscovered()); + + ASSERT_EQ(0, nodes.a.internal_failure_count); + + /* + * Broadcasting discovery with wrong cluster size, it will be reported as internal failure + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 2; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(1, nodes.a.internal_failure_count); + + /* + * Discovery rate limiting test + */ + ASSERT_FALSE(sub.collector.msg.get()); + + msg = uavcan::protocol::dynamic_node_id::server::Discovery(); + msg.configured_cluster_size = 1; // Correct value + ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + // Rinse repeat + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); +} From 3af95e3dd472e342f1d8d108e079e9b552509d23 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 5 May 2015 11:31:55 +0300 Subject: [PATCH 020/268] Log initialization fix --- .../uc_dynamic_node_id_allocation_server.cpp | 26 ++++++++++++------- 1 file changed, 16 insertions(+), 10 deletions(-) diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index c358b80f88..55045e505a 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -228,10 +228,24 @@ int Log::initEmptyLogStorage() { MarshallingStorageDecorator io(storage_); - // Initializing last index + /* + * Writing the zero entry - it must always be default-initialized + */ + entries_[0] = protocol::dynamic_node_id::server::Entry(); + int res = writeEntryToStorage(0, entries_[0]); + if (res < 0) + { + return res; + } + + /* + * Initializing last index + * Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be + * left in an inconsistent state. + */ last_index_ = 0; uint32_t stored_index = 0; - int res = io.setAndGetBack(getLastIndexKey(), stored_index); + res = io.setAndGetBack(getLastIndexKey(), stored_index); if (res < 0) { return res; @@ -241,14 +255,6 @@ int Log::initEmptyLogStorage() return -ErrFailure; } - // Writing the zero entry - it must always be default-initialized - entries_[0] = protocol::dynamic_node_id::server::Entry(); - res = writeEntryToStorage(0, entries_[0]); - if (res < 0) - { - return res; - } - return 0; } From 2273df059a60bfab67be644be30b39eada41f5ab Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 5 May 2015 12:53:33 +0300 Subject: [PATCH 021/268] ClusterManager tests --- .../dynamic_node_id_allocation_server.hpp | 2 + .../uc_dynamic_node_id_allocation_server.cpp | 14 ++- .../dynamic_node_id_allocation_server.cpp | 115 ++++++++++++++++++ 3 files changed, 127 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index b6c02b537c..41f337c02d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -247,6 +247,8 @@ class ClusterManager : private TimerBase : next_index(0) , match_index(0) { } + + void resetIndices(const Log& log); }; enum { MaxServers = protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize }; diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 55045e505a..3c40a56ade 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -539,6 +539,12 @@ int PersistentState::setVotedFor(const NodeID node_id) /* * ClusterManager */ +void ClusterManager::Server::resetIndices(const Log& log) +{ + next_index = Log::Index(log.getLastIndex() + 1U); + match_index = 0; +} + ClusterManager::Server* ClusterManager::findServer(NodeID node_id) { for (uint8_t i = 0; i < num_known_servers_; i++) @@ -581,6 +587,7 @@ void ClusterManager::addServer(NodeID node_id) if (!isKnownServer(node_id) && node_id.isUnicast()) { servers_[num_known_servers_].node_id = node_id; + servers_[num_known_servers_].resetIndices(log_); num_known_servers_ = static_cast(num_known_servers_ + 1U); } else @@ -599,14 +606,14 @@ void ClusterManager::handleTimerEvent(const TimerEvent&) protocol::dynamic_node_id::server::Discovery msg; msg.configured_cluster_size = cluster_size_; + msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0 + for (uint8_t i = 0; i < num_known_servers_; i++) { UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); msg.known_nodes.push_back(servers_[i].node_id.get()); } - msg.known_nodes.push_back(getNode().getNodeID().get()); - UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1)); /* @@ -826,8 +833,7 @@ void ClusterManager::resetAllServerIndices() for (uint8_t i = 0; i < num_known_servers_; i++) { UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - servers_[i].next_index = Log::Index(log_.getLastIndex() + 1U); - servers_[i].match_index = 0; + servers_[i].resetIndices(log_); } } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index af117bceb6..567b386ba1 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -668,3 +668,118 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerOneServer) ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); sub.collector.msg.reset(); } + + +TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + StorageBackend storage; + uavcan::dynamic_node_id_server_impl::Log log(storage); + InterlinkedTestNodesWithSysClock nodes; + + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(3)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_FALSE(mgr.isClusterDiscovered()); + + /* + * Discovery publishing rate check + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + /* + * Discovering other nodes + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 3; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(2, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + sub.collector.msg.reset(); + + ASSERT_FALSE(mgr.isClusterDiscovered()); + + // This will complete the discovery + msg.known_nodes.push_back(127U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(3, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + ASSERT_EQ(127, sub.collector.msg->known_nodes[2]); + sub.collector.msg.reset(); + + // Making sure discovery is now terminated + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(sub.collector.msg.get()); + + /* + * Checking Raft states + */ + ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0)); + ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1)); + ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2)); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); + + mgr.setServerMatchIndex(2, 10); + ASSERT_EQ(10, mgr.getServerMatchIndex(2)); + + mgr.incrementServerNextIndexBy(2, 5); + ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2)); + mgr.decrementServerNextIndex(2); + ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2)); + + mgr.resetAllServerIndices(); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); +} From 6ae48c25cbbdd327a17a89d3168877382abd8999 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 6 May 2015 18:01:27 +0300 Subject: [PATCH 022/268] Naming cleanup --- .../dynamic_node_id_allocation_server.hpp | 78 +++++++++---------- .../uc_dynamic_node_id_allocation_server.cpp | 31 ++++---- 2 files changed, 50 insertions(+), 59 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 41f337c02d..80a8c21fb7 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -75,10 +75,13 @@ public: */ namespace dynamic_node_id_server_impl { + +using namespace protocol::dynamic_node_id::server; + /** * Raft term */ -typedef StorageType::Type Term; +typedef StorageType::Type Term; /** * This class extends the storage backend interface with serialization/deserialization functionality. @@ -106,7 +109,7 @@ public: */ int setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value); int setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value); + Entry::FieldTypes::unique_id& inout_value); /** * Getters simply read and deserialize the value. @@ -117,7 +120,7 @@ public: */ int get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const; int get(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const; + Entry::FieldTypes::unique_id& out_value) const; }; /** @@ -134,14 +137,14 @@ public: private: IDynamicNodeIDStorageBackend& storage_; - protocol::dynamic_node_id::server::Entry entries_[Capacity]; + Entry entries_[Capacity]; Index last_index_; // Index zero always contains an empty entry static IDynamicNodeIDStorageBackend::String getLastIndexKey() { return "log_last_index"; } static IDynamicNodeIDStorageBackend::String makeEntryKey(Index index, const char* postfix); - int readEntryFromStorage(Index index, protocol::dynamic_node_id::server::Entry& out_entry); - int writeEntryToStorage(Index index, const protocol::dynamic_node_id::server::Entry& entry); + int readEntryFromStorage(Index index, Entry& out_entry); + int writeEntryToStorage(Index index, const Entry& entry); int initEmptyLogStorage(); @@ -165,7 +168,7 @@ public: * This method invokes storage IO. * Returned value indicates whether the entry was successfully appended. */ - int append(const protocol::dynamic_node_id::server::Entry& entry); + int append(const Entry& entry); /** * This method invokes storage IO. @@ -177,7 +180,7 @@ public: * Returns nullptr if there's no such index. * This method does not use storage IO. */ - const protocol::dynamic_node_id::server::Entry* getEntryAtIndex(Index index) const; + const Entry* getEntryAtIndex(Index index) const; Index getLastIndex() const { return last_index_; } @@ -234,7 +237,7 @@ class ClusterManager : private TimerBase { typedef MethodBinder&)> + (const ReceivedDataStructure&)> DiscoveryCallback; struct Server @@ -251,13 +254,13 @@ class ClusterManager : private TimerBase void resetIndices(const Log& log); }; - enum { MaxServers = protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize }; + enum { MaxServers = Discovery::FieldTypes::known_nodes::MaxSize }; IDynamicNodeIDStorageBackend& storage_; const Log& log_; - Subscriber discovery_sub_; - mutable Publisher discovery_pub_; + Subscriber discovery_sub_; + mutable Publisher discovery_pub_; Server servers_[MaxServers - 1]; ///< Minus one because the local server is not listed there. @@ -278,7 +281,7 @@ class ClusterManager : private TimerBase virtual void handleTimerEvent(const TimerEvent&); - void handleDiscovery(const ReceivedDataStructure& msg); + void handleDiscovery(const ReceivedDataStructure& msg); void startDiscoveryPublishingTimerIfNotRunning(); @@ -359,24 +362,18 @@ public: */ class RaftCore : private TimerBase { - typedef MethodBinder AppendEntriesCallback; + typedef MethodBinder&, + ServiceResponseDataStructure&)> + AppendEntriesCallback; - typedef MethodBinder RequestVoteCallback; + typedef MethodBinder&, + ServiceResponseDataStructure&)> + RequestVoteCallback; - typedef MethodBinder&)> + typedef MethodBinder&)> AppendEntriesResponseCallback; - typedef MethodBinder&)> + typedef MethodBinder&)> RequestVoteResponseCallback; enum ServerState @@ -386,24 +383,20 @@ class RaftCore : private TimerBase ServerStateLeader }; - dynamic_node_id_server_impl::PersistentState persistent_state_; - - dynamic_node_id_server_impl::Log::Index commit_index_; - - dynamic_node_id_server_impl::ClusterManager cluster_; + PersistentState persistent_state_; + Log::Index commit_index_; + ClusterManager cluster_; MonotonicTime last_activity_timestamp_; bool active_mode_; ServerState server_state_; - ServiceServer append_entries_srv_; - ServiceServer request_vote_srv_; + ServiceServer append_entries_srv_; + ServiceServer request_vote_srv_; - ServiceClient append_entries_client_; - ServiceClient request_vote_client_; + ServiceClient append_entries_client_; + ServiceClient request_vote_client_; virtual void handleTimerEvent(const TimerEvent&); @@ -431,17 +424,17 @@ public: * Inserts one entry into log. This operation may fail, which will not be reported. * Failures are tolerble because all operations are idempotent. */ - void appendLog(const protocol::dynamic_node_id::server::Entry& entry); + void appendLog(const Entry& entry); /** * This class is used to perform log searches. */ struct LogEntryInfo { - protocol::dynamic_node_id::server::Entry entry; + Entry entry; bool committed; - LogEntryInfo(const protocol::dynamic_node_id::server::Entry& arg_entry, bool arg_committed) + LogEntryInfo(const Entry& arg_entry, bool arg_committed) : entry(arg_entry) , committed(arg_committed) { } @@ -462,8 +455,7 @@ public: UAVCAN_ASSERT(try_implicit_cast(predicate, true)); for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index--; index >= 0) { - const protocol::dynamic_node_id::server::Entry* const entry = - persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); UAVCAN_ASSERT(entry != NULL); const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_); if (predicate(info)) diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 3c40a56ade..b3d7ff63c9 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -39,14 +39,14 @@ int MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBacken } int MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& inout_value) + Entry::FieldTypes::unique_id& inout_value) { IDynamicNodeIDStorageBackend::String serialized; - for (uint8_t i = 0; i < protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize; i++) + for (uint8_t i = 0; i < Entry::FieldTypes::unique_id::MaxSize; i++) { serialized.appendFormatted("%02x", inout_value.at(i)); } - UAVCAN_ASSERT(serialized.size() == protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize * 2); + UAVCAN_ASSERT(serialized.size() == Entry::FieldTypes::unique_id::MaxSize * 2); UAVCAN_TRACE("MarshallingStorageDecorator", "Set %s = %s", key.c_str(), serialized.c_str()); storage_.set(key, serialized); @@ -109,9 +109,9 @@ int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& } int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, - protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id& out_value) const + Entry::FieldTypes::unique_id& out_value) const { - static const uint8_t NumBytes = protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id::MaxSize; + static const uint8_t NumBytes = Entry::FieldTypes::unique_id::MaxSize; /* * Reading the storage @@ -164,7 +164,7 @@ IDynamicNodeIDStorageBackend::String Log::makeEntryKey(Index index, const char* return str; } -int Log::readEntryFromStorage(Index index, protocol::dynamic_node_id::server::Entry& out_entry) +int Log::readEntryFromStorage(Index index, Entry& out_entry) { const MarshallingStorageDecorator io(storage_); @@ -195,9 +195,9 @@ int Log::readEntryFromStorage(Index index, protocol::dynamic_node_id::server::En return 0; } -int Log::writeEntryToStorage(Index index, const protocol::dynamic_node_id::server::Entry& entry) +int Log::writeEntryToStorage(Index index, const Entry& entry) { - protocol::dynamic_node_id::server::Entry temp = entry; + Entry temp = entry; MarshallingStorageDecorator io(storage_); @@ -231,7 +231,7 @@ int Log::initEmptyLogStorage() /* * Writing the zero entry - it must always be default-initialized */ - entries_[0] = protocol::dynamic_node_id::server::Entry(); + entries_[0] = Entry(); int res = writeEntryToStorage(0, entries_[0]); if (res < 0) { @@ -302,7 +302,7 @@ int Log::init() return 0; } -int Log::append(const protocol::dynamic_node_id::server::Entry& entry) +int Log::append(const Entry& entry) { if ((last_index_ + 1) >= Capacity) { @@ -364,7 +364,7 @@ int Log::removeEntriesWhereIndexGreaterOrEqual(Index index) return 0; } -const protocol::dynamic_node_id::server::Entry* Log::getEntryAtIndex(Index index) const +const Entry* Log::getEntryAtIndex(Index index) const { UAVCAN_ASSERT(last_index_ < Capacity); return (index <= last_index_) ? &entries_[index] : NULL; @@ -397,7 +397,7 @@ int PersistentState::init() return res; } - const protocol::dynamic_node_id::server::Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); + const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); if (last_entry == NULL) { UAVCAN_ASSERT(0); @@ -603,7 +603,7 @@ void ClusterManager::handleTimerEvent(const TimerEvent&) /* * Filling the message */ - protocol::dynamic_node_id::server::Discovery msg; + Discovery msg; msg.configured_cluster_size = cluster_size_; msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0 @@ -639,7 +639,7 @@ void ClusterManager::handleTimerEvent(const TimerEvent&) } } -void ClusterManager::handleDiscovery(const ReceivedDataStructure& msg) +void ClusterManager::handleDiscovery(const ReceivedDataStructure& msg) { /* * Validating cluster configuration @@ -683,8 +683,7 @@ void ClusterManager::startDiscoveryPublishingTimerIfNotRunning() { if (!isRunning()) { - startPeriodic( - MonotonicDuration::fromMSec(protocol::dynamic_node_id::server::Discovery::BROADCASTING_INTERVAL_MS)); + startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_INTERVAL_MS)); } } From 5e56c8a61273f4ca210df3c8ffab75b2ce30acf0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 7 May 2015 13:58:24 +0300 Subject: [PATCH 023/268] Raft event tracing interface, more states of RaftCore, basic functions of RaftCore --- .../dynamic_node_id_allocation_server.hpp | 154 +++++++++++++--- .../uc_dynamic_node_id_allocation_server.cpp | 173 ++++++++++++++++++ .../dynamic_node_id_allocation_server.cpp | 62 ++++--- 3 files changed, 346 insertions(+), 43 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 80a8c21fb7..2f2e4fa58d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -70,6 +70,25 @@ public: virtual ~IDynamicNodeIDStorageBackend() { } }; +/** + * This interface allows the application to trace events that happen in the server. + */ +class IDynamicNodeIDAllocationServerEventTracer +{ +public: + /** + * The server invokes this method every time it believes that a noteworthy event has happened. + * The table of event codes can be found in the server sources. + * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures + * full backward compatibility. + * @param event_code Event code, see the sources for the enum with values. + * @param event_argument Value associated with the event; its meaning depends on the event code. + */ + virtual void onEvent(uint16_t event_code, int64_t event_argument) = 0; + + virtual ~IDynamicNodeIDAllocationServerEventTracer() { } +}; + /** * Internals, do not use anything from this namespace directly. */ @@ -83,6 +102,37 @@ using namespace protocol::dynamic_node_id::server; */ typedef StorageType::Type Term; +/** + * @ref IDynamicNodeIDAllocationServerEventTracer. + * Event codes cannot be changed, only new ones can be added. + */ +enum TraceEvent +{ + // Event name Argument + // 0 + TraceError, // error code (may be negated) + TraceLogLastIndexRestored, // recovered last index value + TraceLogAppend, // index of new entry + TraceLogRemove, // new last index value + TraceCurrentTermRestored, // current term + // 5 + TraceCurrentTermUpdate, // current term + TraceVotedForRestored, // value of votedFor + TraceVotedForUpdate, // value of votedFor + TraceDiscoveryBroadcast, // number of known servers + TraceNewServerDiscovered, // node ID of the new server + // 10 + TraceDiscoveryReceived, // node ID of the sender + TraceClusterSizeInited, // cluster size + TraceRaftCoreInited, // update interval in usec + TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader + TraceRaftModeSwitch, // 0 - Passive, 1 - Active + // 15 + TraceRaftNewLogEntry, // node ID value + + NumTraceEventCodes +}; + /** * This class extends the storage backend interface with serialization/deserialization functionality. */ @@ -137,6 +187,7 @@ public: private: IDynamicNodeIDStorageBackend& storage_; + IDynamicNodeIDAllocationServerEventTracer& tracer_; Entry entries_[Capacity]; Index last_index_; // Index zero always contains an empty entry @@ -149,8 +200,9 @@ private: int initEmptyLogStorage(); public: - Log(IDynamicNodeIDStorageBackend& storage) + Log(IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer) : storage_(storage) + , tracer_(tracer) , last_index_(0) { } @@ -195,6 +247,7 @@ public: class PersistentState { IDynamicNodeIDStorageBackend& storage_; + IDynamicNodeIDAllocationServerEventTracer& tracer_; Term current_term_; NodeID voted_for_; @@ -204,10 +257,11 @@ class PersistentState static IDynamicNodeIDStorageBackend::String getVotedForKey() { return "voted_for"; } public: - PersistentState(IDynamicNodeIDStorageBackend& storage) + PersistentState(IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer) : storage_(storage) + , tracer_(tracer) , current_term_(0) - , log_(storage) + , log_(storage, tracer) { } int init(); @@ -257,6 +311,7 @@ class ClusterManager : private TimerBase enum { MaxServers = Discovery::FieldTypes::known_nodes::MaxSize }; IDynamicNodeIDStorageBackend& storage_; + IDynamicNodeIDAllocationServerEventTracer& tracer_; const Log& log_; Subscriber discovery_sub_; @@ -293,9 +348,11 @@ public: * @param storage Needed to read the cluster size parameter from the storage * @param log Needed to initialize nextIndex[] values after elections */ - ClusterManager(INode& node, IDynamicNodeIDStorageBackend& storage, const Log& log) + ClusterManager(INode& node, IDynamicNodeIDStorageBackend& storage, const Log& log, + IDynamicNodeIDAllocationServerEventTracer& tracer) : TimerBase(node) , storage_(storage) + , tracer_(tracer) , log_(log) , discovery_sub_(node) , discovery_pub_(node) @@ -348,7 +405,15 @@ public: 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. + */ uint8_t getNumKnownServers() const { return num_known_servers_; } + + /** + * Cluster size and quorum size are constant. + */ uint8_t getClusterSize() const { return cluster_size_; } uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } @@ -366,13 +431,13 @@ class RaftCore : private TimerBase ServiceResponseDataStructure&)> AppendEntriesCallback; + typedef MethodBinder&)> + AppendEntriesResponseCallback; + typedef MethodBinder&, ServiceResponseDataStructure&)> RequestVoteCallback; - typedef MethodBinder&)> - AppendEntriesResponseCallback; - typedef MethodBinder&)> RequestVoteResponseCallback; @@ -383,48 +448,95 @@ class RaftCore : private TimerBase ServerStateLeader }; + IDynamicNodeIDAllocationServerEventTracer& tracer_; + + /* + * States + */ PersistentState persistent_state_; - Log::Index commit_index_; ClusterManager cluster_; + Log::Index commit_index_; MonotonicTime last_activity_timestamp_; bool active_mode_; - ServerState server_state_; - ServiceServer append_entries_srv_; - ServiceServer request_vote_srv_; + uint8_t next_server_index_; ///< Next server to query for AE or RV RPC + uint8_t num_votes_received_in_this_campaign_; + /* + * Transport + */ + ServiceServer append_entries_srv_; ServiceClient append_entries_client_; + ServiceServer request_vote_srv_; ServiceClient request_vote_client_; - virtual void handleTimerEvent(const TimerEvent&); + /** + * This constant defines the rate at which internal state updates happen. + * It also defines timeouts for AppendEntries and RequestVote RPCs. + */ + static MonotonicDuration getUpdateInterval() { return MonotonicDuration::fromMSec(50); } + + void trace(TraceEvent event, int64_t argument) { tracer_.onEvent(event, argument); } + + INode& getNode() { return append_entries_srv_.getNode(); } + + void updateFollower(const MonotonicTime& current_time); + void updateCandidate(const MonotonicTime& current_time); + void updateLeader(const MonotonicTime& current_time); + + void switchState(ServerState new_state); + + void handleAppendEntriesRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response); + + void handleAppendEntriesResponse(const ServiceCallResult& result); + + void handleRequestVoteRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response); + + void handleRequestVoteResponse(const ServiceCallResult& result); + + virtual void handleTimerEvent(const TimerEvent& event); public: - RaftCore(INode& node, IDynamicNodeIDStorageBackend& storage) + RaftCore(INode& node, IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer) : TimerBase(node) - , persistent_state_(storage) - , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero - , cluster_(node, storage, persistent_state_.getLog()) + , tracer_(tracer) + , 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) , server_state_(ServerStateFollower) + , next_server_index_(0) + , num_votes_received_in_this_campaign_(0) , append_entries_srv_(node) - , request_vote_srv_(node) , append_entries_client_(node) + , request_vote_srv_(node) , request_vote_client_(node) { } /** * Once started, the logic runs in the background until destructor is called. + * @param cluster_size If set, this value will be used and stored in the persistent storage. If not set, + * value from the persistent storage will be used. If not set and there's no such key + * in the persistent storage, initialization will fail. */ - int init(); + int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown); /** - * Inserts one entry into log. This operation may fail, which will not be reported. - * Failures are tolerble because all operations are idempotent. + * Only the leader can call @ref appendLog(). */ - void appendLog(const Entry& entry); + bool isLeader() const { return server_state_ == ServerStateLeader; } + + /** + * Inserts one entry into log. + * Failures are tolerble because all operations are idempotent. + * This method will trigger an assertion failure and return error if the current node is not the leader. + */ + int appendLog(const Entry& entry); /** * This class is used to perform log searches. diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index b3d7ff63c9..56bd9e8773 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -286,6 +286,8 @@ int Log::init() last_index_ = Index(value); } + tracer_.onEvent(TraceLogLastIndexRestored, last_index_); + // Restoring log entries - note that index 0 always exists for (Index index = 0; index <= last_index_; index++) { @@ -309,6 +311,8 @@ int Log::append(const Entry& entry) return -ErrLogic; } + tracer_.onEvent(TraceLogAppend, last_index_ + 1U); + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. int res = writeEntryToStorage(Index(last_index_ + 1), entry); if (res < 0) @@ -345,6 +349,8 @@ int Log::removeEntriesWhereIndexGreaterOrEqual(Index index) return -ErrLogic; } + tracer_.onEvent(TraceLogRemove, index - 1U); + MarshallingStorageDecorator io(storage_); uint32_t new_last_index = index - 1U; int res = io.setAndGetBack(getLastIndexKey(), new_last_index); @@ -437,6 +443,8 @@ int PersistentState::init() } } + tracer_.onEvent(TraceCurrentTermRestored, current_term_); + if (current_term_ < last_entry->term) { UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", @@ -481,6 +489,8 @@ int PersistentState::init() voted_for_ = NodeID(uint8_t(stored_voted_for)); } + tracer_.onEvent(TraceVotedForRestored, voted_for_.get()); + return 0; } @@ -492,6 +502,8 @@ int PersistentState::setCurrentTerm(const Term term) return -ErrInvalidParam; } + tracer_.onEvent(TraceCurrentTermUpdate, term); + MarshallingStorageDecorator io(storage_); Term tmp = term; @@ -518,6 +530,8 @@ int PersistentState::setVotedFor(const NodeID node_id) return -ErrInvalidParam; } + tracer_.onEvent(TraceVotedForUpdate, node_id.get()); + MarshallingStorageDecorator io(storage_); uint32_t tmp = node_id.get(); @@ -586,6 +600,7 @@ void ClusterManager::addServer(NodeID node_id) UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxServers - 2)); if (!isKnownServer(node_id) && node_id.isUnicast()) { + tracer_.onEvent(TraceNewServerDiscovered, node_id.get()); servers_[num_known_servers_].node_id = node_id; servers_[num_known_servers_].resetIndices(log_); num_known_servers_ = static_cast(num_known_servers_ + 1U); @@ -600,6 +615,8 @@ void ClusterManager::handleTimerEvent(const TimerEvent&) { UAVCAN_ASSERT(num_known_servers_ < cluster_size_); + tracer_.onEvent(TraceDiscoveryBroadcast, num_known_servers_); + /* * Filling the message */ @@ -641,6 +658,8 @@ void ClusterManager::handleTimerEvent(const TimerEvent&) void ClusterManager::handleDiscovery(const ReceivedDataStructure& msg) { + tracer_.onEvent(TraceDiscoveryReceived, msg.getSrcNodeID().get()); + /* * Validating cluster configuration * If there's a case of misconfiguration, the message will be ignored. @@ -730,6 +749,8 @@ int ClusterManager::init(const uint8_t init_cluster_size) } } + tracer_.onEvent(TraceClusterSizeInited, cluster_size_); + UAVCAN_ASSERT(cluster_size_ > 0); UAVCAN_ASSERT(cluster_size_ <= MaxServers); @@ -836,6 +857,158 @@ void ClusterManager::resetAllServerIndices() } } +/* + * RaftCore + */ +void RaftCore::updateFollower(const MonotonicTime& current_time) +{ + (void)current_time; +} + +void RaftCore::updateCandidate(const MonotonicTime& current_time) +{ + (void)current_time; +} + +void RaftCore::updateLeader(const MonotonicTime& current_time) +{ + (void)current_time; +} + +void RaftCore::switchState(ServerState new_state) +{ + if (server_state_ != new_state) + { + trace(TraceRaftStateSwitch, new_state); + } +} + +void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) +{ + (void)request; + (void)response; +} + +void RaftCore::handleAppendEntriesResponse(const ServiceCallResult& result) +{ + (void)result; +} + +void RaftCore::handleRequestVoteRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) +{ + (void)request; + (void)response; +} + +void RaftCore::handleRequestVoteResponse(const ServiceCallResult& result) +{ + (void)result; +} + +void RaftCore::handleTimerEvent(const TimerEvent& event) +{ + switch (server_state_) + { + case ServerStateFollower: + { + updateFollower(event.real_time); + break; + } + case ServerStateCandidate: + { + updateCandidate(event.real_time); + break; + } + case ServerStateLeader: + { + updateLeader(event.real_time); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } +} + +int RaftCore::init(uint8_t cluster_size) +{ + /* + * 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; + + /* + * Initializing internals + */ + int res = persistent_state_.init(); + if (res < 0) + { + return res; + } + + res = cluster_.init(cluster_size); + if (res < 0) + { + return res; + } + + res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest)); + if (res < 0) + { + return res; + } + + res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest)); + if (res < 0) + { + return res; + } + + res = append_entries_client_.init(); + if (res < 0) + { + return res; + } + append_entries_client_.setRequestTimeout(getUpdateInterval()); + + res = request_vote_client_.init(); + if (res < 0) + { + return res; + } + request_vote_client_.setRequestTimeout(getUpdateInterval()); + + startPeriodic(getUpdateInterval()); + + trace(TraceRaftCoreInited, getUpdateInterval().toUSec()); + + UAVCAN_ASSERT(res >= 0); + return 0; +} + +int RaftCore::appendLog(const Entry& entry) +{ + if (isLeader()) + { + trace(TraceRaftNewLogEntry, entry.node_id); + return persistent_state_.getLog().append(entry); + } + else + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } +} + } // dynamic_node_id_server_impl } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 567b386ba1..83e85618ad 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -53,6 +53,15 @@ public: }; +class EventTracer : public uavcan::IDynamicNodeIDAllocationServerEventTracer +{ + virtual void onEvent(uavcan::uint16_t event_code, uavcan::int64_t event_argument) + { + std::cout << "Event\t" << event_code << "\t" << event_argument << std::endl; + } +}; + + static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry @@ -140,10 +149,11 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) TEST(DynamicNodeIDAllocationServer, LogInitialization) { + EventTracer tracer; // No log data in the storage - initializing empty log { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, log.init()); @@ -157,7 +167,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) // Nonempty storage, one item { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); storage.set("log_last_index", "0"); ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found @@ -174,7 +184,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) // Nonempty storage, broken data { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); storage.set("log_last_index", "foobar"); ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value @@ -197,7 +207,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) // Nonempty storage, many items { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); storage.set("log_last_index", "1"); // 2 items - 0, 1 storage.set("log0_term", "0"); @@ -235,8 +245,9 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) TEST(DynamicNodeIDAllocationServer, LogAppend) { + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, log.init()); @@ -307,8 +318,9 @@ TEST(DynamicNodeIDAllocationServer, LogAppend) TEST(DynamicNodeIDAllocationServer, LogRemove) { + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); /* * Filling the log fully @@ -359,12 +371,13 @@ TEST(DynamicNodeIDAllocationServer, LogRemove) TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { + EventTracer tracer; /* * First initialization */ { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, pers.init()); @@ -382,12 +395,12 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { // This log is used to initialize the storage - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); ASSERT_LE(0, log.init()); } ASSERT_LE(1, storage.getNumKeys()); - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); ASSERT_LE(0, pers.init()); @@ -404,14 +417,14 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { // This log is used to initialize the storage - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); ASSERT_LE(0, log.init()); } ASSERT_LE(1, storage.getNumKeys()); storage.set("current_term", "1"); - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); ASSERT_GT(0, pers.init()); // Fails because current term is not zero @@ -432,7 +445,7 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { // This log is used to initialize the storage - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); ASSERT_LE(0, log.init()); uavcan::protocol::dynamic_node_id::server::Entry entry; @@ -443,7 +456,7 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) } ASSERT_LE(4, storage.getNumKeys()); - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); ASSERT_GT(0, pers.init()); // Fails because log is not empty @@ -468,8 +481,9 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) TEST(DynamicNodeIDAllocationServer, PersistentStorage) { + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage); + uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); /* * Initializing @@ -549,15 +563,17 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; + EventTracer tracer; + /* * Simple initialization */ { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); // Too big ASSERT_GT(0, mgr.init(MaxClusterSize + 1)); @@ -579,10 +595,10 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) */ { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); // Not configured ASSERT_GT(0, mgr.init()); @@ -601,11 +617,12 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerOneServer) uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); /* * Pub and sub @@ -675,11 +692,12 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage); + uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log); + uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); /* * Pub and sub From 944ac75d935a624cdd95d060af0d31451c6a11ef Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 7 May 2015 18:16:26 +0300 Subject: [PATCH 024/268] Raft AE and RV servers --- .../dynamic_node_id_allocation_server.hpp | 20 +- .../uc_dynamic_node_id_allocation_server.cpp | 296 +++++++++++++++--- 2 files changed, 277 insertions(+), 39 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 2f2e4fa58d..f826fd9189 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -124,11 +124,17 @@ enum TraceEvent // 10 TraceDiscoveryReceived, // node ID of the sender TraceClusterSizeInited, // cluster size + TraceInvalidClusterSizeReceived, // received cluster size TraceRaftCoreInited, // update interval in usec TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader - TraceRaftModeSwitch, // 0 - Passive, 1 - Active // 15 + TraceRaftModeSwitch, // 0 - Passive, 1 - Active TraceRaftNewLogEntry, // node ID value + TraceRaftRequestIgnored, // node ID of the client + TraceRaftVoteRequestReceived, // node ID of the client + TraceRaftPersistStateUpdateError, // negative error code + // 20 + TraceRaftCommitIndexUpdate, // new commit index value NumTraceEventCodes }; @@ -227,6 +233,7 @@ public: * Returned value indicates whether the requested operation has been carried out successfully. */ int removeEntriesWhereIndexGreaterOrEqual(Index index); + int removeEntriesWhereIndexGreater(Index index); /** * Returns nullptr if there's no such index. @@ -269,6 +276,7 @@ public: Term getCurrentTerm() const { return current_term_; } NodeID getVotedFor() const { return voted_for_; } + bool isVotedForSet() const { return voted_for_.isUnicast(); } Log& getLog() { return log_; } const Log& getLog() const { return log_; } @@ -282,6 +290,7 @@ public: * Invokes storage IO. */ int setVotedFor(NodeID node_id); + int resetVotedFor() { return setVotedFor(NodeID(0)); } }; /** @@ -331,7 +340,6 @@ class ClusterManager : private TimerBase Server* findServer(NodeID node_id); const Server* findServer(NodeID node_id) const; - bool isKnownServer(NodeID node_id) const; void addServer(NodeID node_id); virtual void handleTimerEvent(const TimerEvent&); @@ -368,6 +376,11 @@ public: */ int init(uint8_t init_cluster_size = ClusterSizeUnknown); + /** + * Whether such server has been discovered earlier. + */ + bool isKnownServer(NodeID node_id) const; + /** * An invalid node ID will be returned if there's no such server. * The local server is not listed there. @@ -482,11 +495,14 @@ class RaftCore : private TimerBase INode& getNode() { return append_entries_srv_.getNode(); } + void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); } + void updateFollower(const MonotonicTime& current_time); void updateCandidate(const MonotonicTime& current_time); void updateLeader(const MonotonicTime& current_time); void switchState(ServerState new_state); + void setActiveMode(bool new_active); void handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 56bd9e8773..83f6a12ee0 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -344,32 +344,41 @@ int Log::removeEntriesWhereIndexGreaterOrEqual(Index index) { UAVCAN_ASSERT(last_index_ < Capacity); - if (((index) >= Capacity) || (index == 0)) + if (((index) >= Capacity) || (index <= 0)) { return -ErrLogic; } - tracer_.onEvent(TraceLogRemove, index - 1U); - - MarshallingStorageDecorator io(storage_); uint32_t new_last_index = index - 1U; - int res = io.setAndGetBack(getLastIndexKey(), new_last_index); - if (res < 0) + + tracer_.onEvent(TraceLogRemove, new_last_index); + + if (new_last_index != last_index_) { - return res; + MarshallingStorageDecorator io(storage_); + int res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != index - 1U) + { + return -ErrFailure; + } + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Entries removed, last index %u --> %u", + unsigned(last_index_), unsigned(new_last_index)); + last_index_ = Index(new_last_index); } - if (new_last_index != index - 1U) - { - return -ErrFailure; - } - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Entries removed, last index %u --> %u", - unsigned(last_index_), unsigned(new_last_index)); - last_index_ = Index(new_last_index); // Removal operation leaves dangling entries in storage, it's OK return 0; } +int Log::removeEntriesWhereIndexGreater(Index index) +{ + return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U)); +} + const Entry* Log::getEntryAtIndex(Index index) const { UAVCAN_ASSERT(last_index_ < Capacity); @@ -577,24 +586,6 @@ const ClusterManager::Server* ClusterManager::findServer(NodeID node_id) const return const_cast(this)->findServer(node_id); } -bool ClusterManager::isKnownServer(NodeID node_id) const -{ - if (node_id == getNode().getNodeID()) - { - return true; - } - for (uint8_t i = 0; i < num_known_servers_; i++) - { - UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); - if (servers_[i].node_id == node_id) - { - return true; - } - } - return false; -} - void ClusterManager::addServer(NodeID node_id) { UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxServers - 2)); @@ -666,6 +657,7 @@ void ClusterManager::handleDiscovery(const ReceivedDataStructure& msg */ if (msg.configured_cluster_size != cluster_size_) { + tracer_.onEvent(TraceInvalidClusterSizeReceived, msg.configured_cluster_size); getNode().registerInternalFailure("Bad Raft cluster size"); return; } @@ -778,6 +770,24 @@ int ClusterManager::init(const uint8_t init_cluster_size) return 0; } +bool ClusterManager::isKnownServer(NodeID node_id) const +{ + if (node_id == getNode().getNodeID()) + { + return true; + } + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); + if (servers_[i].node_id == node_id) + { + return true; + } + } + return false; +} + NodeID ClusterManager::getRemoteServerNodeIDAtIndex(uint8_t index) const { if (index < num_known_servers_) @@ -875,19 +885,162 @@ void RaftCore::updateLeader(const MonotonicTime& current_time) (void)current_time; } -void RaftCore::switchState(ServerState new_state) +void RaftCore::switchState(const ServerState new_state) { if (server_state_ != new_state) { + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "State switch: %d --> %d", + int(server_state_), int(new_state)); trace(TraceRaftStateSwitch, new_state); + + server_state_ = new_state; + + cluster_.resetAllServerIndices(); + + if (server_state_ == ServerStateFollower) + { + setActiveMode(false); + } + } +} + +void RaftCore::setActiveMode(const bool new_active) +{ + if (active_mode_ != new_active) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Mode switch: %d --> %d", + int(active_mode_), int(new_active)); + trace(TraceRaftModeSwitch, new_active); + + active_mode_ = new_active; } } void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { - (void)request; - (void)response; + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + return; + } + + registerActivity(); + + UAVCAN_ASSERT(response.isResponseEnabled()); // This is default + + /* + * 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()) + { + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + switchState(ServerStateFollower); + + if (!response.isResponseEnabled()) + { + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + response.success = false; + + /* + * Step 1 (see Raft paper) + * Reject the request if the leader has stale term number. + */ + if (request.term < persistent_state_.getCurrentTerm()) + { + response.setResponseEnabled(true); + return; + } + + switchState(ServerStateFollower); + + /* + * Step 2 + * Reject the request if the assumed log index does not exist on the local node. + */ + const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index); + if (prev_entry == NULL) + { + response.setResponseEnabled(true); + return; + } + + /* + * Step 3 + * Drop log entries if term number does not match. + * Ignore the request if the persistent state cannot be updated. + */ + if (prev_entry->term != request.prev_log_term) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index); + response.setResponseEnabled(res >= 0); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + return; + } + + /* + * Step 4 + * Update the log with new entries - this will possibly require to rewrite existing entries. + * Ignore the request if the persistent state cannot be updated. + */ + if (request.prev_log_index != persistent_state_.getLog().getLastIndex()) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + + for (uint8_t i = 0; i < request.entries.size(); i++) + { + const int res = persistent_state_.getLog().append(request.entries[i]); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; // Response will not be sent, the server will assume that we're dead + } + } + + /* + * Step 5 + * Update the commit index. + */ + if (request.leader_commit > commit_index_) + { + commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex()); + trace(TraceRaftCommitIndexUpdate, commit_index_); + } + + response.setResponseEnabled(true); + response.success = true; } void RaftCore::handleAppendEntriesResponse(const ServiceCallResult& result) @@ -898,8 +1051,77 @@ void RaftCore::handleAppendEntriesResponse(const ServiceCallResult& request, ServiceResponseDataStructure& response) { - (void)request; - (void)response; + trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + return; + } + + 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()) + { + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + switchState(ServerStateFollower); + + if (!response.isResponseEnabled()) + { + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + + if (request.term < response.term) + { + response.vote_granted = false; + } + else + { + const bool can_vote = !persistent_state_.isVotedForSet() || + (persistent_state_.getVotedFor() == request.getSrcNodeID()); + const bool log_is_up_to_date = + persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term); + + response.vote_granted = can_vote && log_is_up_to_date; + + if (response.vote_granted) + { + registerActivity(); // This is necessary to avoid excessive elections + + const int res = persistent_state_.setVotedFor(request.getSrcNodeID()); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + } } void RaftCore::handleRequestVoteResponse(const ServiceCallResult& result) From ce752d93bd72f39d9b4b6006c1d6badb281f5f6f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 7 May 2015 19:11:13 +0300 Subject: [PATCH 025/268] Raft AE and RV RPC response handlers --- .../dynamic_node_id_allocation_server.hpp | 21 +++++- .../uc_dynamic_node_id_allocation_server.cpp | 74 ++++++++++++++++++- 2 files changed, 91 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index f826fd9189..8bc71db869 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -132,9 +132,11 @@ enum TraceEvent TraceRaftNewLogEntry, // node ID value TraceRaftRequestIgnored, // node ID of the client TraceRaftVoteRequestReceived, // node ID of the client - TraceRaftPersistStateUpdateError, // negative error code + TraceRaftVoteRequestSucceeded, // node ID of the server // 20 + TraceRaftPersistStateUpdateError, // negative error code TraceRaftCommitIndexUpdate, // new commit index value + TraceRaftNewerTermInResponse, // new term value NumTraceEventCodes }; @@ -461,6 +463,17 @@ class RaftCore : private TimerBase ServerStateLeader }; + struct PendingAppendEntriesFields + { + Log::Index prev_log_index; + Log::Index num_entries; + + PendingAppendEntriesFields() + : prev_log_index(0) + , num_entries(0) + { } + }; + IDynamicNodeIDAllocationServerEventTracer& tracer_; /* @@ -477,6 +490,8 @@ class RaftCore : private TimerBase uint8_t next_server_index_; ///< Next server to query for AE or RV RPC uint8_t num_votes_received_in_this_campaign_; + PendingAppendEntriesFields pending_append_entries_fields_; + /* * Transport */ @@ -489,7 +504,7 @@ class RaftCore : private TimerBase * This constant defines the rate at which internal state updates happen. * It also defines timeouts for AppendEntries and RequestVote RPCs. */ - static MonotonicDuration getUpdateInterval() { return MonotonicDuration::fromMSec(50); } + static MonotonicDuration getUpdateInterval() { return MonotonicDuration::fromMSec(100); } void trace(TraceEvent event, int64_t argument) { tracer_.onEvent(event, argument); } @@ -504,6 +519,8 @@ class RaftCore : private TimerBase void switchState(ServerState new_state); void setActiveMode(bool new_active); + void tryIncrementCurrentTermFromResponse(Term new_term); + void handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 83f6a12ee0..cf23c22265 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -901,6 +901,9 @@ void RaftCore::switchState(const ServerState new_state) { setActiveMode(false); } + + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; } } @@ -916,6 +919,18 @@ void RaftCore::setActiveMode(const bool new_active) } } +void RaftCore::tryIncrementCurrentTermFromResponse(Term new_term) +{ + trace(TraceRaftNewerTermInResponse, new_term); + const int res = persistent_state_.setCurrentTerm(new_term); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + registerActivity(); // Deferring future elections + switchState(ServerStateFollower); +} + void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { @@ -1045,7 +1060,32 @@ void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& result) { - (void)result; + if (!result.isSuccessful()) + { + return; + } + + if (result.response.term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.response.term); + } + else + { + if (result.response.success) + { + cluster_.incrementServerNextIndexBy(result.server_node_id, pending_append_entries_fields_.num_entries); + cluster_.setServerMatchIndex(result.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); + } + } + + pending_append_entries_fields_ = PendingAppendEntriesFields(); + // Rest of the logic is implemented in periodic update handlers. } void RaftCore::handleRequestVoteRequest(const ReceivedDataStructure& request, @@ -1126,11 +1166,41 @@ void RaftCore::handleRequestVoteRequest(const ReceivedDataStructure& result) { - (void)result; + if (server_state_ != ServerStateCandidate) + { + UAVCAN_ASSERT(num_votes_received_in_this_campaign_ == 0); // Making sure it was reset + return; // State has been switched, so we don't actually need the response anymore + } + + if (!result.isSuccessful()) + { + return; + } + + trace(TraceRaftVoteRequestSucceeded, result.server_node_id.get()); + + if (result.response.term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.response.term); + } + else + { + if (result.response.vote_granted) + { + num_votes_received_in_this_campaign_++; + } + } + // Rest of the logic is implemented in periodic update handlers. + // I'm no fan of asynchronous programming. At all. } void RaftCore::handleTimerEvent(const TimerEvent& event) { + if (cluster_.hadDiscoveryActivity()) + { + setActiveMode(true); + } + switch (server_state_) { case ServerStateFollower: From a1ee2efea0418d9c1f8ef3ca06f987b9ac4a5780 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 7 May 2015 19:48:05 +0300 Subject: [PATCH 026/268] All Raft logic finished except time updates --- .../dynamic_node_id_allocation_server.hpp | 3 ++ .../uc_dynamic_node_id_allocation_server.cpp | 50 +++++++++++++++++++ .../dynamic_node_id_allocation_server.cpp | 9 ++++ 3 files changed, 62 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 8bc71db869..8260f55093 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -137,6 +137,7 @@ enum TraceEvent TraceRaftPersistStateUpdateError, // negative error code TraceRaftCommitIndexUpdate, // new commit index value TraceRaftNewerTermInResponse, // new term value + TraceRaftNewEntryCommitted, // new commit index value NumTraceEventCodes }; @@ -521,6 +522,8 @@ class RaftCore : private TimerBase void tryIncrementCurrentTermFromResponse(Term new_term); + void propagateCommitIndex(); + void handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index cf23c22265..85d83010dc 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -883,6 +883,7 @@ void RaftCore::updateCandidate(const MonotonicTime& current_time) void RaftCore::updateLeader(const MonotonicTime& current_time) { (void)current_time; + propagateCommitIndex(); } void RaftCore::switchState(const ServerState new_state) @@ -931,6 +932,55 @@ void RaftCore::tryIncrementCurrentTermFromResponse(Term new_term) switchState(ServerStateFollower); } +void RaftCore::propagateCommitIndex() +{ + // Objective is to estimate whether we can safely increment commit index value + UAVCAN_ASSERT(server_state_ == ServerStateLeader); + + 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; + } + } + + if (commit_index_fully_replicated && cluster_.isClusterDiscovered()) + { + setActiveMode(false); // Commit index is the same on all nodes, enabling passive mode + } + } + else + { + // Not all local entries are committed + setActiveMode(true); + + uint8_t num_nodes_with_next_log_entry_available = 1; // Local node + for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) + { + const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); + if (match_index > commit_index_) + { + num_nodes_with_next_log_entry_available++; + } + } + + if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize()) + { + commit_index_++; + trace(TraceRaftNewEntryCommitted, commit_index_); + // AT THIS POINT ALLOCATION IS COMPLETE + } + } +} + void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 83e85618ad..fa5bd78964 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -801,3 +801,12 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); } + + +TEST(DynamicNodeIDAllocationServer, ObjectSizes) +{ + std::cout << "Log: " << sizeof(uavcan::dynamic_node_id_server_impl::Log) << std::endl; + std::cout << "PersistentState: " << sizeof(uavcan::dynamic_node_id_server_impl::PersistentState) << std::endl; + std::cout << "ClusterManager: " << sizeof(uavcan::dynamic_node_id_server_impl::ClusterManager) << std::endl; + std::cout << "RaftCore: " << sizeof(uavcan::dynamic_node_id_server_impl::RaftCore) << std::endl; +} From 1a640e67631566cf1a7ae9aa1697eecf0f12c6ab Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 12:29:28 +0300 Subject: [PATCH 027/268] Properly defined timeouts and max cluster size --- .../server/1011.Discovery.uavcan | 2 +- .../server/220.AppendEntries.uavcan | 4 +++ .../dynamic_node_id_allocation_server.hpp | 30 ++++++++++------ .../uc_dynamic_node_id_allocation_server.cpp | 35 +++++++++++-------- .../dynamic_node_id_allocation_server.cpp | 10 +++--- 5 files changed, 50 insertions(+), 31 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan index 33f15dd1fe..fb34c2757c 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/1011.Discovery.uavcan @@ -17,4 +17,4 @@ uint8 configured_cluster_size # Node ID of servers that are known to the publishing server, including the publishing server itself. # Capacity of this array defines maximum size of the server cluster. # -uint8[<=7] known_nodes +uint8[<=5] known_nodes diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index b6d6253bf7..5551e160f5 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -3,6 +3,10 @@ # 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 + uint32 term uint8 prev_log_index uint8 prev_log_term diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 8260f55093..4f1e046a0a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -475,6 +475,12 @@ class RaftCore : private TimerBase { } }; + /* + * Constants + */ + const MonotonicDuration update_interval_; ///< AE requests will be issued at this rate + const MonotonicDuration base_activity_timeout_; + IDynamicNodeIDAllocationServerEventTracer& tracer_; /* @@ -501,21 +507,17 @@ class RaftCore : private TimerBase ServiceServer request_vote_srv_; ServiceClient request_vote_client_; - /** - * This constant defines the rate at which internal state updates happen. - * It also defines timeouts for AppendEntries and RequestVote RPCs. - */ - static MonotonicDuration getUpdateInterval() { return MonotonicDuration::fromMSec(100); } - void trace(TraceEvent event, int64_t argument) { tracer_.onEvent(event, argument); } - INode& getNode() { return append_entries_srv_.getNode(); } + INode& getNode() { return append_entries_srv_.getNode(); } + const INode& getNode() const { return append_entries_srv_.getNode(); } void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); } + bool isActivityTimedOut() const; - void updateFollower(const MonotonicTime& current_time); - void updateCandidate(const MonotonicTime& current_time); - void updateLeader(const MonotonicTime& current_time); + void updateFollower(); + void updateCandidate(); + void updateLeader(); void switchState(ServerState new_state); void setActiveMode(bool new_active); @@ -537,8 +539,14 @@ class RaftCore : private TimerBase virtual void handleTimerEvent(const TimerEvent& event); public: - RaftCore(INode& node, IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer) + RaftCore(INode& node, IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer, + MonotonicDuration update_interval = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS), + MonotonicDuration base_activity_timeout = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_BASE_ELECTION_TIMEOUT_MS)) : TimerBase(node) + , update_interval_(update_interval) + , base_activity_timeout_(base_activity_timeout) , tracer_(tracer) , persistent_state_(storage, tracer) , cluster_(node, storage, persistent_state_.getLog(), tracer) diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 85d83010dc..3a913c9058 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -870,19 +870,26 @@ void ClusterManager::resetAllServerIndices() /* * RaftCore */ -void RaftCore::updateFollower(const MonotonicTime& current_time) +bool RaftCore::isActivityTimedOut() const { - (void)current_time; + const int multiplier = static_cast(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); } -void RaftCore::updateCandidate(const MonotonicTime& current_time) +void RaftCore::updateFollower() { - (void)current_time; } -void RaftCore::updateLeader(const MonotonicTime& current_time) +void RaftCore::updateCandidate() +{ +} + +void RaftCore::updateLeader() { - (void)current_time; propagateCommitIndex(); } @@ -1244,7 +1251,7 @@ void RaftCore::handleRequestVoteResponse(const ServiceCallResult& r // I'm no fan of asynchronous programming. At all. } -void RaftCore::handleTimerEvent(const TimerEvent& event) +void RaftCore::handleTimerEvent(const TimerEvent&) { if (cluster_.hadDiscoveryActivity()) { @@ -1255,17 +1262,17 @@ void RaftCore::handleTimerEvent(const TimerEvent& event) { case ServerStateFollower: { - updateFollower(event.real_time); + updateFollower(); break; } case ServerStateCandidate: { - updateCandidate(event.real_time); + updateCandidate(); break; } case ServerStateLeader: { - updateLeader(event.real_time); + updateLeader(); break; } default: @@ -1320,18 +1327,18 @@ int RaftCore::init(uint8_t cluster_size) { return res; } - append_entries_client_.setRequestTimeout(getUpdateInterval()); + append_entries_client_.setRequestTimeout(update_interval_); res = request_vote_client_.init(); if (res < 0) { return res; } - request_vote_client_.setRequestTimeout(getUpdateInterval()); + request_vote_client_.setRequestTimeout(update_interval_); - startPeriodic(getUpdateInterval()); + startPeriodic(update_interval_); - trace(TraceRaftCoreInited, getUpdateInterval().toUSec()); + trace(TraceRaftCoreInited, update_interval_.toUSec()); UAVCAN_ASSERT(res >= 0); return 0; diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index fa5bd78964..a31ded55a8 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -580,14 +580,14 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) ASSERT_EQ(0, storage.getNumKeys()); // OK - ASSERT_LE(0, mgr.init(7)); + ASSERT_LE(0, mgr.init(5)); ASSERT_EQ(1, storage.getNumKeys()); - ASSERT_EQ("7", storage.get("cluster_size")); + ASSERT_EQ("5", storage.get("cluster_size")); // Testing other states ASSERT_EQ(0, mgr.getNumKnownServers()); - ASSERT_EQ(7, mgr.getClusterSize()); - ASSERT_EQ(4, mgr.getQuorumSize()); + ASSERT_EQ(5, mgr.getClusterSize()); + ASSERT_EQ(3, mgr.getQuorumSize()); ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid()); } /* @@ -605,7 +605,7 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) ASSERT_EQ(0, storage.getNumKeys()); // OK - storage.set("cluster_size", "7"); + storage.set("cluster_size", "5"); ASSERT_LE(0, mgr.init()); ASSERT_EQ(1, storage.getNumKeys()); } From 6985c72dd32d27ba489482c79e6d7df2aada7329 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 13:30:55 +0300 Subject: [PATCH 028/268] Follower and candidate logic implemented --- .../server/221.RequestVote.uavcan | 2 +- .../dynamic_node_id_allocation_server.hpp | 26 ++-- .../uc_dynamic_node_id_allocation_server.cpp | 111 +++++++++++++++--- 3 files changed, 113 insertions(+), 26 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan index 958be2612e..113c6ee3d2 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/221.RequestVote.uavcan @@ -4,8 +4,8 @@ # uint32 term +uint32 last_log_term uint8 last_log_index -uint8 last_log_term --- diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 4f1e046a0a..1fa392ec46 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -134,6 +134,7 @@ enum TraceEvent TraceRaftVoteRequestReceived, // node ID of the client TraceRaftVoteRequestSucceeded, // node ID of the server // 20 + TraceRaftVoteRequestInitiation, // node ID of the server TraceRaftPersistStateUpdateError, // negative error code TraceRaftCommitIndexUpdate, // new commit index value TraceRaftNewerTermInResponse, // new term value @@ -301,6 +302,10 @@ public: */ class ClusterManager : private TimerBase { +public: + enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize }; + +private: typedef MethodBinder&)> @@ -320,8 +325,6 @@ class ClusterManager : private TimerBase void resetIndices(const Log& log); }; - enum { MaxServers = Discovery::FieldTypes::known_nodes::MaxSize }; - IDynamicNodeIDStorageBackend& storage_; IDynamicNodeIDAllocationServerEventTracer& tracer_; const Log& log_; @@ -329,7 +332,7 @@ class ClusterManager : private TimerBase Subscriber discovery_sub_; mutable Publisher discovery_pub_; - Server servers_[MaxServers - 1]; ///< Minus one because the local server is not listed there. + Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there. uint8_t cluster_size_; uint8_t num_known_servers_; @@ -504,8 +507,11 @@ class RaftCore : private TimerBase */ ServiceServer append_entries_srv_; ServiceClient append_entries_client_; - ServiceServer request_vote_srv_; - ServiceClient request_vote_client_; + ServiceServer request_vote_srv_; + + enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 }; + LazyConstructor > + request_vote_clients_[NumRequestVoteClients]; void trace(TraceEvent event, int64_t argument) { tracer_.onEvent(event, argument); } @@ -515,6 +521,8 @@ class RaftCore : private TimerBase void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); } bool isActivityTimedOut() const; + void handlePersistentStateUpdateError(int error); + void updateFollower(); void updateCandidate(); void updateLeader(); @@ -559,8 +567,12 @@ public: , append_entries_srv_(node) , append_entries_client_(node) , request_vote_srv_(node) - , request_vote_client_(node) - { } + { + for (uint8_t i = 0; i < NumRequestVoteClients; i++) + { + request_vote_clients_[i].construct(node); + } + } /** * Once started, the logic runs in the background until destructor is called. diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 3a913c9058..c8a7ba4410 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -588,7 +588,7 @@ const ClusterManager::Server* ClusterManager::findServer(NodeID node_id) const void ClusterManager::addServer(NodeID node_id) { - UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxServers - 2)); + UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); if (!isKnownServer(node_id) && node_id.isUnicast()) { tracer_.onEvent(TraceNewServerDiscovered, node_id.get()); @@ -715,7 +715,7 @@ int ClusterManager::init(const uint8_t init_cluster_size) "Cluster size is neither configured nor stored in the storage"); return res; } - if ((value == 0) || (value > MaxServers)) + if ((value == 0) || (value > MaxClusterSize)) { UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster size is invalid"); return -ErrFailure; @@ -724,7 +724,7 @@ int ClusterManager::init(const uint8_t init_cluster_size) } else { - if ((init_cluster_size == 0) || (init_cluster_size > MaxServers)) + if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize)) { return -ErrInvalidParam; } @@ -744,7 +744,7 @@ int ClusterManager::init(const uint8_t init_cluster_size) tracer_.onEvent(TraceClusterSizeInited, cluster_size_); UAVCAN_ASSERT(cluster_size_ > 0); - UAVCAN_ASSERT(cluster_size_ <= MaxServers); + UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize); /* * Initializing pub/sub and timer @@ -880,12 +880,81 @@ bool RaftCore::isActivityTimedOut() const return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout); } +void RaftCore::handlePersistentStateUpdateError(int error) +{ + UAVCAN_ASSERT(error < 0); + trace(TraceRaftPersistStateUpdateError, error); + switchState(ServerStateFollower); + setActiveMode(false); // Goodnight sweet prince + registerActivity(); // Deferring reelections +} + void RaftCore::updateFollower() { + if (active_mode_ && isActivityTimedOut()) + { + switchState(ServerStateCandidate); + setActiveMode(true); + registerActivity(); + } } void RaftCore::updateCandidate() { + if (num_votes_received_in_this_campaign_ > 0) + { + const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); + + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Election complete, won: %d", int(won)); + + switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader + setActiveMode(true); + } + else + { + // Set votedFor, abort on failure + int res = persistent_state_.setVotedFor(getNode().getNodeID()); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + // Increment current term, abort on failure + res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + num_votes_received_in_this_campaign_ = 1; // Voting for self + + RequestVote::Request req; + req.last_log_index = persistent_state_.getLog().getLastIndex(); + 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++) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + if (!node_id.isUnicast()) + { + break; + } + + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Requesting vote from %d", int(node_id.get())); + trace(TraceRaftVoteRequestInitiation, node_id.get()); + + res = request_vote_clients_[i]->call(node_id, req); + if (res < 0) + { + trace(TraceError, res); + } + } + + UAVCAN_ASSERT(res >= 0); + } } void RaftCore::updateLeader() @@ -905,13 +974,14 @@ void RaftCore::switchState(const ServerState new_state) cluster_.resetAllServerIndices(); - if (server_state_ == ServerStateFollower) - { - setActiveMode(false); - } - 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(); } } @@ -937,6 +1007,7 @@ void RaftCore::tryIncrementCurrentTermFromResponse(Term new_term) } registerActivity(); // Deferring future elections switchState(ServerStateFollower); + setActiveMode(false); } void RaftCore::propagateCommitIndex() @@ -1022,6 +1093,7 @@ void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& result) { + UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + if (!result.isSuccessful()) { return; @@ -1181,6 +1256,7 @@ void RaftCore::handleRequestVoteRequest(const ReceivedDataStructure& result) { - if (server_state_ != ServerStateCandidate) - { - UAVCAN_ASSERT(num_votes_received_in_this_campaign_ == 0); // Making sure it was reset - return; // State has been switched, so we don't actually need the response anymore - } + UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled if (!result.isSuccessful()) { @@ -1329,12 +1401,15 @@ int RaftCore::init(uint8_t cluster_size) } append_entries_client_.setRequestTimeout(update_interval_); - res = request_vote_client_.init(); - if (res < 0) + for (uint8_t i = 0; i < NumRequestVoteClients; i++) { - return res; + res = request_vote_clients_[i]->init(); + if (res < 0) + { + return res; + } + request_vote_clients_[i]->setRequestTimeout(update_interval_); } - request_vote_client_.setRequestTimeout(update_interval_); startPeriodic(update_interval_); From a7c09ed714cceeb1e44e22077382f8e5186951c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 14:41:15 +0300 Subject: [PATCH 029/268] Raft Leader implementation. RaftCore is now finished. --- .../server/220.AppendEntries.uavcan | 2 +- .../dynamic_node_id_allocation_server.hpp | 4 +- .../uc_dynamic_node_id_allocation_server.cpp | 45 +++++++++++++++++++ 3 files changed, 49 insertions(+), 2 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index 5551e160f5..2e49f4c26c 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -8,8 +8,8 @@ uint16 DEFAULT_REQUEST_TIMEOUT_MS = 100 uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1000 # request timeout * max cluster size * 2 requests uint32 term +uint32 prev_log_term uint8 prev_log_index -uint8 prev_log_term uint8 leader_commit # Request message fits one CAN frame when there's no entries to send. Entry[<=5] entries diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 1fa392ec46..8c2ec8ce1e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -139,6 +139,8 @@ enum TraceEvent TraceRaftCommitIndexUpdate, // new commit index value TraceRaftNewerTermInResponse, // new term value TraceRaftNewEntryCommitted, // new commit index value + // 25 + TraceRaftAppendEntriesCallFailure, // error code (may be negated) NumTraceEventCodes }; @@ -497,7 +499,7 @@ class RaftCore : private TimerBase bool active_mode_; ServerState server_state_; - uint8_t next_server_index_; ///< Next server to query for AE or RV RPC + uint8_t next_server_index_; ///< Next server to query AE from uint8_t num_votes_received_in_this_campaign_; PendingAppendEntriesFields pending_append_entries_fields_; diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index c8a7ba4410..6ff4bfc61c 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -960,6 +960,51 @@ void RaftCore::updateCandidate() void RaftCore::updateLeader() { propagateCommitIndex(); + + // Leader simply emits one AppendEntry at every update, iterating over all available servers + if (next_server_index_ >= cluster_.getClusterSize()) + { + next_server_index_ = 0; + } + + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); + UAVCAN_ASSERT(node_id.isUnicast()); + + AppendEntries::Request req; + req.term = persistent_state_.getCurrentTerm(); + req.leader_commit = commit_index_; + + req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); + + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); + if (entry == NULL) + { + UAVCAN_ASSERT(0); + handlePersistentStateUpdateError(-ErrLogic); + return; + } + + req.prev_log_term = entry->term; + + for (Log::Index index = cluster_.getServerNextIndex(node_id); + index <= persistent_state_.getLog().getLastIndex(); + index++) + { + req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); + if (req.entries.size() == req.entries.capacity()) + { + break; + } + } + + pending_append_entries_fields_.num_entries = req.entries.size(); + pending_append_entries_fields_.prev_log_index = req.prev_log_index; + + const int res = append_entries_client_.call(node_id, req); + if (res < 0) + { + trace(TraceRaftAppendEntriesCallFailure, res); + } } void RaftCore::switchState(const ServerState new_state) From 43f849cc10a277e2004a153d3ab391298ff44373 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 14:57:27 +0300 Subject: [PATCH 030/268] Log commit callback - needed by the main allocator class to broadcast allocation responses --- .../dynamic_node_id_allocation_server.hpp | 21 ++++++++++++++++++- .../uc_dynamic_node_id_allocation_server.cpp | 4 ++++ 2 files changed, 24 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 8c2ec8ce1e..e04b8e7043 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -441,6 +441,20 @@ public: bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); } }; +/** + * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log. + */ +class ILeaderLogCommitHandler +{ +public: + /** + * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). + */ + virtual void onEntryCommitted(const Entry& entry) = 0; + + virtual ~ILeaderLogCommitHandler() { } +}; + /** * This class implements log replication and voting. * It does not implement client-server interaction at all; instead it just exposes a public method for adding @@ -487,6 +501,7 @@ class RaftCore : private TimerBase const MonotonicDuration base_activity_timeout_; IDynamicNodeIDAllocationServerEventTracer& tracer_; + ILeaderLogCommitHandler& log_commit_handler_; /* * States @@ -549,7 +564,10 @@ class RaftCore : private TimerBase virtual void handleTimerEvent(const TimerEvent& event); public: - RaftCore(INode& node, IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer, + RaftCore(INode& node, + IDynamicNodeIDStorageBackend& storage, + IDynamicNodeIDAllocationServerEventTracer& tracer, + ILeaderLogCommitHandler& log_commit_handler, MonotonicDuration update_interval = MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS), MonotonicDuration base_activity_timeout = @@ -558,6 +576,7 @@ public: , update_interval_(update_interval) , base_activity_timeout_(base_activity_timeout) , tracer_(tracer) + , log_commit_handler_(log_commit_handler) , persistent_state_(storage, tracer) , cluster_(node, storage, persistent_state_.getLog(), tracer) , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 6ff4bfc61c..4638ac75fb 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -1059,6 +1059,7 @@ void RaftCore::propagateCommitIndex() { // Objective is to estimate whether we can safely increment commit index value UAVCAN_ASSERT(server_state_ == ServerStateLeader); + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); if (commit_index_ == persistent_state_.getLog().getLastIndex()) { @@ -1098,8 +1099,11 @@ void RaftCore::propagateCommitIndex() if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize()) { commit_index_++; + UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed trace(TraceRaftNewEntryCommitted, commit_index_); + // AT THIS POINT ALLOCATION IS COMPLETE + log_commit_handler_.onEntryCommitted(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); } } } From 7d607a4dec02226c0d6076e5498b81c9e063fe06 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 15:02:27 +0300 Subject: [PATCH 031/268] Added missing tests for Raft --- .../dynamic_node_id_allocation_server.cpp | 18 ++++++++++++++++-- 1 file changed, 16 insertions(+), 2 deletions(-) diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index a31ded55a8..e852f488d6 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -355,14 +355,17 @@ TEST(DynamicNodeIDAllocationServer, LogRemove) storage.failOnSetCalls(false); ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); ASSERT_EQ(59, log.getLastIndex()); - ASSERT_EQ("59", storage.get("log_last_index")); + ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30)); + ASSERT_EQ(30, log.getLastIndex()); + ASSERT_EQ("30", storage.get("log_last_index")); + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1)); ASSERT_EQ(0, log.getLastIndex()); - ASSERT_EQ("0", storage.get("log_last_index")); storage.print(); @@ -521,16 +524,27 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorage) /* * Changing votedFor */ + ASSERT_FALSE(pers.isVotedForSet()); ASSERT_EQ(0, pers.getVotedFor().get()); ASSERT_LE(0, pers.setVotedFor(0)); ASSERT_EQ(0, pers.getVotedFor().get()); ASSERT_LE(0, pers.setVotedFor(45)); ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_TRUE(pers.isVotedForSet()); ASSERT_EQ("1", storage.get("log_last_index")); ASSERT_EQ("2", storage.get("current_term")); ASSERT_EQ("45", storage.get("voted_for")); + ASSERT_LE(0, pers.resetVotedFor()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ("0", storage.get("voted_for")); + + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_TRUE(pers.isVotedForSet()); + ASSERT_EQ("45", storage.get("voted_for")); + /* * Handling errors */ From 1f7c0b40b3c63345bae14be587eb5f7b75049fc0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 16:35:36 +0300 Subject: [PATCH 032/268] Basic Raft test --- .../uc_dynamic_node_id_allocation_server.cpp | 6 +- .../dynamic_node_id_allocation_server.cpp | 59 ++++++++++++++++++- 2 files changed, 61 insertions(+), 4 deletions(-) diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 4638ac75fb..98b0ef0106 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -952,8 +952,6 @@ void RaftCore::updateCandidate() trace(TraceError, res); } } - - UAVCAN_ASSERT(res >= 0); } } @@ -1374,7 +1372,7 @@ void RaftCore::handleRequestVoteResponse(const ServiceCallResult& r void RaftCore::handleTimerEvent(const TimerEvent&) { - if (cluster_.hadDiscoveryActivity()) + if (cluster_.hadDiscoveryActivity() && isLeader()) { setActiveMode(true); } @@ -1448,6 +1446,7 @@ int RaftCore::init(uint8_t cluster_size) { return res; } + append_entries_client_.setCallback(AppendEntriesResponseCallback(this, &RaftCore::handleAppendEntriesResponse)); append_entries_client_.setRequestTimeout(update_interval_); for (uint8_t i = 0; i < NumRequestVoteClients; i++) @@ -1457,6 +1456,7 @@ int RaftCore::init(uint8_t cluster_size) { return res; } + request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); request_vote_clients_[i]->setRequestTimeout(update_interval_); } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index e852f488d6..55a74f93f4 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -55,10 +55,31 @@ public: class EventTracer : public uavcan::IDynamicNodeIDAllocationServerEventTracer { + const std::string id_; + virtual void onEvent(uavcan::uint16_t event_code, uavcan::int64_t event_argument) { - std::cout << "Event\t" << event_code << "\t" << event_argument << std::endl; + std::cout << "EVENT [" << id_ << "]\t" << event_code << "\t" << event_argument << std::endl; } + +public: + EventTracer() { } + + EventTracer(const std::string& id) : id_(id) { } +}; + + +class CommitHandler : public uavcan::dynamic_node_id_server_impl::ILeaderLogCommitHandler +{ + const std::string id_; + + virtual void onEntryCommitted(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + { + std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; + } + +public: + CommitHandler(const std::string& id) : id_(id) { } }; @@ -817,6 +838,42 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) } +TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + EventTracer tracer_a("a"); + EventTracer tracer_b("b"); + StorageBackend storage_a; + StorageBackend storage_b; + CommitHandler commit_handler_a("a"); + CommitHandler commit_handler_b("b"); + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::dynamic_node_id_server_impl::RaftCore raft_a(nodes.a, storage_a, tracer_a, commit_handler_a); + uavcan::dynamic_node_id_server_impl::RaftCore raft_b(nodes.b, storage_b, tracer_b, commit_handler_b); + + /* + * Initialization + */ + ASSERT_LE(0, raft_a.init(2)); + ASSERT_LE(0, raft_b.init(2)); + + /* + * Running and trying not to fall + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + // The one with lower node ID must become a leader + ASSERT_TRUE(raft_a.isLeader()); + ASSERT_FALSE(raft_b.isLeader()); +} + + TEST(DynamicNodeIDAllocationServer, ObjectSizes) { std::cout << "Log: " << sizeof(uavcan::dynamic_node_id_server_impl::Log) << std::endl; From dab32220e09066e379ab2f95d372286558afafa5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 17:14:12 +0300 Subject: [PATCH 033/268] Improved Raft event tracer - added event code to string conversion --- .../dynamic_node_id_allocation_server.hpp | 10 ++++- .../uc_dynamic_node_id_allocation_server.cpp | 45 +++++++++++++++++++ .../dynamic_node_id_allocation_server.cpp | 21 ++++++++- 3 files changed, 73 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index e04b8e7043..0d9a57255b 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -76,6 +76,14 @@ public: class IDynamicNodeIDAllocationServerEventTracer { public: +#if UAVCAN_TOSTRING + /** + * It is safe to call this function with any argument. + * If the event code is out of range, an assertion failure will be triggered and an error text will be returned. + */ + static const char* getEventName(uint16_t code); +#endif + /** * The server invokes this method every time it believes that a noteworthy event has happened. * The table of event codes can be found in the server sources. @@ -658,7 +666,7 @@ public: } }; -} // namespace dynamic_node_id_impl +} // namespace dynamic_node_id_server_impl /** * diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 98b0ef0106..5707b165e0 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -15,6 +15,51 @@ namespace uavcan { +/* + * IDynamicNodeIDAllocationServerEventTracer + */ +#if UAVCAN_TOSTRING +const char* IDynamicNodeIDAllocationServerEventTracer::getEventName(uint16_t code) +{ + using namespace dynamic_node_id_server_impl; + + // import re + // make_strings = lambda s: ',\n'.join('"%s"' % x for x in re.findall(r'\ \ \ \ Trace([A-Za-z0-9]+),', s)) + static const char* const Strings[NumTraceEventCodes] = + { + "Error", + "LogLastIndexRestored", + "LogAppend", + "LogRemove", + "CurrentTermRestored", + "CurrentTermUpdate", + "VotedForRestored", + "VotedForUpdate", + "DiscoveryBroadcast", + "NewServerDiscovered", + "DiscoveryReceived", + "ClusterSizeInited", + "InvalidClusterSizeReceived", + "RaftCoreInited", + "RaftStateSwitch", + "RaftModeSwitch", + "RaftNewLogEntry", + "RaftRequestIgnored", + "RaftVoteRequestReceived", + "RaftVoteRequestSucceeded", + "RaftVoteRequestInitiation", + "RaftPersistStateUpdateError", + "RaftCommitIndexUpdate", + "RaftNewerTermInResponse", + "RaftNewEntryCommitted", + "RaftAppendEntriesCallFailure" + }; + uavcan::StaticAssert::check(); + UAVCAN_ASSERT(code < NumTraceEventCodes); + return (code < NumTraceEventCodes) ? Strings[code] : "INVALID_EVENT_CODE"; +} +#endif + namespace dynamic_node_id_server_impl { /* diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 55a74f93f4..5de7b7a213 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -57,9 +57,9 @@ class EventTracer : public uavcan::IDynamicNodeIDAllocationServerEventTracer { const std::string id_; - virtual void onEvent(uavcan::uint16_t event_code, uavcan::int64_t event_argument) + virtual void onEvent(uavcan::uint16_t code, uavcan::int64_t argument) { - std::cout << "EVENT [" << id_ << "]\t" << event_code << "\t" << event_argument << std::endl; + std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; } public: @@ -874,6 +874,23 @@ TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) } +TEST(DynamicNodeIDAllocationServer, EventCodeToString) +{ + using uavcan::IDynamicNodeIDAllocationServerEventTracer; + using namespace uavcan::dynamic_node_id_server_impl; + + // Simply checking some error codes + ASSERT_STREQ("Error", + IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceError)); + ASSERT_STREQ("RaftModeSwitch", + IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftModeSwitch)); + ASSERT_STREQ("RaftAppendEntriesCallFailure", + IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); + ASSERT_STREQ("DiscoveryReceived", + IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceDiscoveryReceived)); +} + + TEST(DynamicNodeIDAllocationServer, ObjectSizes) { std::cout << "Log: " << sizeof(uavcan::dynamic_node_id_server_impl::Log) << std::endl; From 1d7e83bd71df9c9d348ef1fe6babd9d149e71380 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 17:32:22 +0300 Subject: [PATCH 034/268] Raft log append test --- .../dynamic_node_id_allocation_server.hpp | 7 ++++++- .../uc_dynamic_node_id_allocation_server.cpp | 7 ++++++- .../dynamic_node_id_allocation_server.cpp | 16 ++++++++++++++++ 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 0d9a57255b..8521859e84 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -611,6 +611,11 @@ public: */ int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown); + /** + * This function is mostly needed for testing. + */ + Log::Index getCommitIndex() const { return commit_index_; } + /** * Only the leader can call @ref appendLog(). */ @@ -621,7 +626,7 @@ public: * Failures are tolerble because all operations are idempotent. * This method will trigger an assertion failure and return error if the current node is not the leader. */ - int appendLog(const Entry& entry); + int appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id); /** * This class is used to perform log searches. diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 5707b165e0..6d216a484e 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -1513,10 +1513,15 @@ int RaftCore::init(uint8_t cluster_size) return 0; } -int RaftCore::appendLog(const Entry& entry) +int RaftCore::appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) { if (isLeader()) { + Entry entry; + entry.node_id = node_id.get(); + entry.unique_id = unique_id; + entry.term = persistent_state_.getCurrentTerm(); + trace(TraceRaftNewLogEntry, entry.node_id); return persistent_state_.getLog().append(entry); } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 5de7b7a213..2d8dfd97b3 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -871,6 +871,22 @@ TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) // The one with lower node ID must become a leader ASSERT_TRUE(raft_a.isLeader()); ASSERT_FALSE(raft_b.isLeader()); + + ASSERT_EQ(0, raft_a.getCommitIndex()); + ASSERT_EQ(0, raft_b.getCommitIndex()); + + /* + * Adding some stuff + */ + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id unique_id; + uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); + + ASSERT_LE(0, raft_a.appendLog(unique_id, uavcan::NodeID(1))); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_EQ(1, raft_a.getCommitIndex()); + ASSERT_EQ(1, raft_b.getCommitIndex()); } From e48fa77d85bce9c7dfce2b88369c1beecd9f5c29 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 18:43:27 +0300 Subject: [PATCH 035/268] Raft logic fixes & more tests --- .../dynamic_node_id_allocation_server.hpp | 2 +- .../uc_dynamic_node_id_allocation_server.cpp | 100 +++++++++--------- .../dynamic_node_id_allocation_server.cpp | 67 +++++++++--- 3 files changed, 101 insertions(+), 68 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 8521859e84..e71c6752f5 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -136,7 +136,7 @@ enum TraceEvent TraceRaftCoreInited, // update interval in usec TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader // 15 - TraceRaftModeSwitch, // 0 - Passive, 1 - Active + TraceRaftActiveSwitch, // 0 - Passive, 1 - Active TraceRaftNewLogEntry, // node ID value TraceRaftRequestIgnored, // node ID of the client TraceRaftVoteRequestReceived, // node ID of the client diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index 6d216a484e..db6a793f29 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -42,7 +42,7 @@ const char* IDynamicNodeIDAllocationServerEventTracer::getEventName(uint16_t cod "InvalidClusterSizeReceived", "RaftCoreInited", "RaftStateSwitch", - "RaftModeSwitch", + "RaftActiveSwitch", "RaftNewLogEntry", "RaftRequestIgnored", "RaftVoteRequestReceived", @@ -939,13 +939,14 @@ void RaftCore::updateFollower() if (active_mode_ && isActivityTimedOut()) { switchState(ServerStateCandidate); - setActiveMode(true); registerActivity(); } } void RaftCore::updateCandidate() { + UAVCAN_ASSERT(active_mode_); + if (num_votes_received_in_this_campaign_ > 0) { const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); @@ -953,7 +954,6 @@ void RaftCore::updateCandidate() UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Election complete, won: %d", int(won)); switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader - setActiveMode(true); } else { @@ -1002,52 +1002,55 @@ void RaftCore::updateCandidate() void RaftCore::updateLeader() { - propagateCommitIndex(); - - // Leader simply emits one AppendEntry at every update, iterating over all available servers - if (next_server_index_ >= cluster_.getClusterSize()) + if (active_mode_ || (next_server_index_ > 0)) { - next_server_index_ = 0; - } + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); + UAVCAN_ASSERT(node_id.isUnicast()); - const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); - UAVCAN_ASSERT(node_id.isUnicast()); - - AppendEntries::Request req; - req.term = persistent_state_.getCurrentTerm(); - req.leader_commit = commit_index_; - - req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); - - const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); - if (entry == NULL) - { - UAVCAN_ASSERT(0); - handlePersistentStateUpdateError(-ErrLogic); - return; - } - - req.prev_log_term = entry->term; - - for (Log::Index index = cluster_.getServerNextIndex(node_id); - index <= persistent_state_.getLog().getLastIndex(); - index++) - { - req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); - if (req.entries.size() == req.entries.capacity()) + next_server_index_++; + if (next_server_index_ >= cluster_.getNumKnownServers()) { - break; + next_server_index_ = 0; + } + + AppendEntries::Request req; + req.term = persistent_state_.getCurrentTerm(); + req.leader_commit = commit_index_; + + req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); + + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); + if (entry == NULL) + { + UAVCAN_ASSERT(0); + handlePersistentStateUpdateError(-ErrLogic); + return; + } + + req.prev_log_term = entry->term; + + for (Log::Index index = cluster_.getServerNextIndex(node_id); + index <= persistent_state_.getLog().getLastIndex(); + index++) + { + req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); + if (req.entries.size() == req.entries.capacity()) + { + break; + } + } + + pending_append_entries_fields_.num_entries = req.entries.size(); + pending_append_entries_fields_.prev_log_index = req.prev_log_index; + + const int res = append_entries_client_.call(node_id, req); + if (res < 0) + { + trace(TraceRaftAppendEntriesCallFailure, res); } } - pending_append_entries_fields_.num_entries = req.entries.size(); - pending_append_entries_fields_.prev_log_index = req.prev_log_index; - - const int res = append_entries_client_.call(node_id, req); - if (res < 0) - { - trace(TraceRaftAppendEntriesCallFailure, res); - } + propagateCommitIndex(); } void RaftCore::switchState(const ServerState new_state) @@ -1077,9 +1080,9 @@ void RaftCore::setActiveMode(const bool new_active) { if (active_mode_ != new_active) { - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Mode switch: %d --> %d", + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Active switch: %d --> %d", int(active_mode_), int(new_active)); - trace(TraceRaftModeSwitch, new_active); + trace(TraceRaftActiveSwitch, new_active); active_mode_ = new_active; } @@ -1119,10 +1122,8 @@ void RaftCore::propagateCommitIndex() } } - if (commit_index_fully_replicated && cluster_.isClusterDiscovered()) - { - setActiveMode(false); // Commit index is the same on all nodes, enabling passive mode - } + 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 { @@ -1348,7 +1349,6 @@ void RaftCore::handleRequestVoteRequest(const ReceivedDataStructure */ +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + #include #include +#include #include #include "helpers.hpp" @@ -840,10 +846,13 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) { + using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::protocol::dynamic_node_id::server; + uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; - uavcan::DefaultDataTypeRegistrator _reg2; - uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; EventTracer tracer_a("a"); EventTracer tracer_b("b"); @@ -854,14 +863,14 @@ TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::RaftCore raft_a(nodes.a, storage_a, tracer_a, commit_handler_a); - uavcan::dynamic_node_id_server_impl::RaftCore raft_b(nodes.b, storage_b, tracer_b, commit_handler_b); + std::auto_ptr raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + std::auto_ptr raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b)); /* * Initialization */ - ASSERT_LE(0, raft_a.init(2)); - ASSERT_LE(0, raft_b.init(2)); + ASSERT_LE(0, raft_a->init(2)); + ASSERT_LE(0, raft_b->init(2)); /* * Running and trying not to fall @@ -869,24 +878,48 @@ TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); // The one with lower node ID must become a leader - ASSERT_TRUE(raft_a.isLeader()); - ASSERT_FALSE(raft_b.isLeader()); + ASSERT_TRUE(raft_a->isLeader()); + ASSERT_FALSE(raft_b->isLeader()); - ASSERT_EQ(0, raft_a.getCommitIndex()); - ASSERT_EQ(0, raft_b.getCommitIndex()); + ASSERT_EQ(0, raft_a->getCommitIndex()); + ASSERT_EQ(0, raft_b->getCommitIndex()); /* * Adding some stuff */ - uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id unique_id; + Entry::FieldTypes::unique_id unique_id; uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); - ASSERT_LE(0, raft_a.appendLog(unique_id, uavcan::NodeID(1))); + ASSERT_LE(0, raft_a->appendLog(unique_id, uavcan::NodeID(1))); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); - ASSERT_EQ(1, raft_a.getCommitIndex()); - ASSERT_EQ(1, raft_b.getCommitIndex()); + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); + + /* + * Terminating the leader - the Follower will continue to sleep + */ + raft_a.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + /* + * Reinitializing the leader - current Follower will become the new Leader + */ + storage_a.reset(); + + raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + ASSERT_LE(0, raft_a->init(2)); + ASSERT_EQ(0, raft_a->getCommitIndex()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + ASSERT_FALSE(raft_a->isLeader()); + ASSERT_TRUE(raft_b->isLeader()); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); } @@ -898,8 +931,8 @@ TEST(DynamicNodeIDAllocationServer, EventCodeToString) // Simply checking some error codes ASSERT_STREQ("Error", IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceError)); - ASSERT_STREQ("RaftModeSwitch", - IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftModeSwitch)); + ASSERT_STREQ("RaftActiveSwitch", + IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftActiveSwitch)); ASSERT_STREQ("RaftAppendEntriesCallFailure", IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); ASSERT_STREQ("DiscoveryReceived", From 6a35e65ecc38e1efdb72a1facaf7283c59742699 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 19:53:30 +0300 Subject: [PATCH 036/268] Removed misleading comment --- .../protocol/dynamic_node_id/server/220.AppendEntries.uavcan | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index 2e49f4c26c..f650e91306 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -11,7 +11,7 @@ uint32 term uint32 prev_log_term uint8 prev_log_index uint8 leader_commit -# Request message fits one CAN frame when there's no entries to send. + Entry[<=5] entries --- From 952009c284d40dac9f2f0eccf33319b83b5e242d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 20:00:30 +0300 Subject: [PATCH 037/268] Allocation request manager (untested) --- .../dynamic_node_id_allocation_server.hpp | 74 ++++++- .../uc_dynamic_node_id_allocation_server.cpp | 186 ++++++++++++++++++ .../dynamic_node_id_allocation_server.cpp | 24 ++- 3 files changed, 273 insertions(+), 11 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index e71c6752f5..f8bbe2d434 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -671,6 +671,67 @@ public: } }; +/** + * The main allocator must implement this interface. + */ +class IAllocationRequestHandler +{ +public: + typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID; + + virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; + + virtual ~IAllocationRequestHandler() { } +}; + +/** + * This class manages communication with allocation clients. + * Three-stage unique ID exchange is implemented here, as well as response publication. + */ +class AllocationRequestManager +{ + typedef MethodBinder&)> + AllocationCallback; + + const MonotonicDuration stage_timeout_; + + bool active_; + MonotonicTime last_message_timestamp_; + protocol::dynamic_node_id::Allocation::FieldTypes::unique_id current_unique_id_; + + IAllocationRequestHandler& handler_; + + Subscriber allocation_sub_; + Publisher allocation_pub_; + + enum { InvalidStage = 0 }; + + static uint8_t detectRequestStage(const protocol::dynamic_node_id::Allocation& msg); + uint8_t getExpectedStage() const; + + void broadcastIntermediateAllocationResponse(); + + void handleAllocation(const ReceivedDataStructure& msg); + +public: + AllocationRequestManager(INode& node, IAllocationRequestHandler& handler) + : stage_timeout_(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS)) + , active_(false) + , handler_(handler) + , allocation_sub_(node) + , allocation_pub_(node) + { } + + int init(); + + void setActive(bool x); + bool isActive() const { return active_; } + + int broadcastAllocationResponse(const IAllocationRequestHandler::UniqueID& unique_id, NodeID allocated_node_id); +}; + } // namespace dynamic_node_id_server_impl /** @@ -678,21 +739,20 @@ public: */ class DynamicNodeIDAllocationServer { - typedef MethodBinder&)> - AllocationCallback; - typedef MethodBinder&)> NodeStatusCallback; typedef Map PendingGetNodeInfoAttemptsMap; + /* + * States + */ PendingGetNodeInfoAttemptsMap pending_get_node_info_attempts_; - Subscriber allocation_sub_; - Publisher allocation_pub_; + /* + * Transport + */ Subscriber node_status_sub_; diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index db6a793f29..cfa7ffdb07 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -799,6 +799,7 @@ int ClusterManager::init(const uint8_t init_cluster_size) { return res; } + (void)discovery_pub_.setPriority(TransferPriorityLow); res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery)); if (res < 0) @@ -1532,6 +1533,191 @@ int RaftCore::appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID no } } +/* + * AllocationRequestManager + */ +uint8_t AllocationRequestManager::detectRequestStage(const protocol::dynamic_node_id::Allocation& msg) +{ + const uint8_t max_bytes_per_request = protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + + if ((msg.unique_id.size() != max_bytes_per_request) && + (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) && + (msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD + { + return InvalidStage; + } + if (msg.first_part_of_unique_id) + { + return 1; // Note that CAN FD frames can deliver the unique ID in one stage! + } + if (msg.unique_id.size() == protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + if (msg.unique_id.size() < protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 3; + } + return InvalidStage; +} + +uint8_t AllocationRequestManager::getExpectedStage() const +{ + if (current_unique_id_.empty()) + { + return 1; + } + if (current_unique_id_.size() >= (protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) + { + return 3; + } + if (current_unique_id_.size() >= protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + return InvalidStage; +} + +void AllocationRequestManager::broadcastIntermediateAllocationResponse() +{ + UAVCAN_ASSERT(active_); + + protocol::dynamic_node_id::Allocation msg; + msg.unique_id = current_unique_id_; + UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); + + const int res = allocation_pub_.broadcast(msg); + if (res < 0) + { + allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast"); + } +} + +void AllocationRequestManager::handleAllocation(const ReceivedDataStructure& msg) +{ + if (!msg.isAnonymousTransfer()) + { + return; // This is a response from another allocator, ignore + } + + if (!active_) + { + return; // The local node is not the leader, ignore + } + + /* + * Reset the expected stage on timeout + */ + if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_)) + { + UAVCAN_TRACE("AllocationRequestReceiver", "Stage timeout, reset"); + current_unique_id_.clear(); + } + last_message_timestamp_ = msg.getMonotonicTimestamp(); + + /* + * Checking if request stage matches the expected stage + */ + const uint8_t request_stage = detectRequestStage(msg); + if (request_stage == InvalidStage) + { + return; // No way + } + + const uint8_t expected_stage = getExpectedStage(); + if (request_stage == InvalidStage) + { + current_unique_id_.clear(); + return; + } + + if (request_stage != expected_stage) + { + return; // Ignore - stage mismatch + } + + const uint8_t max_expected_bytes = static_cast(current_unique_id_.capacity() - current_unique_id_.size()); + UAVCAN_ASSERT(max_expected_bytes > 0); + if (msg.unique_id.size() > max_expected_bytes) + { + return; // Malformed request + } + + /* + * Updating the local state + */ + for (uint8_t i = 0; i < msg.unique_id.size(); i++) + { + current_unique_id_.push_back(msg.unique_id[i]); + } + + /* + * Proceeding with allocation if possible + */ + if (current_unique_id_.size() == current_unique_id_.capacity()) + { + UAVCAN_TRACE("AllocationRequestReceiver", "Allocation request received; preferred node ID: %d", + int(msg.node_id)); + + IAllocationRequestHandler::UniqueID unique_id; + copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); + current_unique_id_.clear(); + + handler_.handleAllocationRequest(unique_id, msg.node_id); + } + else + { + broadcastIntermediateAllocationResponse(); + } +} + +int AllocationRequestManager::init() +{ + int res = allocation_pub_.init(); + if (res < 0) + { + return res; + } + (void)allocation_pub_.setPriority(TransferPriorityLow); + + res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation)); + if (res < 0) + { + return res; + } + allocation_sub_.allowAnonymousTransfers(); + + return 0; +} + +void AllocationRequestManager::setActive(bool x) +{ + active_ = x; + if (!active_) + { + current_unique_id_.clear(); + } +} + +int AllocationRequestManager::broadcastAllocationResponse(const IAllocationRequestHandler::UniqueID& unique_id, + NodeID allocated_node_id) +{ + if (!active_) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + protocol::dynamic_node_id::Allocation msg; + + msg.unique_id.resize(msg.unique_id.capacity()); + copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); + + msg.node_id = allocated_node_id.get(); + + return allocation_pub_.broadcast(msg); +} + } // dynamic_node_id_server_impl } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index c33f2ddf13..e28f31c0b9 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -940,10 +940,26 @@ TEST(DynamicNodeIDAllocationServer, EventCodeToString) } +TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) +{ + +} + + TEST(DynamicNodeIDAllocationServer, ObjectSizes) { - std::cout << "Log: " << sizeof(uavcan::dynamic_node_id_server_impl::Log) << std::endl; - std::cout << "PersistentState: " << sizeof(uavcan::dynamic_node_id_server_impl::PersistentState) << std::endl; - std::cout << "ClusterManager: " << sizeof(uavcan::dynamic_node_id_server_impl::ClusterManager) << std::endl; - std::cout << "RaftCore: " << sizeof(uavcan::dynamic_node_id_server_impl::RaftCore) << std::endl; + std::cout << "Log: " + << sizeof(uavcan::dynamic_node_id_server_impl::Log) << std::endl; + + std::cout << "PersistentState: " + << sizeof(uavcan::dynamic_node_id_server_impl::PersistentState) << std::endl; + + std::cout << "ClusterManager: " + << sizeof(uavcan::dynamic_node_id_server_impl::ClusterManager) << std::endl; + + std::cout << "RaftCore: " + << sizeof(uavcan::dynamic_node_id_server_impl::RaftCore) << std::endl; + + std::cout << "AllocationRequestManager: " + << sizeof(uavcan::dynamic_node_id_server_impl::AllocationRequestManager) << std::endl; } From 618e4c766acade4faed41bc383beaa8eaef5fe15 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 20:27:05 +0300 Subject: [PATCH 038/268] Tests for Allocation request manager --- .../uc_dynamic_node_id_allocation_server.cpp | 7 +- .../dynamic_node_id_allocation_server.cpp | 94 +++++++++++++++++++ 2 files changed, 99 insertions(+), 2 deletions(-) diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index cfa7ffdb07..b22c382012 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -1586,6 +1586,9 @@ void AllocationRequestManager::broadcastIntermediateAllocationResponse() msg.unique_id = current_unique_id_; UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); + UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", + unsigned(msg.unique_id.size())); + const int res = allocation_pub_.broadcast(msg); if (res < 0) { @@ -1610,7 +1613,7 @@ void AllocationRequestManager::handleAllocation(const ReceivedDataStructure (last_message_timestamp_ + stage_timeout_)) { - UAVCAN_TRACE("AllocationRequestReceiver", "Stage timeout, reset"); + UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); current_unique_id_.clear(); } last_message_timestamp_ = msg.getMonotonicTimestamp(); @@ -1656,7 +1659,7 @@ void AllocationRequestManager::handleAllocation(const ReceivedDataStructure #include #include +#include #include "helpers.hpp" class StorageBackend : public uavcan::IDynamicNodeIDStorageBackend @@ -89,6 +90,47 @@ public: }; +class AllocationRequestHandler : public uavcan::dynamic_node_id_server_impl::IAllocationRequestHandler +{ + std::vector > requests_; + +public: + virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + requests_.push_back(std::pair(unique_id, preferred_node_id)); + } + + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + if (requests_.empty()) + { + std::cout << "No pending requests" << std::endl; + return false; + } + + const std::pair pair = requests_.at(requests_.size() - 1U); + requests_.pop_back(); + + if (pair.first != unique_id) + { + std::cout << "Unique ID mismatch" << std::endl; + return false; + } + + if (pair.second != preferred_node_id) + { + std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")" + << std::endl; + return false; + } + + return true; + } + + void reset() { requests_.clear(); } +}; + + static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry @@ -942,7 +984,59 @@ TEST(DynamicNodeIDAllocationServer, EventCodeToString) TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) { + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server_impl; + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDAllocationClient client(nodes.b); + + /* + * Client initialization + */ + 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)); + + /* + * Request manager initialization + */ + AllocationRequestHandler handler; + + AllocationRequestManager manager(nodes.a, handler); + + ASSERT_LE(0, manager.init()); + + ASSERT_FALSE(manager.isActive()); + manager.setActive(true); + ASSERT_TRUE(manager.isActive()); + + /* + * Allocation + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(handler.matchAndPopLastRequest(hwver.unique_id, PreferredNodeID)); + + ASSERT_LE(0, manager.broadcastAllocationResponse(hwver.unique_id, PreferredNodeID)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + /* + * Checking the client + */ + ASSERT_TRUE(client.isAllocationComplete()); + + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); } From aca9fcb12c3b164e61ccd536f35757566c8f0d8f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 21:00:17 +0300 Subject: [PATCH 039/268] Very basic implementation of DynamicNodeIDAllocationServer - not all logic is implemented yet, but it can be used for testing already --- .../dynamic_node_id_allocation_server.hpp | 35 ++++++++++++++++-- .../uc_dynamic_node_id_allocation_server.cpp | 37 +++++++++++++++++++ .../dynamic_node_id_allocation_server.cpp | 2 + 3 files changed, 71 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index f8bbe2d434..4aa98056aa 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -735,27 +735,56 @@ public: } // namespace dynamic_node_id_server_impl /** - * + * This class implements the top-level allocation logic and server API. */ -class DynamicNodeIDAllocationServer +class DynamicNodeIDAllocationServer : public dynamic_node_id_server_impl::IAllocationRequestHandler + , public dynamic_node_id_server_impl::ILeaderLogCommitHandler { typedef MethodBinder&)> NodeStatusCallback; + typedef MethodBinder&)> + GetNodeInfoResponseCallback; + typedef Map PendingGetNodeInfoAttemptsMap; + enum { MaxGetNodeInfoAttempts = 5 }; + /* * States */ PendingGetNodeInfoAttemptsMap pending_get_node_info_attempts_; + dynamic_node_id_server_impl::RaftCore raft_core_; + dynamic_node_id_server_impl::AllocationRequestManager allocation_request_manager_; /* * Transport */ - Subscriber node_status_sub_; + ServiceClient get_node_info_client_; + INode& getNode() { return get_node_info_client_.getNode(); } + + virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id); + + virtual void onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry); + +public: + DynamicNodeIDAllocationServer(INode& node, + IDynamicNodeIDStorageBackend& storage, + IDynamicNodeIDAllocationServerEventTracer& tracer) + : pending_get_node_info_attempts_(node.getAllocator()) + , raft_core_(node, storage, tracer, *this) + , allocation_request_manager_(node, *this) + , node_status_sub_(node) + , get_node_info_client_(node) + { } + + enum { ClusterSizeUnknown = dynamic_node_id_server_impl::ClusterManager::ClusterSizeUnknown }; + + int init(uint8_t cluster_size = ClusterSizeUnknown); }; } diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp index b22c382012..588e944ac3 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp @@ -1723,4 +1723,41 @@ int AllocationRequestManager::broadcastAllocationResponse(const IAllocationReque } // dynamic_node_id_server_impl +/* + * DynamicNodeIDAllocationServer + */ +void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) +{ + // TODO implement proper allocation logic + (void)raft_core_.appendLog(unique_id, preferred_node_id); +} + +void DynamicNodeIDAllocationServer::onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry) +{ + const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); + if (res < 0) + { + getNode().registerInternalFailure("Dynamic allocation final broadcast"); + } +} + +int DynamicNodeIDAllocationServer::init(uint8_t cluster_size) +{ + int res = raft_core_.init(cluster_size); + if (res < 0) + { + return res; + } + + res = allocation_request_manager_.init(); + if (res < 0) + { + return res; + } + + // TODO Initialize the node info transport + + return 0; +} + } diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 6468d44474..840f4b5adc 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -1056,4 +1056,6 @@ TEST(DynamicNodeIDAllocationServer, ObjectSizes) std::cout << "AllocationRequestManager: " << sizeof(uavcan::dynamic_node_id_server_impl::AllocationRequestManager) << std::endl; + + std::cout << "DynamicNodeIDAllocationServer: " << sizeof(uavcan::DynamicNodeIDAllocationServer) << std::endl; } From 69c361e259d15d0e459bb7a8b93fdca6c4d7b628 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 8 May 2015 23:12:57 +0300 Subject: [PATCH 040/268] Proper allocation logic with basic test --- .../dynamic_node_id_allocation_server.hpp | 22 ++- .../uc_dynamic_node_id_allocation_server.cpp | 136 +++++++++++++++++- .../dynamic_node_id_allocation_server.cpp | 52 +++++++ 3 files changed, 203 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp index 4aa98056aa..3e1315a9af 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp @@ -460,6 +460,11 @@ public: */ virtual void onEntryCommitted(const Entry& entry) = 0; + /** + * Assume false by default. + */ + virtual void onLeaderChange(bool local_node_is_leader) = 0; + virtual ~ILeaderLogCommitHandler() { } }; @@ -655,7 +660,7 @@ public: inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const { UAVCAN_ASSERT(try_implicit_cast(predicate, true)); - for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index--; index >= 0) + for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index >= 0; index--) { const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); UAVCAN_ASSERT(entry != NULL); @@ -737,8 +742,8 @@ public: /** * This class implements the top-level allocation logic and server API. */ -class DynamicNodeIDAllocationServer : public dynamic_node_id_server_impl::IAllocationRequestHandler - , public dynamic_node_id_server_impl::ILeaderLogCommitHandler +class DynamicNodeIDAllocationServer : private dynamic_node_id_server_impl::IAllocationRequestHandler + , private dynamic_node_id_server_impl::ILeaderLogCommitHandler { typedef MethodBinder 0)) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); @@ -1062,6 +1067,12 @@ void RaftCore::switchState(const ServerState new_state) int(server_state_), int(new_state)); trace(TraceRaftStateSwitch, new_state); + if ((ServerStateLeader == server_state_) || + (ServerStateLeader == new_state)) + { + log_commit_handler_.onLeaderChange(ServerStateLeader == new_state); + } + server_state_ = new_state; cluster_.resetAllServerIndices(); @@ -1721,18 +1732,137 @@ int AllocationRequestManager::broadcastAllocationResponse(const IAllocationReque return allocation_pub_.broadcast(msg); } +/* + * There's no reason to remove these types from the class scope, except that otherwise the half-broken Eclipse CDT + * indexer goes bananas. + */ +struct UniqueIDLogPredicate +{ + const IAllocationRequestHandler::UniqueID unique_id; + + UniqueIDLogPredicate(const IAllocationRequestHandler::UniqueID& uid) + : unique_id(uid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.unique_id == unique_id; + } +}; + +struct NodeIDLogPredicate +{ + const NodeID node_id; + + NodeIDLogPredicate(const NodeID& nid) + : node_id(nid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.node_id == node_id.get(); + } +}; + } // dynamic_node_id_server_impl /* * DynamicNodeIDAllocationServer */ -void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) +bool DynamicNodeIDAllocationServer::isNodeIDTaken(const NodeID node_id) const { - // TODO implement proper allocation logic - (void)raft_core_.appendLog(unique_id, preferred_node_id); + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Testing if node ID %d is taken", int(node_id.get())); + return raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::NodeIDLogPredicate(node_id)); +} + +NodeID DynamicNodeIDAllocationServer::findFreeNodeID(const NodeID preferred_node_id) const +{ + uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + + // Up + while (candidate <= NodeID::Max) + { + if (!isNodeIDTaken(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + candidate--; // This has been tested already + + // Down + while (candidate > 0) + { + if (!isNodeIDTaken(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); +} + +void DynamicNodeIDAllocationServer::allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) +{ + const NodeID allocated_node_id = findFreeNodeID(preferred_node_id); + if (!allocated_node_id.isUnicast()) + { + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Request ignored - no free node ID left"); + return; + } + + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "New node ID allocated: %d", int(allocated_node_id.get())); + const int res = raft_core_.appendLog(unique_id, allocated_node_id); + if (res < 0) + { + getNode().registerInternalFailure("Raft log append"); + } +} + +void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) +{ + // TODO: allocation requests must not be served if the list of unidentified nodes is not empty + + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::UniqueIDLogPredicate(unique_id)); + + if (result.isConstructed()) + { + if (result->committed) + { + tryPublishAllocationResult(result->entry); + UAVCAN_TRACE("DynamicNodeIDAllocationServer", + "Allocation request served with existing allocation; node ID %d", + int(result->entry.node_id)); + } + else + { + UAVCAN_TRACE("DynamicNodeIDAllocationServer", + "Allocation request ignored - allocation exists but not committed yet; node ID %d", + int(result->entry.node_id)); + } + } + else + { + allocateNewNode(unique_id, preferred_node_id); + } } void DynamicNodeIDAllocationServer::onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry) +{ + tryPublishAllocationResult(entry); +} + +void DynamicNodeIDAllocationServer::onLeaderChange(bool local_node_is_leader) +{ + UAVCAN_TRACE("DynamicNodeIDAllocationServer", "I am leader: %d", int(local_node_is_leader)); + allocation_request_manager_.setActive(local_node_is_leader); +} + +void DynamicNodeIDAllocationServer::tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) { const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); if (res < 0) diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 840f4b5adc..47da8124f3 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -85,6 +85,11 @@ class CommitHandler : public uavcan::dynamic_node_id_server_impl::ILeaderLogComm std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; } + virtual void onLeaderChange(bool local_node_is_leader) + { + std::cout << "I AM LEADER: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + } + public: CommitHandler(const std::string& id) : id_(id) { } }; @@ -1040,6 +1045,53 @@ TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) } +TEST(DynamicNodeIDAllocationServer, Main) +{ + using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + + EventTracer tracer; + StorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + /* + * Server + */ + uavcan::DynamicNodeIDAllocationServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(1)); + + /* + * Client + */ + uavcan::DynamicNodeIDAllocationClient client(nodes.b); + uavcan::protocol::HardwareVersion hwver; + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, client.start(hwver, PreferredNodeID)); + + /* + * Fire + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(4000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); +} + + TEST(DynamicNodeIDAllocationServer, ObjectSizes) { std::cout << "Log: " From 8ea708b77ee7d5f4be3854c78182c06aebfe733e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 11:55:36 +0300 Subject: [PATCH 041/268] Renamed DynamicNodeIDClient --- ..._client.hpp => dynamic_node_id_client.hpp} | 14 ++++---- ...ient.cpp => uc_dynamic_node_id_client.cpp} | 32 +++++++++---------- .../dynamic_node_id_allocation_server.cpp | 6 ++-- ..._client.cpp => dynamic_node_id_client.cpp} | 10 +++--- 4 files changed, 30 insertions(+), 32 deletions(-) rename libuavcan/include/uavcan/protocol/{dynamic_node_id_allocation_client.hpp => dynamic_node_id_client.hpp} (89%) rename libuavcan/src/protocol/{uc_dynamic_node_id_allocation_client.cpp => uc_dynamic_node_id_client.cpp} (79%) rename libuavcan/test/protocol/{dynamic_node_id_allocation_client.cpp => dynamic_node_id_client.cpp} (95%) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp similarity index 89% rename from libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp rename to libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp index 0833ffe24e..00d654d558 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED -#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED #include #include @@ -26,10 +26,10 @@ namespace uavcan * * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. */ -class DynamicNodeIDAllocationClient : private TimerBase +class DynamicNodeIDClient : private TimerBase { - typedef MethodBinder&)> AllocationCallback; @@ -48,7 +48,7 @@ class DynamicNodeIDAllocationClient : private TimerBase void handleAllocation(const ReceivedDataStructure& msg); public: - DynamicNodeIDAllocationClient(INode& node) + DynamicNodeIDClient(INode& node) : TimerBase(node) , dnida_pub_(node) , dnida_sub_(node) @@ -91,4 +91,4 @@ public: } -#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_CLIENT_HPP_INCLUDED diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp similarity index 79% rename from libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp rename to libuavcan/src/protocol/uc_dynamic_node_id_client.cpp index 915c4b3b56..f6150ed3ac 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp @@ -2,19 +2,19 @@ * Copyright (C) 2015 Pavel Kirienko */ -#include +#include namespace uavcan { -void DynamicNodeIDAllocationClient::terminate() +void DynamicNodeIDClient::terminate() { - UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated"); + UAVCAN_TRACE("DynamicNodeIDClient", "Client terminated"); stop(); dnida_sub_.stop(); } -void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) +void DynamicNodeIDClient::handleTimerEvent(const TimerEvent&) { // This method implements Rule B UAVCAN_ASSERT(preferred_node_id_.isValid()); @@ -35,17 +35,16 @@ void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)); // Broadcasting - UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Broadcasting 1st stage: preferred ID: %d", + UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 1st stage: preferred ID: %d", static_cast(preferred_node_id_.get())); const int res = dnida_pub_.broadcast(msg); if (res < 0) { - dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed"); + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); } } -void DynamicNodeIDAllocationClient::handleAllocation( - const ReceivedDataStructure& msg) +void DynamicNodeIDClient::handleAllocation(const ReceivedDataStructure& msg) { /* * TODO This method can blow the stack easily @@ -59,7 +58,7 @@ void DynamicNodeIDAllocationClient::handleAllocation( } startPeriodic(getPeriod()); // Restarting the timer - Rule C - UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Request timer reset because of Allocation message from %d", + UAVCAN_TRACE("DynamicNodeIDClient", "Request timer reset because of Allocation message from %d", static_cast(msg.getSrcNodeID().get())); // Rule D @@ -88,13 +87,12 @@ void DynamicNodeIDAllocationClient::handleAllocation( unique_id_ + msg.unique_id.size())); // Broadcasting the response - UAVCAN_TRACE("DynamicNodeIDAllocationClient", - "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d", + UAVCAN_TRACE("DynamicNodeIDClient", "Broadcasting 2nd stage: preferred ID: %d, size of unique ID: %d", static_cast(preferred_node_id_.get()), static_cast(second_stage.unique_id.size())); const int res = dnida_pub_.broadcast(second_stage); if (res < 0) { - dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDAllocationClient request failed"); + dnida_pub_.getNode().registerInternalFailure("DynamicNodeIDClient request failed"); } } @@ -106,16 +104,16 @@ void DynamicNodeIDAllocationClient::handleAllocation( { allocated_node_id_ = msg.node_id; allocator_node_id_ = msg.getSrcNodeID(); - UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Allocation complete, node ID %d provided by %d", + UAVCAN_TRACE("DynamicNodeIDClient", "Allocation complete, node ID %d provided by %d", static_cast(allocated_node_id_.get()), static_cast(allocator_node_id_.get())); terminate(); UAVCAN_ASSERT(isAllocationComplete()); } } -int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardware_version, - const NodeID preferred_node_id, - const TransferPriority transfer_priority) +int DynamicNodeIDClient::start(const protocol::HardwareVersion& hardware_version, + const NodeID preferred_node_id, + const TransferPriority transfer_priority) { terminate(); @@ -168,7 +166,7 @@ int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardwa return res; } - res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation)); + res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation)); if (res < 0) { return res; diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp index 47da8124f3..64254fc143 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include #include "helpers.hpp" class StorageBackend : public uavcan::IDynamicNodeIDStorageBackend @@ -999,7 +999,7 @@ TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) // Node A is Allocator, Node B is Allocatee InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); - uavcan::DynamicNodeIDAllocationClient client(nodes.b); + uavcan::DynamicNodeIDClient client(nodes.b); /* * Client initialization @@ -1073,7 +1073,7 @@ TEST(DynamicNodeIDAllocationServer, Main) /* * Client */ - uavcan::DynamicNodeIDAllocationClient client(nodes.b); + uavcan::DynamicNodeIDClient client(nodes.b); uavcan::protocol::HardwareVersion hwver; for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) { diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp b/libuavcan/test/protocol/dynamic_node_id_client.cpp similarity index 95% rename from libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp rename to libuavcan/test/protocol/dynamic_node_id_client.cpp index ecf84ec493..c9c327ece7 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_client.cpp @@ -3,16 +3,16 @@ */ #include -#include +#include #include "helpers.hpp" -TEST(DynamicNodeIDAllocationClient, Basic) +TEST(DynamicNodeIDClient, Basic) { // Node A is Allocator, Node B is Allocatee InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); - uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + uavcan::DynamicNodeIDClient dnidac(nodes.b); uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; @@ -162,11 +162,11 @@ TEST(DynamicNodeIDAllocationClient, Basic) } -TEST(DynamicNodeIDAllocationClient, NonPassiveMode) +TEST(DynamicNodeIDClient, NonPassiveMode) { InterlinkedTestNodesWithSysClock nodes; - uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + uavcan::DynamicNodeIDClient dnidac(nodes.b); uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; From 0ee3a7f311c48da599ed50adff25be2a435f0178 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:03:30 +0300 Subject: [PATCH 042/268] Data type info provider moved to header --- .../protocol/data_type_info_provider.hpp | 128 +++++++++++++++- .../protocol/uc_data_type_info_provider.cpp | 140 ------------------ 2 files changed, 124 insertions(+), 144 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_data_type_info_provider.cpp diff --git a/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp index 9ec95c73d4..646b1f0612 100644 --- a/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp +++ b/libuavcan/include/uavcan/protocol/data_type_info_provider.hpp @@ -10,6 +10,7 @@ #include #include #include +#include namespace uavcan { @@ -34,13 +35,107 @@ class UAVCAN_EXPORT DataTypeInfoProvider : Noncopyable INode& getNode() { return cats_srv_.getNode(); } - static bool isValidDataTypeKind(DataTypeKind kind); + static bool isValidDataTypeKind(DataTypeKind kind) + { + return (kind == DataTypeKindMessage) || (kind == DataTypeKindService); + } void handleComputeAggregateTypeSignatureRequest(const protocol::ComputeAggregateTypeSignature::Request& request, - protocol::ComputeAggregateTypeSignature::Response& response); + protocol::ComputeAggregateTypeSignature::Response& response) + { + const DataTypeKind kind = DataTypeKind(request.kind.value); // No mapping needed + if (!isValidDataTypeKind(kind)) + { + UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request with invalid DataTypeKind %d", + kind); + return; + } + + UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request for dtk=%d, len(known_ids)=%d", + int(request.kind.value), int(request.known_ids.size())); + + // Correcting the mask length according to the data type kind + response.mutually_known_ids = request.known_ids; + response.mutually_known_ids.resize( + static_cast(DataTypeID::getMaxValueForDataTypeKind(kind).get() + 1U)); + + response.aggregate_signature = + GlobalDataTypeRegistry::instance().computeAggregateSignature(kind, response.mutually_known_ids).get(); + } void handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request, - protocol::GetDataTypeInfo::Response& response); + protocol::GetDataTypeInfo::Response& response) + { + /* + * Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID + */ + const DataTypeDescriptor* desc = NULL; + + if (request.name.empty()) + { + response.id = request.id; // Pre-setting the fields so they have meaningful values even in + response.kind = request.kind; // ...case of failure. + + if (!isValidDataTypeKind(DataTypeKind(request.kind.value))) + { + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i", + static_cast(request.kind.value)); + return; + } + + desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id); + } + else + { + response.name = request.name; + + desc = GlobalDataTypeRegistry::instance().find(request.name.c_str()); + } + + if (desc == NULL) + { + UAVCAN_TRACE("DataTypeInfoProvider", + "Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'", + static_cast(request.id), static_cast(request.kind.value), request.name.c_str()); + return; + } + + UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str()); + + /* + * Filling the response struct + */ + response.signature = desc->getSignature().get(); + response.id = desc->getID().get(); + response.kind.value = desc->getKind(); + response.mask = protocol::GetDataTypeInfo::Response::MASK_KNOWN; + response.name = desc->getFullName(); + + const Dispatcher& dispatcher = getNode().getDispatcher(); + + if (desc->getKind() == DataTypeKindService) + { + if (dispatcher.hasServer(desc->getID().get())) + { + response.mask |= protocol::GetDataTypeInfo::Response::MASK_SERVING; + } + } + else if (desc->getKind() == DataTypeKindMessage) + { + if (dispatcher.hasSubscriber(desc->getID().get())) + { + response.mask |= protocol::GetDataTypeInfo::Response::MASK_SUBSCRIBED; + } + if (dispatcher.hasPublisher(desc->getID().get())) + { + response.mask |= protocol::GetDataTypeInfo::Response::MASK_PUBLISHING; + } + } + else + { + UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror. + } + } public: explicit DataTypeInfoProvider(INode& node) @@ -48,7 +143,32 @@ public: , gdti_srv_(node) { } - int start(); + int start() + { + int res = 0; + + res = cats_srv_.start( + ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest)); + if (res < 0) + { + goto fail; + } + + res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); + if (res < 0) + { + goto fail; + } + + UAVCAN_ASSERT(res >= 0); + return res; + + fail: + UAVCAN_ASSERT(res < 0); + cats_srv_.stop(); + gdti_srv_.stop(); + return res; + } }; } diff --git a/libuavcan/src/protocol/uc_data_type_info_provider.cpp b/libuavcan/src/protocol/uc_data_type_info_provider.cpp deleted file mode 100644 index 22dfd2b7a4..0000000000 --- a/libuavcan/src/protocol/uc_data_type_info_provider.cpp +++ /dev/null @@ -1,140 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include - -namespace uavcan -{ - -bool DataTypeInfoProvider::isValidDataTypeKind(DataTypeKind kind) -{ - return (kind == DataTypeKindMessage) || (kind == DataTypeKindService); -} - -void DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest( - const protocol::ComputeAggregateTypeSignature::Request& request, - protocol::ComputeAggregateTypeSignature::Response& response) -{ - const DataTypeKind kind = DataTypeKind(request.kind.value); // No mapping needed - if (!isValidDataTypeKind(kind)) - { - UAVCAN_TRACE("DataTypeInfoProvider", - "ComputeAggregateTypeSignature request with invalid DataTypeKind %d", kind); - return; - } - - UAVCAN_TRACE("DataTypeInfoProvider", "ComputeAggregateTypeSignature request for dtk=%d, len(known_ids)=%d", - int(request.kind.value), int(request.known_ids.size())); - - // Correcting the mask length according to the data type kind - response.mutually_known_ids = request.known_ids; - response.mutually_known_ids.resize(static_cast(DataTypeID::getMaxValueForDataTypeKind(kind).get() + 1U)); - - response.aggregate_signature = - GlobalDataTypeRegistry::instance().computeAggregateSignature(kind, response.mutually_known_ids).get(); -} - -void DataTypeInfoProvider::handleGetDataTypeInfoRequest(const protocol::GetDataTypeInfo::Request& request, - protocol::GetDataTypeInfo::Response& response) -{ - /* - * Asking the Global Data Type Registry for the matching type descriptor, either by name or by ID - */ - const DataTypeDescriptor* desc = NULL; - - if (request.name.empty()) - { - response.id = request.id; // Pre-setting the fields so they have meaningful values even in - response.kind = request.kind; // ...case of failure. - - if (!isValidDataTypeKind(DataTypeKind(request.kind.value))) - { - UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request with invalid DataTypeKind %i", - static_cast(request.kind.value)); - return; - } - - desc = GlobalDataTypeRegistry::instance().find(DataTypeKind(request.kind.value), request.id); - } - else - { - response.name = request.name; - - desc = GlobalDataTypeRegistry::instance().find(request.name.c_str()); - } - - if (desc == NULL) - { - UAVCAN_TRACE("DataTypeInfoProvider", - "Cannot process GetDataTypeInfo for nonexistent type: dtid=%i dtk=%i name='%s'", - static_cast(request.id), static_cast(request.kind.value), request.name.c_str()); - return; - } - - UAVCAN_TRACE("DataTypeInfoProvider", "GetDataTypeInfo request for %s", desc->toString().c_str()); - - /* - * Filling the response struct - */ - response.signature = desc->getSignature().get(); - response.id = desc->getID().get(); - response.kind.value = desc->getKind(); - response.mask = protocol::GetDataTypeInfo::Response::MASK_KNOWN; - response.name = desc->getFullName(); - - const Dispatcher& dispatcher = getNode().getDispatcher(); - - if (desc->getKind() == DataTypeKindService) - { - if (dispatcher.hasServer(desc->getID().get())) - { - response.mask |= protocol::GetDataTypeInfo::Response::MASK_SERVING; - } - } - else if (desc->getKind() == DataTypeKindMessage) - { - if (dispatcher.hasSubscriber(desc->getID().get())) - { - response.mask |= protocol::GetDataTypeInfo::Response::MASK_SUBSCRIBED; - } - if (dispatcher.hasPublisher(desc->getID().get())) - { - response.mask |= protocol::GetDataTypeInfo::Response::MASK_PUBLISHING; - } - } - else - { - UAVCAN_ASSERT(0); // That means that GDTR somehow found a type of an unknown kind. The horror. - } -} - -int DataTypeInfoProvider::start() -{ - int res = 0; - - res = cats_srv_.start( - ComputeAggregateTypeSignatureCallback(this, &DataTypeInfoProvider::handleComputeAggregateTypeSignatureRequest)); - if (res < 0) - { - goto fail; - } - - res = gdti_srv_.start(GetDataTypeInfoCallback(this, &DataTypeInfoProvider::handleGetDataTypeInfoRequest)); - if (res < 0) - { - goto fail; - } - - UAVCAN_ASSERT(res >= 0); - return res; - -fail: - UAVCAN_ASSERT(res < 0); - cats_srv_.stop(); - gdti_srv_.stop(); - return res; -} - -} From 899fa9320133a5d30e858ddaa9c39e82c5618ad3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:07:37 +0300 Subject: [PATCH 043/268] Global time sync master moved to header --- .../protocol/global_time_sync_master.hpp | 187 +++++++++++++++- .../protocol/uc_global_time_sync_master.cpp | 206 ------------------ 2 files changed, 180 insertions(+), 213 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_global_time_sync_master.cpp diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp index e658b33730..4b69451a0a 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -5,11 +5,15 @@ #ifndef UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED #define UAVCAN_PROTOCOL_GLOBAL_TIME_SYNC_MASTER_HPP_INCLUDED +#include #include #include #include #include #include +#include +#include +#include namespace uavcan { @@ -42,11 +46,61 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase UAVCAN_ASSERT(iface_index < MaxCanIfaces); } - int init(); + int init() + { + const int res = pub_.init(); + if (res >= 0) + { + TransferSender* const ts = pub_.getTransferSender(); + UAVCAN_ASSERT(ts != NULL); + ts->setIfaceMask(uint8_t(1 << iface_index_)); + ts->setCanIOFlags(CanIOFlagLoopback); - void setTxTimestamp(UtcTime ts); + const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority + if (prio_res < 0) + { + return prio_res; + } + } + return res; + } - int publish(TransferID tid, MonotonicTime current_time); + void setTxTimestamp(UtcTime ts) + { + if (ts.isZero()) + { + UAVCAN_ASSERT(0); + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts"); + return; + } + if (!prev_tx_utc_.isZero()) + { + prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it + pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict"); + return; + } + prev_tx_utc_ = ts; + } + + int publish(TransferID tid, MonotonicTime current_time) + { + UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); + UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); + + const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; + iface_prev_pub_mono_ = current_time; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; + + protocol::GlobalTimeSync msg; + msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec(); + prev_tx_utc_ = UtcTime(); + + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", + static_cast(msg.previous_transmission_timestamp_usec), + int(iface_index_), int(tid.get())); + return pub_.broadcast(msg, tid); + } }; INode& node_; @@ -55,9 +109,43 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase DataTypeID dtid_; bool initialized_; - virtual void handleLoopbackFrame(const RxFrame& frame); + virtual void handleLoopbackFrame(const RxFrame& frame) + { + const uint8_t iface = frame.getIfaceIndex(); + if (initialized_ && iface < MaxCanIfaces) + { + if (frame.getDataTypeID() == dtid_ && + frame.getTransferType() == TransferTypeMessageBroadcast && + frame.isLast() && frame.isFirst() && + frame.getSrcNodeID() == node_.getNodeID()) + { + iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp()); + } + } + else + { + UAVCAN_ASSERT(0); + } + } - int getNextTransferID(TransferID& tid); + int getNextTransferID(TransferID& tid) + { + const MonotonicDuration max_transfer_interval = + MonotonicDuration::fromMSec(protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS); + + const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast); + const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval; + TransferID* const tid_ptr = + node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); + if (tid_ptr == NULL) + { + return -ErrMemory; + } + + tid = *tid_ptr; + tid_ptr->increment(); + return 0; + } public: explicit GlobalTimeSyncMaster(INode& node) @@ -71,7 +159,45 @@ public: * Must be called before the master can be used. * Returns negative error code. */ - int init(); + int init() + { + if (initialized_) + { + return 0; + } + + // Data type ID + const DataTypeDescriptor* const desc = + GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName()); + if (desc == NULL) + { + return -ErrUnknownDataType; + } + dtid_ = desc->getID(); + + // Iface master array + int res = -ErrLogic; + for (uint8_t i = 0; i < MaxCanIfaces; i++) + { + if (!iface_masters_[i].isConstructed()) + { + iface_masters_[i].construct(node_, i); + } + res = iface_masters_[i]->init(); + if (res < 0) + { + break; + } + } + + // Loopback listener + initialized_ = res >= 0; + if (initialized_) + { + LoopbackFrameListenerBase::startListening(); + } + return res; + } /** * Whether the master instance has been initialized. @@ -89,7 +215,54 @@ public: * * Returns negative error code. */ - int publish(); + int publish() + { + if (!initialized_) + { + const int res = init(); + if (res < 0) + { + return res; + } + } + + /* + * Enforce max frequency + */ + const MonotonicTime current_time = node_.getMonotonicTime(); + { + const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_; + UAVCAN_ASSERT(since_prev_pub.isPositive()); + if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS) + { + UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped"); + return 0; + } + prev_pub_mono_ = current_time; + } + + /* + * Obtain common Transfer ID for all masters + */ + TransferID tid; + { + const int tid_res = getNextTransferID(tid); + if (tid_res < 0) + { + return tid_res; + } + } + + for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) + { + const int res = iface_masters_[i]->publish(tid, current_time); + if (res < 0) + { + return res; + } + } + return 0; + } }; } diff --git a/libuavcan/src/protocol/uc_global_time_sync_master.cpp b/libuavcan/src/protocol/uc_global_time_sync_master.cpp deleted file mode 100644 index 6d90cf2eb6..0000000000 --- a/libuavcan/src/protocol/uc_global_time_sync_master.cpp +++ /dev/null @@ -1,206 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#if !UAVCAN_TINY - -#include -#include -#include -#include - -namespace uavcan -{ -/* - * GlobalTimeSyncMaster::IfaceMaster - */ -int GlobalTimeSyncMaster::IfaceMaster::init() -{ - const int res = pub_.init(); - if (res >= 0) - { - TransferSender* const ts = pub_.getTransferSender(); - UAVCAN_ASSERT(ts != NULL); - ts->setIfaceMask(uint8_t(1 << iface_index_)); - ts->setCanIOFlags(CanIOFlagLoopback); - - const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority - if (prio_res < 0) - { - return prio_res; - } - } - return res; -} - -void GlobalTimeSyncMaster::IfaceMaster::setTxTimestamp(UtcTime ts) -{ - if (ts.isZero()) - { - UAVCAN_ASSERT(0); - pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster zero TX ts"); - return; - } - if (!prev_tx_utc_.isZero()) - { - prev_tx_utc_ = UtcTime(); // Reset again, because there's something broken in the driver and we don't trust it - pub_.getNode().registerInternalFailure("GlobalTimeSyncMaster pub conflict"); - return; - } - prev_tx_utc_ = ts; -} - -int GlobalTimeSyncMaster::IfaceMaster::publish(TransferID tid, MonotonicTime current_time) -{ - UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); - UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); - - const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; - iface_prev_pub_mono_ = current_time; - UAVCAN_ASSERT(since_prev_pub.isPositive()); - const bool long_period = since_prev_pub.toMSec() >= protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; - - protocol::GlobalTimeSync msg; - msg.previous_transmission_timestamp_usec = long_period ? 0 : prev_tx_utc_.toUSec(); - prev_tx_utc_ = UtcTime(); - - UAVCAN_TRACE("GlobalTimeSyncMaster", "Publishing %llu iface=%i tid=%i", - static_cast(msg.previous_transmission_timestamp_usec), - int(iface_index_), int(tid.get())); - return pub_.broadcast(msg, tid); -} - -/* - * GlobalTimeSyncMaster - */ -void GlobalTimeSyncMaster::handleLoopbackFrame(const RxFrame& frame) -{ - const uint8_t iface = frame.getIfaceIndex(); - if (initialized_ && iface < MaxCanIfaces) - { - if (frame.getDataTypeID() == dtid_ && - frame.getTransferType() == TransferTypeMessageBroadcast && - frame.isLast() && frame.isFirst() && - frame.getSrcNodeID() == node_.getNodeID()) - { - iface_masters_[iface]->setTxTimestamp(frame.getUtcTimestamp()); - } - } - else - { - UAVCAN_ASSERT(0); - } -} - -int GlobalTimeSyncMaster::getNextTransferID(TransferID& tid) -{ - const MonotonicDuration max_transfer_interval = - MonotonicDuration::fromMSec(protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS); - - const OutgoingTransferRegistryKey otr_key(dtid_, TransferTypeMessageBroadcast, NodeID::Broadcast); - const MonotonicTime otr_deadline = node_.getMonotonicTime() + max_transfer_interval; - TransferID* const tid_ptr = - node_.getDispatcher().getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); - if (tid_ptr == NULL) - { - return -ErrMemory; - } - - tid = *tid_ptr; - tid_ptr->increment(); - return 0; -} - -int GlobalTimeSyncMaster::init() -{ - if (initialized_) - { - return 0; - } - - // Data type ID - const DataTypeDescriptor* const desc = - GlobalDataTypeRegistry::instance().find(DataTypeKindMessage, protocol::GlobalTimeSync::getDataTypeFullName()); - if (desc == NULL) - { - return -ErrUnknownDataType; - } - dtid_ = desc->getID(); - - // Iface master array - int res = -ErrLogic; - for (uint8_t i = 0; i < MaxCanIfaces; i++) - { - if (!iface_masters_[i].isConstructed()) - { - iface_masters_[i].construct(node_, i); - } - res = iface_masters_[i]->init(); - if (res < 0) - { - break; - } - } - - // Loopback listener - initialized_ = res >= 0; - if (initialized_) - { - LoopbackFrameListenerBase::startListening(); - } - return res; -} - -int GlobalTimeSyncMaster::publish() -{ - if (!initialized_) - { - const int res = init(); - if (res < 0) - { - return res; - } - } - - /* - * Enforce max frequency - */ - const MonotonicTime current_time = node_.getMonotonicTime(); - { - const MonotonicDuration since_prev_pub = current_time - prev_pub_mono_; - UAVCAN_ASSERT(since_prev_pub.isPositive()); - if (since_prev_pub.toMSec() < protocol::GlobalTimeSync::MIN_PUBLICATION_PERIOD_MS) - { - UAVCAN_TRACE("GlobalTimeSyncMaster", "Publication skipped"); - return 0; - } - prev_pub_mono_ = current_time; - } - - /* - * Obtain common Transfer ID for all masters - */ - TransferID tid; - { - const int tid_res = getNextTransferID(tid); - if (tid_res < 0) - { - return tid_res; - } - } - - for (uint8_t i = 0; i < node_.getDispatcher().getCanIOManager().getNumIfaces(); i++) - { - const int res = iface_masters_[i]->publish(tid, current_time); - if (res < 0) - { - return res; - } - } - return 0; -} - -} - -#endif From fc7e2421008f76dea2633dd28840059f41880e61 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:10:04 +0300 Subject: [PATCH 044/268] Global time sync slave moved to header --- .../protocol/global_time_sync_slave.hpp | 104 ++++++++++++++-- .../protocol/uc_global_time_sync_slave.cpp | 115 ------------------ 2 files changed, 97 insertions(+), 122 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_global_time_sync_slave.cpp diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp index 3da6532998..0490fe0e84 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_slave.hpp @@ -8,6 +8,8 @@ #include #include #include +#include +#include namespace uavcan { @@ -43,13 +45,93 @@ class UAVCAN_EXPORT GlobalTimeSyncSlave : Noncopyable ISystemClock& getSystemClock() const { return sub_.getNode().getSystemClock(); } - void adjustFromMsg(const ReceivedDataStructure& msg); + void adjustFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0); + const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_; - void updateFromMsg(const ReceivedDataStructure& msg); + UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i", + static_cast(adjustment.toUSec()), + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_)); - void processMsg(const ReceivedDataStructure& msg); + if (!suppressed_) + { + getSystemClock().adjustUtc(adjustment); + } + last_adjustment_ts_ = msg.getMonotonicTimestamp(); + state_ = Update; + } - void handleGlobalTimeSync(const ReceivedDataStructure& msg); + void updateFromMsg(const ReceivedDataStructure& msg) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + + prev_ts_utc_ = msg.getUtcTimestamp(); + prev_ts_mono_ = msg.getMonotonicTimestamp(); + master_nid_ = msg.getSrcNodeID(); + prev_iface_index_ = msg.getIfaceIndex(); + prev_tid_ = msg.getTransferID(); + state_ = Adjust; + } + + void processMsg(const ReceivedDataStructure& msg) + { + const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; + UAVCAN_ASSERT(!since_prev_msg.isNegative()); + + const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); + const bool switch_master = msg.getSrcNodeID() < master_nid_; + const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; + + if (switch_master || pub_timeout || needs_init) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", + int(needs_init), int(switch_master), int(pub_timeout)); + updateFromMsg(msg); + } + else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) + { + if (state_ == Adjust) + { + const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; + const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; + const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; + if (msg_invalid || wrong_tid || wrong_timing) + { + UAVCAN_TRACE("GlobalTimeSyncSlave", + "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", + int(msg_invalid), int(wrong_tid), int(wrong_timing)); + state_ = Update; + } + } + if (state_ == Adjust) + { + adjustFromMsg(msg); + } + else + { + updateFromMsg(msg); + } + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", + int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); + } + } + + void handleGlobalTimeSync(const ReceivedDataStructure& msg) + { + if (msg.getTransferType() == TransferTypeMessageBroadcast) + { + processMsg(msg); + } + else + { + UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType())); + } + } public: explicit GlobalTimeSyncSlave(INode& node) @@ -64,7 +146,10 @@ public: * attention from the application, other than to handle a clock adjustment request occasionally. * Returns negative error code. */ - int start(); + int start() + { + return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync)); + } /** * Enable or disable the suppressed mode. @@ -89,13 +174,18 @@ public: * Note that immediately after start up the slave will be INACTIVE until it finds a master. * Please read the specs to learn more. */ - bool isActive() const; + bool isActive() const + { + const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_; + return !last_adjustment_ts_.isZero() && + (since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS); + } /** * Node ID of the master the slave is currently locked on. * Returns an invalid Node ID if there's no active master. */ - NodeID getMasterNodeID() const; + NodeID getMasterNodeID() const { return isActive() ? master_nid_ : NodeID(); } /** * Last time when the local clock adjustment was performed. diff --git a/libuavcan/src/protocol/uc_global_time_sync_slave.cpp b/libuavcan/src/protocol/uc_global_time_sync_slave.cpp deleted file mode 100644 index 5a52ee29ae..0000000000 --- a/libuavcan/src/protocol/uc_global_time_sync_slave.cpp +++ /dev/null @@ -1,115 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include -#include - -namespace uavcan -{ - -void GlobalTimeSyncSlave::adjustFromMsg(const ReceivedDataStructure& msg) -{ - UAVCAN_ASSERT(msg.previous_transmission_timestamp_usec > 0); - const UtcDuration adjustment = UtcTime::fromUSec(msg.previous_transmission_timestamp_usec) - prev_ts_utc_; - - UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment: usec=%lli snid=%i iface=%i suppress=%i", - static_cast(adjustment.toUSec()), - int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex()), int(suppressed_)); - - if (!suppressed_) - { - getSystemClock().adjustUtc(adjustment); - } - last_adjustment_ts_ = msg.getMonotonicTimestamp(); - state_ = Update; -} - -void GlobalTimeSyncSlave::updateFromMsg(const ReceivedDataStructure& msg) -{ - UAVCAN_TRACE("GlobalTimeSyncSlave", "Update: snid=%i iface=%i", - int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); - - prev_ts_utc_ = msg.getUtcTimestamp(); - prev_ts_mono_ = msg.getMonotonicTimestamp(); - master_nid_ = msg.getSrcNodeID(); - prev_iface_index_ = msg.getIfaceIndex(); - prev_tid_ = msg.getTransferID(); - state_ = Adjust; -} - -void GlobalTimeSyncSlave::processMsg(const ReceivedDataStructure& msg) -{ - const MonotonicDuration since_prev_msg = msg.getMonotonicTimestamp() - prev_ts_mono_; - UAVCAN_ASSERT(!since_prev_msg.isNegative()); - - const bool needs_init = !master_nid_.isValid() || prev_ts_mono_.isZero(); - const bool switch_master = msg.getSrcNodeID() < master_nid_; - const bool pub_timeout = since_prev_msg.toMSec() > protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; - - if (switch_master || pub_timeout || needs_init) - { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Force update: needs_init=%i switch_master=%i pub_timeout=%i", - int(needs_init), int(switch_master), int(pub_timeout)); - updateFromMsg(msg); - } - else if (msg.getIfaceIndex() == prev_iface_index_ && msg.getSrcNodeID() == master_nid_) - { - if (state_ == Adjust) - { - const bool msg_invalid = msg.previous_transmission_timestamp_usec == 0; - const bool wrong_tid = prev_tid_.computeForwardDistance(msg.getTransferID()) != 1; - const bool wrong_timing = since_prev_msg.toMSec() > protocol::GlobalTimeSync::MAX_PUBLICATION_PERIOD_MS; - if (msg_invalid || wrong_tid || wrong_timing) - { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Adjustment skipped: msg_invalid=%i wrong_tid=%i wrong_timing=%i", - int(msg_invalid), int(wrong_tid), int(wrong_timing)); - state_ = Update; - } - } - if (state_ == Adjust) - { - adjustFromMsg(msg); - } - else - { - updateFromMsg(msg); - } - } - else - { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Ignored: snid=%i iface=%i", - int(msg.getSrcNodeID().get()), int(msg.getIfaceIndex())); - } -} - -void GlobalTimeSyncSlave::handleGlobalTimeSync(const ReceivedDataStructure& msg) -{ - if (msg.getTransferType() == TransferTypeMessageBroadcast) - { - processMsg(msg); - } - else - { - UAVCAN_TRACE("GlobalTimeSyncSlave", "Invalid transfer type %i", int(msg.getTransferType())); - } -} - -int GlobalTimeSyncSlave::start() -{ - return sub_.start(GlobalTimeSyncCallback(this, &GlobalTimeSyncSlave::handleGlobalTimeSync)); -} - -bool GlobalTimeSyncSlave::isActive() const -{ - const MonotonicDuration since_prev_adj = getSystemClock().getMonotonic() - last_adjustment_ts_; - return !last_adjustment_ts_.isZero() && since_prev_adj.toMSec() <= protocol::GlobalTimeSync::PUBLISHER_TIMEOUT_MS; -} - -NodeID GlobalTimeSyncSlave::getMasterNodeID() const -{ - return isActive() ? master_nid_ : NodeID(); -} - -} From 6e19d1c7ad83ab0d54b61d698d541fa00653a49e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:17:57 +0300 Subject: [PATCH 045/268] Logger moved to header --- libuavcan/include/uavcan/protocol/logger.hpp | 56 ++++++++++++++-- libuavcan/src/protocol/uc_logger.cpp | 69 -------------------- 2 files changed, 51 insertions(+), 74 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_logger.cpp diff --git a/libuavcan/include/uavcan/protocol/logger.hpp b/libuavcan/include/uavcan/protocol/logger.hpp index fcacfc4801..5856e07355 100644 --- a/libuavcan/include/uavcan/protocol/logger.hpp +++ b/libuavcan/include/uavcan/protocol/logger.hpp @@ -6,9 +6,11 @@ #define UAVCAN_PROTOCOL_LOGGER_HPP_INCLUDED #include +#include #include #include #include +#include #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) # error UAVCAN_CPP_VERSION @@ -62,7 +64,7 @@ public: * This value is higher than any valid severity value. * Use it to completely suppress the output. */ - static const LogLevel LevelAboveAll = (1 << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1; + static LogLevel getLogLevelAboveAll() { return (1U << protocol::debug::LogLevel::FieldTypes::value::BitLen) - 1U; } private: enum { DefaultTxTimeoutMs = 2000 }; @@ -72,7 +74,10 @@ private: LogLevel level_; ILogSink* external_sink_; - LogLevel getExternalSinkLevel() const; + LogLevel getExternalSinkLevel() const + { + return (external_sink_ == NULL) ? getLogLevelAboveAll() : external_sink_->getLogLevel(); + } public: explicit Logger(INode& node) @@ -89,7 +94,15 @@ public: * Must be called once before use. * Returns negative error code. */ - int init(); + int init() + { + const int res = logmsg_pub_.init(); + if (res < 0) + { + return res; + } + return logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority + } /** * Logs one message. Please consider using helper methods instead of this one. @@ -102,7 +115,19 @@ public: * * Returns negative error code. */ - int log(const protocol::debug::LogMessage& message); + int log(const protocol::debug::LogMessage& message) + { + int retval = 0; + if (message.level.value >= getExternalSinkLevel()) + { + external_sink_->log(message); + } + if (message.level.value >= level_) + { + retval = logmsg_pub_.broadcast(message); + } + return retval; + } /** * Severity filter for UAVCAN broadcasting. @@ -177,7 +202,28 @@ public: #else - int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT; + int log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT + { + #if UAVCAN_EXCEPTIONS + try + #endif + { + if (level >= level_ || level >= getExternalSinkLevel()) + { + msg_buf_.level.value = level; + msg_buf_.source = source; + msg_buf_.text = text; + return log(msg_buf_); + } + return 0; + } + #if UAVCAN_EXCEPTIONS + catch (...) + { + return -ErrFailure; + } + #endif + } int logDebug(const char* source, const char* text) UAVCAN_NOEXCEPT { diff --git a/libuavcan/src/protocol/uc_logger.cpp b/libuavcan/src/protocol/uc_logger.cpp deleted file mode 100644 index 241267ac50..0000000000 --- a/libuavcan/src/protocol/uc_logger.cpp +++ /dev/null @@ -1,69 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include - -namespace uavcan -{ - -const Logger::LogLevel Logger::LevelAboveAll; - -Logger::LogLevel Logger::getExternalSinkLevel() const -{ - return (external_sink_ == NULL) ? LevelAboveAll : external_sink_->getLogLevel(); -} - -int Logger::init() -{ - const int res = logmsg_pub_.init(); - if (res < 0) - { - return res; - } - return logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority -} - -int Logger::log(const protocol::debug::LogMessage& message) -{ - int retval = 0; - if (message.level.value >= getExternalSinkLevel()) - { - external_sink_->log(message); - } - if (message.level.value >= level_) - { - retval = logmsg_pub_.broadcast(message); - } - return retval; -} - -#if UAVCAN_CPP_VERSION < UAVCAN_CPP11 - -int Logger::log(LogLevel level, const char* source, const char* text) UAVCAN_NOEXCEPT -{ -#if UAVCAN_EXCEPTIONS - try -#endif - { - if (level >= level_ || level >= getExternalSinkLevel()) - { - msg_buf_.level.value = level; - msg_buf_.source = source; - msg_buf_.text = text; - return log(msg_buf_); - } - return 0; - } -#if UAVCAN_EXCEPTIONS - catch (...) - { - return -ErrFailure; - } -#endif -} - -#endif - -} From 894d951c33f278a68551ee1027190358cfad6082 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:21:46 +0300 Subject: [PATCH 046/268] Network compat checker moved to header --- .../protocol/network_compat_checker.hpp | 189 ++++++++++++++++- .../protocol/uc_network_compat_checker.cpp | 200 ------------------ 2 files changed, 179 insertions(+), 210 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_network_compat_checker.cpp diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index 5a39b5c6fc..ca6635451a 100644 --- a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp +++ b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp @@ -5,12 +5,17 @@ #ifndef UAVCAN_PROTOCOL_NETWORK_COMPAT_CHECKER_HPP_INCLUDED #define UAVCAN_PROTOCOL_NETWORK_COMPAT_CHECKER_HPP_INCLUDED +#include +#include #include #include #include +#include #include #include #include +#include +#include namespace uavcan { @@ -58,18 +63,143 @@ class UAVCAN_EXPORT NetworkCompatibilityChecker : Noncopyable INode& getNode() { return ns_sub_.getNode(); } const INode& getNode() const { return ns_sub_.getNode(); } - MonotonicDuration getNetworkDiscoveryDelay() const; + MonotonicDuration getNetworkDiscoveryDelay() const + { + // Base duration is constant - NodeStatus max publication period + MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS); + // Additional duration depends on the node priority - gets larger with higher Node ID + dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10); + return dur; + } - NodeID findNextUncheckedNode(); + NodeID findNextUncheckedNode() + { + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i)) + { + nid_mask_checked_[i] = true; + return NodeID(i); + } + } + return NodeID(); + } - int waitForCATSResponse(); + int waitForCATSResponse() + { + while (cats_cln_.isPending()) + { + const int res = getNode().spin(MonotonicDuration::fromMSec(10)); + if (res < 0 || !result_.isOk()) + { + return res; + } + } + return 0; + } - void handleNodeStatus(const ReceivedDataStructure& msg); - void handleCATSResponse(ServiceCallResult& resp); + void handleNodeStatus(const ReceivedDataStructure& msg) + { + if (!nid_mask_present_.test(msg.getSrcNodeID().get())) + { + UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get())); + nid_mask_present_[msg.getSrcNodeID().get()] = true; + } - int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind); - int checkOneNode(NodeID nid); - int checkNodes(); + if (msg.getSrcNodeID() == getNode().getNodeID()) + { + UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get())); + result_.conflicting_node = msg.getSrcNodeID(); + } + } + + void handleCATSResponse(ServiceCallResult& resp) + { + last_cats_request_ok_ = resp.isSuccessful(); + if (last_cats_request_ok_) + { + const DataTypeSignature sign = GlobalDataTypeRegistry::instance(). + computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids); + + UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu", + int(resp.server_node_id.get()), static_cast(sign.get()), + static_cast(resp.response.aggregate_signature)); + + if (sign.get() != resp.response.aggregate_signature) + { + result_.conflicting_node = resp.server_node_id; + } + } + } + + int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind) + { + StaticAssert::check(); + StaticAssert::check(); + + UAVCAN_ASSERT(nid.isUnicast()); + UAVCAN_ASSERT(!cats_cln_.isPending()); + + checking_dtkind_ = kind; + protocol::ComputeAggregateTypeSignature::Request request; + request.kind.value = kind; + GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids); + + int res = cats_cln_.call(nid, request); + if (res < 0) + { + return res; + } + res = waitForCATSResponse(); + if (res < 0) + { + return res; + } + if (!last_cats_request_ok_) + { + return -ErrFailure; + } + return 0; + } + + int checkOneNode(NodeID nid) + { + if (nid == getNode().getNodeID()) + { + result_.conflicting_node = nid; // NodeID collision + return 0; + } + + const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage); + if (res < 0 || !result_.isOk()) + { + return res; + } + return checkOneNodeOneDataTypeKind(nid, DataTypeKindService); + } + + int checkNodes() + { + (void)nid_mask_checked_.reset(); + result_ = NetworkCompatibilityCheckResult(); + + while (result_.isOk()) + { + const NodeID nid = findNextUncheckedNode(); + if (nid.isValid()) + { + UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get())); + const int res = checkOneNode(nid); + if (res < 0) + { + result_.num_failed_nodes++; + } + UAVCAN_TRACE("NodeInitializer", "Checked nid=%i result=%i", int(nid.get()), res); + } + else { break; } + } + return 0; + } public: explicit NetworkCompatibilityChecker(INode& node) @@ -90,7 +220,42 @@ public: * * Returns negative error code. */ - int execute(); + int execute() + { + int res = 0; + + if (!getNode().getNodeID().isUnicast()) + { + result_.conflicting_node = getNode().getNodeID(); + goto exit; + } + + res = ns_sub_.start(NodeStatusCallback(this, &NetworkCompatibilityChecker::handleNodeStatus)); + if (res < 0) + { + goto exit; + } + + cats_cln_.setCallback(CATSResponseCallback(this, &NetworkCompatibilityChecker::handleCATSResponse)); + res = cats_cln_.init(); + if (res < 0) + { + goto exit; + } + + res = getNode().spin(getNetworkDiscoveryDelay()); + if (res < 0) + { + goto exit; + } + + res = checkNodes(); + + exit: + ns_sub_.stop(); + cats_cln_.cancel(); + return res; + } /** * This method can return a meaningful result only if @ref execute() has been executed successfully once. @@ -101,7 +266,11 @@ public: * Convenience method. Should be called immediately before @ref execute(). * Returns negative error code. */ - static int publishGlobalDiscoveryRequest(INode& node); + static int publishGlobalDiscoveryRequest(INode& node) + { + Publisher pub(node); + return pub.broadcast(protocol::GlobalDiscoveryRequest()); + } }; } diff --git a/libuavcan/src/protocol/uc_network_compat_checker.cpp b/libuavcan/src/protocol/uc_network_compat_checker.cpp deleted file mode 100644 index 6738f723cb..0000000000 --- a/libuavcan/src/protocol/uc_network_compat_checker.cpp +++ /dev/null @@ -1,200 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#if !UAVCAN_TINY - -#include -#include -#include -#include -#include - -namespace uavcan -{ - -MonotonicDuration NetworkCompatibilityChecker::getNetworkDiscoveryDelay() const -{ - // Base duration is constant - NodeStatus max publication period - MonotonicDuration dur = MonotonicDuration::fromMSec(protocol::NodeStatus::MAX_PUBLICATION_PERIOD_MS); - // Additional duration depends on the node priority - gets larger with higher Node ID - dur += MonotonicDuration::fromMSec(getNode().getNodeID().get() * 10); - return dur; -} - -NodeID NetworkCompatibilityChecker::findNextUncheckedNode() -{ - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - if (nid_mask_present_.test(i) && !nid_mask_checked_.test(i)) - { - nid_mask_checked_[i] = true; - return NodeID(i); - } - } - return NodeID(); -} - -int NetworkCompatibilityChecker::waitForCATSResponse() -{ - while (cats_cln_.isPending()) - { - const int res = getNode().spin(MonotonicDuration::fromMSec(10)); - if (res < 0 || !result_.isOk()) - { - return res; - } - } - return 0; -} - -void NetworkCompatibilityChecker::handleNodeStatus(const ReceivedDataStructure& msg) -{ - if (!nid_mask_present_.test(msg.getSrcNodeID().get())) - { - UAVCAN_TRACE("NodeInitializer", "New node nid=%i", int(msg.getSrcNodeID().get())); - nid_mask_present_[msg.getSrcNodeID().get()] = true; - } - - if (msg.getSrcNodeID() == getNode().getNodeID()) - { - UAVCAN_TRACE("NodeInitializer", "Node ID collision; nid=%i", int(msg.getSrcNodeID().get())); - result_.conflicting_node = msg.getSrcNodeID(); - } -} - -void NetworkCompatibilityChecker::handleCATSResponse(ServiceCallResult& resp) -{ - last_cats_request_ok_ = resp.isSuccessful(); - if (last_cats_request_ok_) - { - const DataTypeSignature sign = GlobalDataTypeRegistry::instance(). - computeAggregateSignature(checking_dtkind_, resp.response.mutually_known_ids); - - UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu", - int(resp.server_node_id.get()), static_cast(sign.get()), - static_cast(resp.response.aggregate_signature)); - - if (sign.get() != resp.response.aggregate_signature) - { - result_.conflicting_node = resp.server_node_id; - } - } -} - -int NetworkCompatibilityChecker::checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind) -{ - StaticAssert::check(); - StaticAssert::check(); - - UAVCAN_ASSERT(nid.isUnicast()); - UAVCAN_ASSERT(!cats_cln_.isPending()); - - checking_dtkind_ = kind; - protocol::ComputeAggregateTypeSignature::Request request; - request.kind.value = kind; - GlobalDataTypeRegistry::instance().getDataTypeIDMask(kind, request.known_ids); - - int res = cats_cln_.call(nid, request); - if (res < 0) - { - return res; - } - res = waitForCATSResponse(); - if (res < 0) - { - return res; - } - if (!last_cats_request_ok_) - { - return -ErrFailure; - } - return 0; -} - -int NetworkCompatibilityChecker::checkOneNode(NodeID nid) -{ - if (nid == getNode().getNodeID()) - { - result_.conflicting_node = nid; // NodeID collision - return 0; - } - - const int res = checkOneNodeOneDataTypeKind(nid, DataTypeKindMessage); - if (res < 0 || !result_.isOk()) - { - return res; - } - return checkOneNodeOneDataTypeKind(nid, DataTypeKindService); -} - -int NetworkCompatibilityChecker::checkNodes() -{ - (void)nid_mask_checked_.reset(); - result_ = NetworkCompatibilityCheckResult(); - - while (result_.isOk()) - { - const NodeID nid = findNextUncheckedNode(); - if (nid.isValid()) - { - UAVCAN_TRACE("NodeInitializer", "Checking nid=%i", int(nid.get())); - const int res = checkOneNode(nid); - if (res < 0) - { - result_.num_failed_nodes++; - } - UAVCAN_TRACE("NodeInitializer", "Checked nid=%i result=%i", int(nid.get()), res); - } - else { break; } - } - return 0; -} - -int NetworkCompatibilityChecker::execute() -{ - int res = 0; - - if (!getNode().getNodeID().isUnicast()) - { - result_.conflicting_node = getNode().getNodeID(); - goto exit; - } - - res = ns_sub_.start(NodeStatusCallback(this, &NetworkCompatibilityChecker::handleNodeStatus)); - if (res < 0) - { - goto exit; - } - - cats_cln_.setCallback(CATSResponseCallback(this, &NetworkCompatibilityChecker::handleCATSResponse)); - res = cats_cln_.init(); - if (res < 0) - { - goto exit; - } - - res = getNode().spin(getNetworkDiscoveryDelay()); - if (res < 0) - { - goto exit; - } - - res = checkNodes(); - -exit: - ns_sub_.stop(); - cats_cln_.cancel(); - return res; -} - -int NetworkCompatibilityChecker::publishGlobalDiscoveryRequest(INode& node) -{ - Publisher pub(node); - return pub.broadcast(protocol::GlobalDiscoveryRequest()); -} - -} - -#endif From 00ec7186b09e61ea2fbe3d6ea13044b816a348be Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:24:31 +0300 Subject: [PATCH 047/268] Node status monitor moved to header --- .../uavcan/protocol/node_status_monitor.hpp | 133 ++++++++++++++-- .../src/protocol/uc_node_status_monitor.cpp | 143 ------------------ 2 files changed, 124 insertions(+), 152 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_node_status_monitor.cpp diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index e9028e37b9..cf0dd7fd9b 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -5,10 +5,13 @@ #ifndef UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED #define UAVCAN_PROTOCOL_NODE_STATUS_MONITOR_HPP_INCLUDED +#include #include #include #include #include +#include +#include namespace uavcan { @@ -40,7 +43,7 @@ public: }; private: - static const unsigned TimerPeriodMs100 = 5; + enum { TimerPeriodMs100 = 5 }; typedef MethodBinder&)> @@ -65,13 +68,71 @@ private: mutable Entry entries_[NodeID::Max]; // [1, NodeID::Max] - Entry& getEntry(NodeID node_id) const; + Entry& getEntry(NodeID node_id) const + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeStatusMonitor NodeID"); + } + return entries_[node_id.get() - 1]; + } - void changeNodeStatus(const NodeID node_id, const Entry new_entry_value); + void changeNodeStatus(const NodeID node_id, const Entry new_entry_value) + { + Entry& entry = getEntry(node_id); + if (entry.status_code != new_entry_value.status_code) + { + NodeStatusChangeEvent event; + event.node_id = node_id; - void handleNodeStatus(const ReceivedDataStructure& msg); + event.old_status.known = entry.time_since_last_update_ms100 >= 0; + event.old_status.status_code = entry.status_code; - virtual void handleTimerEvent(const TimerEvent&); + event.status.known = true; + event.status.status_code = new_entry_value.status_code; + + UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()), + (event.old_status.known ? "known" : "new"), + int(event.old_status.status_code), int(event.status.status_code)); + handleNodeStatusChange(event); + } + entry = new_entry_value; + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + Entry new_entry_value; + new_entry_value.time_since_last_update_ms100 = 0; + new_entry_value.status_code = msg.status_code; + + changeNodeStatus(msg.getSrcNodeID(), new_entry_value); + + handleNodeStatusMessage(msg); + } + + virtual void handleTimerEvent(const TimerEvent&) + { + const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + Entry& entry = getEntry(nid); + 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)); + if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100) + { + Entry new_entry_value; + new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; + new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE; + changeNodeStatus(nid, new_entry_value); + } + } + } + } protected: /** @@ -107,25 +168,79 @@ public: * Destroy the object to stop it. * Returns negative error code. */ - int start(); + int start() + { + const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); + if (res >= 0) + { + TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); + } + return res; + } /** * Make the node unknown. */ - void forgetNode(NodeID node_id); + void forgetNode(NodeID node_id) + { + if (node_id.isValid()) + { + Entry& entry = getEntry(node_id); + entry = Entry(); + } + else + { + UAVCAN_ASSERT(0); + } + } /** * Returns status of a given node. * Unknown nodes are considered offline. */ - NodeStatus getNodeStatus(NodeID node_id) const; + NodeStatus getNodeStatus(NodeID node_id) const + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return NodeStatus(); + } + NodeStatus status; + const Entry& entry = getEntry(node_id); + if (entry.time_since_last_update_ms100 >= 0) + { + status.known = true; + status.status_code = entry.status_code; + } + return status; + } /** * This helper method allows to quickly estimate the overall network health. * Status of the local node is not considered. * Returns an invalid Node ID value if there's no known nodes in the network. */ - NodeID findNodeWithWorstStatus() const; + NodeID findNodeWithWorstStatus() const + { + NodeID nid_with_worst_status; + NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK; + + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + const NodeID nid(i); + UAVCAN_ASSERT(nid.isUnicast()); + const Entry& entry = getEntry(nid); + if (entry.time_since_last_update_ms100 >= 0) + { + if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid()) + { + nid_with_worst_status = nid; + worst_status_code = entry.status_code; + } + } + } + return nid_with_worst_status; + } }; } diff --git a/libuavcan/src/protocol/uc_node_status_monitor.cpp b/libuavcan/src/protocol/uc_node_status_monitor.cpp deleted file mode 100644 index 7904b8ecc4..0000000000 --- a/libuavcan/src/protocol/uc_node_status_monitor.cpp +++ /dev/null @@ -1,143 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include -#include -#include - -namespace uavcan -{ - -const unsigned NodeStatusMonitor::TimerPeriodMs100; - -NodeStatusMonitor::Entry& NodeStatusMonitor::getEntry(NodeID node_id) const -{ - if (node_id.get() < 1 || node_id.get() > NodeID::Max) - { - handleFatalError("NodeStatusMonitor NodeID"); - } - return entries_[node_id.get() - 1]; -} - -void NodeStatusMonitor::changeNodeStatus(const NodeID node_id, const Entry new_entry_value) -{ - Entry& entry = getEntry(node_id); - if (entry.status_code != new_entry_value.status_code) - { - NodeStatusChangeEvent event; - event.node_id = node_id; - - event.old_status.known = entry.time_since_last_update_ms100 >= 0; - event.old_status.status_code = entry.status_code; - - event.status.known = true; - event.status.status_code = new_entry_value.status_code; - - UAVCAN_TRACE("NodeStatusMonitor", "Node %i [%s] status change: %i --> %i", int(node_id.get()), - (event.old_status.known ? "known" : "new"), - int(event.old_status.status_code), int(event.status.status_code)); - handleNodeStatusChange(event); - } - entry = new_entry_value; -} - -void NodeStatusMonitor::handleNodeStatus(const ReceivedDataStructure& msg) -{ - Entry new_entry_value; - new_entry_value.time_since_last_update_ms100 = 0; - new_entry_value.status_code = msg.status_code; - - changeNodeStatus(msg.getSrcNodeID(), new_entry_value); - - handleNodeStatusMessage(msg); -} - -void NodeStatusMonitor::handleTimerEvent(const TimerEvent&) -{ - const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; - - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - const NodeID nid(i); - UAVCAN_ASSERT(nid.isUnicast()); - Entry& entry = getEntry(nid); - 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)); - if (entry.time_since_last_update_ms100 >= OfflineTimeoutMs100) - { - Entry new_entry_value; - new_entry_value.time_since_last_update_ms100 = OfflineTimeoutMs100; - new_entry_value.status_code = protocol::NodeStatus::STATUS_OFFLINE; - changeNodeStatus(nid, new_entry_value); - } - } - } -} - -int NodeStatusMonitor::start() -{ - const int res = sub_.start(NodeStatusCallback(this, &NodeStatusMonitor::handleNodeStatus)); - if (res >= 0) - { - TimerBase::startPeriodic(MonotonicDuration::fromMSec(TimerPeriodMs100 * 100)); - } - return res; -} - -void NodeStatusMonitor::forgetNode(NodeID node_id) -{ - if (node_id.isValid()) - { - Entry& entry = getEntry(node_id); - entry = Entry(); - } - else - { - UAVCAN_ASSERT(0); - } -} - -NodeStatusMonitor::NodeStatus NodeStatusMonitor::getNodeStatus(NodeID node_id) const -{ - if (!node_id.isValid()) - { - UAVCAN_ASSERT(0); - return NodeStatus(); - } - NodeStatus status; - const Entry& entry = getEntry(node_id); - if (entry.time_since_last_update_ms100 >= 0) - { - status.known = true; - status.status_code = entry.status_code; - } - return status; -} - -NodeID NodeStatusMonitor::findNodeWithWorstStatus() const -{ - NodeID nid_with_worst_status; - NodeStatusCode worst_status_code = protocol::NodeStatus::STATUS_OK; - - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - const NodeID nid(i); - UAVCAN_ASSERT(nid.isUnicast()); - const Entry& entry = getEntry(nid); - if (entry.time_since_last_update_ms100 >= 0) - { - if (entry.status_code > worst_status_code || !nid_with_worst_status.isValid()) - { - nid_with_worst_status = nid; - worst_status_code = entry.status_code; - } - } - } - return nid_with_worst_status; -} - -} From ee85d2d73a0631c5a19ce81fda8576f6de55faa3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:26:22 +0300 Subject: [PATCH 048/268] Panic broadcaster moved to header --- .../uavcan/protocol/panic_broadcaster.hpp | 48 +++++++++++++-- .../src/protocol/uc_panic_broadcaster.cpp | 61 ------------------- 2 files changed, 42 insertions(+), 67 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_panic_broadcaster.cpp diff --git a/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp index c05a55fe2e..2a15b5d1d6 100644 --- a/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp +++ b/libuavcan/include/uavcan/protocol/panic_broadcaster.hpp @@ -5,6 +5,7 @@ #ifndef UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED #define UAVCAN_PROTOCOL_PANIC_BROADCASTER_HPP_INCLUDED +#include #include #include #include @@ -19,9 +20,19 @@ class UAVCAN_EXPORT PanicBroadcaster : private TimerBase Publisher pub_; protocol::Panic msg_; - void publishOnce(); + void publishOnce() + { + const int res = pub_.broadcast(msg_); + if (res < 0) + { + pub_.getNode().registerInternalFailure("Panic pub failed"); + } + } - virtual void handleTimerEvent(const TimerEvent&); + virtual void handleTimerEvent(const TimerEvent&) + { + publishOnce(); + } public: explicit PanicBroadcaster(INode& node) @@ -37,16 +48,41 @@ public: * @param short_reason Short ASCII string that describes the reason of the panic, 7 characters max. * If the string exceeds 7 characters, it will be truncated. */ - void panic(const char* short_reason_description); + void panic(const char* short_reason_description) + { + msg_.reason_text.clear(); + const char* p = short_reason_description; + while (p && *p) + { + if (msg_.reason_text.size() == msg_.reason_text.capacity()) + { + break; + } + msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p)); + p++; + } + + UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str()); + + publishOnce(); + startPeriodic(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS)); + } /** * Stop broadcasting immediately. */ - void dontPanic(); // Where's my towel + void dontPanic() // Where's my towel + { + stop(); + msg_.reason_text.clear(); + } - bool isPanicking() const; + bool isPanicking() const { return isRunning(); } - const typename protocol::Panic::FieldTypes::reason_text& getReason() const; + const typename protocol::Panic::FieldTypes::reason_text& getReason() const + { + return msg_.reason_text; + } }; } diff --git a/libuavcan/src/protocol/uc_panic_broadcaster.cpp b/libuavcan/src/protocol/uc_panic_broadcaster.cpp deleted file mode 100644 index 8c73929586..0000000000 --- a/libuavcan/src/protocol/uc_panic_broadcaster.cpp +++ /dev/null @@ -1,61 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include - -namespace uavcan -{ - -void PanicBroadcaster::publishOnce() -{ - const int res = pub_.broadcast(msg_); - if (res < 0) - { - pub_.getNode().registerInternalFailure("Panic pub failed"); - } -} - -void PanicBroadcaster::handleTimerEvent(const TimerEvent&) -{ - publishOnce(); -} - -void PanicBroadcaster::panic(const char* short_reason_description) -{ - msg_.reason_text.clear(); - const char* p = short_reason_description; - while (p && *p) - { - if (msg_.reason_text.size() == msg_.reason_text.capacity()) - { - break; - } - msg_.reason_text.push_back(protocol::Panic::FieldTypes::reason_text::ValueType(*p)); - p++; - } - - UAVCAN_TRACE("PanicBroadcaster", "Panicking with reason '%s'", getReason().c_str()); - - publishOnce(); - startPeriodic(MonotonicDuration::fromMSec(protocol::Panic::BROADCASTING_INTERVAL_MS)); -} - -void PanicBroadcaster::dontPanic() -{ - stop(); - msg_.reason_text.clear(); -} - -bool PanicBroadcaster::isPanicking() const -{ - return isRunning(); -} - -const typename protocol::Panic::FieldTypes::reason_text& PanicBroadcaster::getReason() const -{ - return msg_.reason_text; -} - -} From 106cb9437986649ab6d1bce1d24dacd92720268e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:35:18 +0300 Subject: [PATCH 049/268] Added a README explaining what's going on --- libuavcan/include/uavcan/protocol/README.md | 13 +++++++++++++ 1 file changed, 13 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/README.md diff --git a/libuavcan/include/uavcan/protocol/README.md b/libuavcan/include/uavcan/protocol/README.md new file mode 100644 index 0000000000..1b29688f36 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/README.md @@ -0,0 +1,13 @@ +High-level protocol logic +========================= + +Classes defined in this directory implement some high-level functions of UAVCAN. + +Since most applications won't use all of them at the same time, their definitions are provided right in the header +files rather than in source files, in order to not pollute the build outputs with unused code (which is also a +requirement for most safety-critical software). + +However, some of the high-level functions that are either always used by the library itself or those that are extremely +likely to be used by the application are defined in .cpp files. + +When adding a new class here, please think twice before putting its definition into a .cpp file. From 15fa5ad163a6e56e661980cf05ae334d685b6a8e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:37:16 +0300 Subject: [PATCH 050/268] Param server moved to header --- .../include/uavcan/protocol/param_server.hpp | 89 ++++++++++++++++- libuavcan/src/protocol/uc_param_server.cpp | 95 ------------------- 2 files changed, 85 insertions(+), 99 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_param_server.cpp diff --git a/libuavcan/include/uavcan/protocol/param_server.hpp b/libuavcan/include/uavcan/protocol/param_server.hpp index 8f7bdef5e0..e5afb04943 100644 --- a/libuavcan/include/uavcan/protocol/param_server.hpp +++ b/libuavcan/include/uavcan/protocol/param_server.hpp @@ -5,10 +5,13 @@ #ifndef UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED #define UAVCAN_PROTOCOL_PARAM_SERVER_HPP_INCLUDED +#include #include #include #include #include +#include +#include namespace uavcan { @@ -101,10 +104,65 @@ class UAVCAN_EXPORT ParamServer ServiceServer save_erase_srv_; IParamManager* manager_; - void handleGetSet(const protocol::param::GetSet::Request& request, protocol::param::GetSet::Response& response); + void handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out) + { + UAVCAN_ASSERT(manager_ != NULL); - void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& request, - protocol::param::ExecuteOpcode::Response& response); + // Recover the name from index + if (in.name.empty()) + { + manager_->getParamNameByIndex(in.index, out.name); + UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str()); + } + else + { + out.name = in.name; + } + + if (out.name.empty()) + { + UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index)); + return; + } + + // Assign if needed, read back + if (!IParamManager::isValueEmpty(in.value)) + { + manager_->assignParamValue(out.name, in.value); + } + manager_->readParamValue(out.name, out.value); + + // Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about + if (!IParamManager::isValueEmpty(out.value)) + { + manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value); + } + else + { + UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str()); + out.name.clear(); + } + } + + void handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in, + protocol::param::ExecuteOpcode::Response& out) + { + UAVCAN_ASSERT(manager_ != NULL); + + if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE) + { + out.ok = manager_->saveAllParams() >= 0; + } + else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE) + { + out.ok = manager_->eraseAllParams() >= 0; + } + else + { + UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode)); + out.ok = false; + } + } public: explicit ParamServer(INode& node) @@ -117,8 +175,31 @@ public: * Starts the parameter server with given param manager instance. * Returns negative error code. */ - int start(IParamManager* manager); + int start(IParamManager* manager) + { + if (manager == NULL) + { + return -ErrInvalidParam; + } + manager_ = manager; + int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet)); + if (res < 0) + { + return res; + } + + res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode)); + if (res < 0) + { + get_set_srv_.stop(); + } + return res; + } + + /** + * @ref IParamManager + */ IParamManager* getParamManager() const { return manager_; } }; diff --git a/libuavcan/src/protocol/uc_param_server.cpp b/libuavcan/src/protocol/uc_param_server.cpp deleted file mode 100644 index 908400af91..0000000000 --- a/libuavcan/src/protocol/uc_param_server.cpp +++ /dev/null @@ -1,95 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include -#include -#include - -namespace uavcan -{ - -void ParamServer::handleGetSet(const protocol::param::GetSet::Request& in, protocol::param::GetSet::Response& out) -{ - UAVCAN_ASSERT(manager_ != NULL); - - // Recover the name from index - if (in.name.empty()) - { - manager_->getParamNameByIndex(in.index, out.name); - UAVCAN_TRACE("ParamServer", "GetSet: Index %i --> '%s'", int(in.index), out.name.c_str()); - } - else - { - out.name = in.name; - } - - if (out.name.empty()) - { - UAVCAN_TRACE("ParamServer", "GetSet: Can't resolve parameter name, index=%i", int(in.index)); - return; - } - - // Assign if needed, read back - if (!IParamManager::isValueEmpty(in.value)) - { - manager_->assignParamValue(out.name, in.value); - } - manager_->readParamValue(out.name, out.value); - - // Check if the value is OK, otherwise reset the name to indicate that we have no idea what is it all about - if (!IParamManager::isValueEmpty(out.value)) - { - manager_->readParamDefaultMaxMin(out.name, out.default_value, out.max_value, out.min_value); - } - else - { - UAVCAN_TRACE("ParamServer", "GetSet: Unknown param: index=%i name='%s'", int(in.index), out.name.c_str()); - out.name.clear(); - } -} - -void ParamServer::handleExecuteOpcode(const protocol::param::ExecuteOpcode::Request& in, - protocol::param::ExecuteOpcode::Response& out) -{ - UAVCAN_ASSERT(manager_ != NULL); - - if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_SAVE) - { - out.ok = manager_->saveAllParams() >= 0; - } - else if (in.opcode == protocol::param::ExecuteOpcode::Request::OPCODE_ERASE) - { - out.ok = manager_->eraseAllParams() >= 0; - } - else - { - UAVCAN_TRACE("ParamServer", "ExecuteOpcode: invalid opcode %i", int(in.opcode)); - out.ok = false; - } -} - -int ParamServer::start(IParamManager* manager) -{ - if (manager == NULL) - { - return -ErrInvalidParam; - } - manager_ = manager; - - int res = get_set_srv_.start(GetSetCallback(this, &ParamServer::handleGetSet)); - if (res < 0) - { - return res; - } - - res = save_erase_srv_.start(ExecuteOpcodeCallback(this, &ParamServer::handleExecuteOpcode)); - if (res < 0) - { - get_set_srv_.stop(); - } - return res; -} - -} From 9d20d983b50b1f01e0c46d8364799cf23b33da9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:38:12 +0300 Subject: [PATCH 051/268] Restart request server moved to header --- .../protocol/restart_request_server.hpp | 25 +++++++++++-- .../protocol/uc_restart_request_server.cpp | 36 ------------------- 2 files changed, 23 insertions(+), 38 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_restart_request_server.cpp diff --git a/libuavcan/include/uavcan/protocol/restart_request_server.hpp b/libuavcan/include/uavcan/protocol/restart_request_server.hpp index 4f9a414e13..d3cd23b5dc 100644 --- a/libuavcan/include/uavcan/protocol/restart_request_server.hpp +++ b/libuavcan/include/uavcan/protocol/restart_request_server.hpp @@ -5,6 +5,7 @@ #ifndef UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED #define UAVCAN_PROTOCOL_RESTART_REQUEST_SERVER_HPP_INCLUDED +#include #include #include #include @@ -44,7 +45,24 @@ class UAVCAN_EXPORT RestartRequestServer : Noncopyable IRestartRequestHandler* handler_; void handleRestartNode(const ReceivedDataStructure& request, - protocol::RestartNode::Response& response) const; + protocol::RestartNode::Response& response) const + { + UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get())); + response.ok = false; + if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER) + { + if (handler_) + { + response.ok = handler_->handleRestartRequest(request.getSrcNodeID()); + } + UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected")); + } + else + { + UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx", + static_cast(request.magic_number)); + } + } public: explicit RestartRequestServer(INode& node) @@ -63,7 +81,10 @@ public: * Starts the server. * Returns negative error code. */ - int start(); + int start() + { + return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode)); + } }; } diff --git a/libuavcan/src/protocol/uc_restart_request_server.cpp b/libuavcan/src/protocol/uc_restart_request_server.cpp deleted file mode 100644 index beb2daa495..0000000000 --- a/libuavcan/src/protocol/uc_restart_request_server.cpp +++ /dev/null @@ -1,36 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#include - -namespace uavcan -{ - -void RestartRequestServer::handleRestartNode(const ReceivedDataStructure& request, - protocol::RestartNode::Response& response) const -{ - UAVCAN_TRACE("RestartRequestServer", "Request from snid=%i", int(request.getSrcNodeID().get())); - response.ok = false; - if (request.magic_number == protocol::RestartNode::Request::MAGIC_NUMBER) - { - if (handler_) - { - response.ok = handler_->handleRestartRequest(request.getSrcNodeID()); - } - UAVCAN_TRACE("RestartRequestServer", "%s", (response.ok ? "Accepted" : "Rejected")); - } - else - { - UAVCAN_TRACE("RestartRequestServer", "Invalid magic number 0x%llx", - static_cast(request.magic_number)); - } -} - -int RestartRequestServer::start() -{ - return srv_.start(RestartNodeCallback(this, &RestartRequestServer::handleRestartNode)); -} - -} From 098c29ce935bc80839fe340742a6cc22e56403db Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 12:39:05 +0300 Subject: [PATCH 052/268] Transport stats provider moved to header --- .../protocol/transport_stats_provider.hpp | 25 +++++++++++- .../protocol/uc_transport_stats_provider.cpp | 40 ------------------- 2 files changed, 23 insertions(+), 42 deletions(-) delete mode 100644 libuavcan/src/protocol/uc_transport_stats_provider.cpp diff --git a/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp b/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp index 173e9c4b7c..34f8aaa0b0 100644 --- a/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp +++ b/libuavcan/include/uavcan/protocol/transport_stats_provider.hpp @@ -5,6 +5,7 @@ #ifndef UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED #define UAVCAN_PROTOCOL_TRANSPORT_STATS_PROVIDER_HPP_INCLUDED +#include #include #include #include @@ -25,7 +26,24 @@ class UAVCAN_EXPORT TransportStatsProvider : Noncopyable ServiceServer srv_; void handleGetTransportStats(const protocol::GetTransportStats::Request&, - protocol::GetTransportStats::Response& resp) const; + protocol::GetTransportStats::Response& resp) const + { + const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter(); + resp.transfer_errors = perf.getErrorCount(); + resp.transfers_tx = perf.getTxTransferCount(); + resp.transfers_rx = perf.getRxTransferCount(); + + const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager(); + for (uint8_t i = 0; i < canio.getNumIfaces(); i++) + { + const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i); + protocol::CANIfaceStats stats; + stats.errors = can_perf.errors; + stats.frames_tx = can_perf.frames_tx; + stats.frames_rx = can_perf.frames_rx; + resp.can_iface_stats.push_back(stats); + } + } public: explicit TransportStatsProvider(INode& node) @@ -36,7 +54,10 @@ public: * Once started, this class requires no further attention. * Returns negative error code. */ - int start(); + int start() + { + return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats)); + } }; } diff --git a/libuavcan/src/protocol/uc_transport_stats_provider.cpp b/libuavcan/src/protocol/uc_transport_stats_provider.cpp deleted file mode 100644 index 63c31c4ba7..0000000000 --- a/libuavcan/src/protocol/uc_transport_stats_provider.cpp +++ /dev/null @@ -1,40 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#include -#if !UAVCAN_TINY - -#include - -namespace uavcan -{ - -void TransportStatsProvider::handleGetTransportStats(const protocol::GetTransportStats::Request&, - protocol::GetTransportStats::Response& resp) const -{ - const TransferPerfCounter& perf = srv_.getNode().getDispatcher().getTransferPerfCounter(); - resp.transfer_errors = perf.getErrorCount(); - resp.transfers_tx = perf.getTxTransferCount(); - resp.transfers_rx = perf.getRxTransferCount(); - - const CanIOManager& canio = srv_.getNode().getDispatcher().getCanIOManager(); - for (uint8_t i = 0; i < canio.getNumIfaces(); i++) - { - const CanIfacePerfCounters can_perf = canio.getIfacePerfCounters(i); - protocol::CANIfaceStats stats; - stats.errors = can_perf.errors; - stats.frames_tx = can_perf.frames_tx; - stats.frames_rx = can_perf.frames_rx; - resp.can_iface_stats.push_back(stats); - } -} - -int TransportStatsProvider::start() -{ - return srv_.start(GetTransportStatsCallback(this, &TransportStatsProvider::handleGetTransportStats)); -} - -} - -#endif From 18d5cb78aadcea61fa76c49437d82041f30b47cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 16:03:22 +0300 Subject: [PATCH 053/268] Refactored node ID allocation server; no changes to the logic --- .../dynamic_node_id_allocation_server.hpp | 806 ------- .../allocation_request_manager.hpp | 255 +++ .../distributed/cluster_manager.hpp | 445 ++++ .../distributed/event.hpp | 127 ++ .../distributed/log.hpp | 318 +++ .../distributed/persistent_state.hpp | 225 ++ .../distributed/raft_core.hpp | 860 ++++++++ .../distributed/server.hpp | 201 ++ .../distributed/types.hpp | 29 + .../storage_backend.hpp | 62 + .../storage_marshaller.hpp | 181 ++ .../protocol/dynamic_node_id_server/types.hpp | 27 + .../uc_dynamic_node_id_allocation_server.cpp | 1893 ----------------- .../server.cpp} | 154 +- 14 files changed, 2810 insertions(+), 2773 deletions(-) delete mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp delete mode 100644 libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp rename libuavcan/test/protocol/{dynamic_node_id_allocation_server.cpp => dynamic_node_id_server/server.cpp} (86%) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp deleted file mode 100644 index 3e1315a9af..0000000000 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_server.hpp +++ /dev/null @@ -1,806 +0,0 @@ -/* - * Copyright (C) 2015 Pavel Kirienko - */ - -#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -// Types used by the server -#include -#include -#include -#include -#include -#include -#include - -namespace uavcan -{ -/** - * This interface is used by the server to read and write stable storage. - * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 - * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward - * 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 IDynamicNodeIDStorageBackend -{ -public: - /** - * Maximum length of keys and values. One pair takes twice as much space. - */ - enum { MaxStringLength = 32 }; - - /** - * It is guaranteed that the server will never require more than this number of key/value pairs. - * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. - */ - enum { MaxKeyValuePairs = 400 }; - - /** - * 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, ArrayModeDynamic, MaxStringLength> String; - - /** - * Read one value from the storage. - * If such key does not exist, or if read failed, an empty string will be returned. - * This method should not block for more than 50 ms. - */ - virtual String get(const String& key) const = 0; - - /** - * Create or update value for the given key. Empty value should be regarded as a request to delete the key. - * This method should not block for more than 50 ms. - * Failures will be ignored. - */ - virtual void set(const String& key, const String& value) = 0; - - virtual ~IDynamicNodeIDStorageBackend() { } -}; - -/** - * This interface allows the application to trace events that happen in the server. - */ -class IDynamicNodeIDAllocationServerEventTracer -{ -public: -#if UAVCAN_TOSTRING - /** - * It is safe to call this function with any argument. - * If the event code is out of range, an assertion failure will be triggered and an error text will be returned. - */ - static const char* getEventName(uint16_t code); -#endif - - /** - * The server invokes this method every time it believes that a noteworthy event has happened. - * The table of event codes can be found in the server sources. - * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures - * full backward compatibility. - * @param event_code Event code, see the sources for the enum with values. - * @param event_argument Value associated with the event; its meaning depends on the event code. - */ - virtual void onEvent(uint16_t event_code, int64_t event_argument) = 0; - - virtual ~IDynamicNodeIDAllocationServerEventTracer() { } -}; - -/** - * Internals, do not use anything from this namespace directly. - */ -namespace dynamic_node_id_server_impl -{ - -using namespace protocol::dynamic_node_id::server; - -/** - * Raft term - */ -typedef StorageType::Type Term; - -/** - * @ref IDynamicNodeIDAllocationServerEventTracer. - * Event codes cannot be changed, only new ones can be added. - */ -enum TraceEvent -{ - // Event name Argument - // 0 - TraceError, // error code (may be negated) - TraceLogLastIndexRestored, // recovered last index value - TraceLogAppend, // index of new entry - TraceLogRemove, // new last index value - TraceCurrentTermRestored, // current term - // 5 - TraceCurrentTermUpdate, // current term - TraceVotedForRestored, // value of votedFor - TraceVotedForUpdate, // value of votedFor - TraceDiscoveryBroadcast, // number of known servers - TraceNewServerDiscovered, // node ID of the new server - // 10 - TraceDiscoveryReceived, // node ID of the sender - TraceClusterSizeInited, // cluster size - TraceInvalidClusterSizeReceived, // received cluster size - TraceRaftCoreInited, // update interval in usec - TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader - // 15 - TraceRaftActiveSwitch, // 0 - Passive, 1 - Active - TraceRaftNewLogEntry, // node ID value - TraceRaftRequestIgnored, // node ID of the client - TraceRaftVoteRequestReceived, // node ID of the client - TraceRaftVoteRequestSucceeded, // node ID of the server - // 20 - TraceRaftVoteRequestInitiation, // node ID of the server - TraceRaftPersistStateUpdateError, // negative error code - TraceRaftCommitIndexUpdate, // new commit index value - TraceRaftNewerTermInResponse, // new term value - TraceRaftNewEntryCommitted, // new commit index value - // 25 - TraceRaftAppendEntriesCallFailure, // error code (may be negated) - - NumTraceEventCodes -}; - -/** - * This class extends the storage backend interface with serialization/deserialization functionality. - */ -class MarshallingStorageDecorator -{ - IDynamicNodeIDStorageBackend& storage_; - - static uint8_t convertLowerCaseHexCharToNibble(char ch); - -public: - MarshallingStorageDecorator(IDynamicNodeIDStorageBackend& storage) - : storage_(storage) - { - // Making sure that there will be no data loss during serialization. - StaticAssert<(sizeof(Term) <= sizeof(uint32_t))>::check(); - } - - /** - * These methods set the value and then immediately read it back. - * 1. Serialize the value. - * 2. Update the value on the backend. - * 3. Call get() with the same value argument. - * The caller then is supposed to check whether the argument has the desired value. - */ - int setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value); - int setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, - Entry::FieldTypes::unique_id& inout_value); - - /** - * Getters simply read and deserialize the value. - * 1. Read the value back from the backend; return false if read fails. - * 2. Deserealize the newly read value; return false if deserialization fails. - * 3. Update the argument with deserialized value. - * 4. Return true. - */ - int get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const; - int get(const IDynamicNodeIDStorageBackend::String& key, - Entry::FieldTypes::unique_id& out_value) const; -}; - -/** - * Raft log. - * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. - * Writes are slow, reads are instantaneous. - */ -class Log -{ -public: - typedef uint8_t Index; - - enum { Capacity = NodeID::Max + 1 }; - -private: - IDynamicNodeIDStorageBackend& storage_; - IDynamicNodeIDAllocationServerEventTracer& tracer_; - Entry entries_[Capacity]; - Index last_index_; // Index zero always contains an empty entry - - static IDynamicNodeIDStorageBackend::String getLastIndexKey() { return "log_last_index"; } - static IDynamicNodeIDStorageBackend::String makeEntryKey(Index index, const char* postfix); - - int readEntryFromStorage(Index index, Entry& out_entry); - int writeEntryToStorage(Index index, const Entry& entry); - - int initEmptyLogStorage(); - -public: - Log(IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer) - : storage_(storage) - , tracer_(tracer) - , 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(); - - /** - * This method invokes storage IO. - * Returned value indicates whether the entry was successfully appended. - */ - int append(const Entry& entry); - - /** - * This method invokes storage IO. - * Returned value indicates whether the requested operation has been carried out successfully. - */ - int removeEntriesWhereIndexGreaterOrEqual(Index index); - int removeEntriesWhereIndexGreater(Index index); - - /** - * Returns nullptr if there's no such index. - * This method does not use storage IO. - */ - const Entry* getEntryAtIndex(Index index) const; - - Index getLastIndex() const { return last_index_; } - - bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const; -}; - - -/** - * This class is a convenient container for persistent state variables defined by Raft. - * Writes are slow, reads are instantaneous. - */ -class PersistentState -{ - IDynamicNodeIDStorageBackend& storage_; - IDynamicNodeIDAllocationServerEventTracer& tracer_; - - Term current_term_; - NodeID voted_for_; - Log log_; - - static IDynamicNodeIDStorageBackend::String getCurrentTermKey() { return "current_term"; } - static IDynamicNodeIDStorageBackend::String getVotedForKey() { return "voted_for"; } - -public: - PersistentState(IDynamicNodeIDStorageBackend& storage, IDynamicNodeIDAllocationServerEventTracer& tracer) - : storage_(storage) - , tracer_(tracer) - , current_term_(0) - , log_(storage, tracer) - { } - - int init(); - - Term getCurrentTerm() const { return current_term_; } - - NodeID getVotedFor() const { return voted_for_; } - bool isVotedForSet() const { return voted_for_.isUnicast(); } - - Log& getLog() { return log_; } - const Log& getLog() const { return log_; } - - /** - * Invokes storage IO. - */ - int setCurrentTerm(Term term); - - /** - * Invokes storage IO. - */ - int setVotedFor(NodeID node_id); - int resetVotedFor() { return setVotedFor(NodeID(0)); } -}; - -/** - * This class maintains the cluster state. - */ -class ClusterManager : private TimerBase -{ -public: - enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize }; - -private: - typedef MethodBinder&)> - DiscoveryCallback; - - struct Server - { - NodeID node_id; - Log::Index next_index; - Log::Index match_index; - - Server() - : next_index(0) - , match_index(0) - { } - - void resetIndices(const Log& log); - }; - - IDynamicNodeIDStorageBackend& storage_; - IDynamicNodeIDAllocationServerEventTracer& tracer_; - const Log& log_; - - Subscriber discovery_sub_; - mutable Publisher discovery_pub_; - - Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there. - - uint8_t cluster_size_; - uint8_t num_known_servers_; - - bool had_discovery_activity_; - - static IDynamicNodeIDStorageBackend::String getStorageKeyForClusterSize() { return "cluster_size"; } - - INode& getNode() { return discovery_sub_.getNode(); } - const INode& getNode() const { return discovery_sub_.getNode(); } - - Server* findServer(NodeID node_id); - const Server* findServer(NodeID node_id) const; - void addServer(NodeID node_id); - - virtual void handleTimerEvent(const TimerEvent&); - - void handleDiscovery(const ReceivedDataStructure& msg); - - void startDiscoveryPublishingTimerIfNotRunning(); - -public: - enum { ClusterSizeUnknown = 0 }; - - /** - * @param node Needed to publish and subscribe to Discovery message - * @param storage Needed to read the cluster size parameter from the storage - * @param log Needed to initialize nextIndex[] values after elections - */ - ClusterManager(INode& node, IDynamicNodeIDStorageBackend& storage, const Log& log, - IDynamicNodeIDAllocationServerEventTracer& tracer) - : TimerBase(node) - , storage_(storage) - , tracer_(tracer) - , log_(log) - , discovery_sub_(node) - , discovery_pub_(node) - , cluster_size_(0) - , num_known_servers_(0) - , had_discovery_activity_(false) - { } - - /** - * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the - * storage backend using key 'cluster_size'. - * Returns negative error code. - */ - int init(uint8_t init_cluster_size = ClusterSizeUnknown); - - /** - * Whether such server has been discovered earlier. - */ - bool isKnownServer(NodeID node_id) const; - - /** - * An invalid node ID will be returned if there's no such server. - * The local server is not listed there. - */ - NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const; - - /** - * See next_index[] in Raft paper. - */ - Log::Index getServerNextIndex(NodeID server_node_id) const; - void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment); - void decrementServerNextIndex(NodeID server_node_id); - - /** - * See match_index[] in Raft paper. - */ - Log::Index getServerMatchIndex(NodeID server_node_id) const; - void setServerMatchIndex(NodeID server_node_id, Log::Index match_index); - - /** - * This method must be called when the current server becomes leader. - */ - void resetAllServerIndices(); - - /** - * 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. - */ - uint8_t getNumKnownServers() const { return num_known_servers_; } - - /** - * Cluster size and quorum size are constant. - */ - uint8_t getClusterSize() const { return cluster_size_; } - uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } - - bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); } -}; - -/** - * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log. - */ -class ILeaderLogCommitHandler -{ -public: - /** - * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). - */ - virtual void onEntryCommitted(const Entry& entry) = 0; - - /** - * Assume false by default. - */ - virtual void onLeaderChange(bool local_node_is_leader) = 0; - - virtual ~ILeaderLogCommitHandler() { } -}; - -/** - * This class implements log replication and voting. - * It does not implement client-server interaction at all; instead it just exposes a public method for adding - * allocation entries. - */ -class RaftCore : private TimerBase -{ - typedef MethodBinder&, - ServiceResponseDataStructure&)> - AppendEntriesCallback; - - typedef MethodBinder&)> - AppendEntriesResponseCallback; - - typedef MethodBinder&, - ServiceResponseDataStructure&)> - RequestVoteCallback; - - typedef MethodBinder&)> - RequestVoteResponseCallback; - - enum ServerState - { - ServerStateFollower, - ServerStateCandidate, - ServerStateLeader - }; - - struct PendingAppendEntriesFields - { - Log::Index prev_log_index; - Log::Index num_entries; - - PendingAppendEntriesFields() - : prev_log_index(0) - , num_entries(0) - { } - }; - - /* - * Constants - */ - const MonotonicDuration update_interval_; ///< AE requests will be issued at this rate - const MonotonicDuration base_activity_timeout_; - - IDynamicNodeIDAllocationServerEventTracer& tracer_; - ILeaderLogCommitHandler& log_commit_handler_; - - /* - * States - */ - PersistentState persistent_state_; - ClusterManager cluster_; - Log::Index commit_index_; - - MonotonicTime last_activity_timestamp_; - bool active_mode_; - ServerState server_state_; - - uint8_t next_server_index_; ///< Next server to query AE from - uint8_t num_votes_received_in_this_campaign_; - - PendingAppendEntriesFields pending_append_entries_fields_; - - /* - * Transport - */ - ServiceServer append_entries_srv_; - ServiceClient append_entries_client_; - ServiceServer request_vote_srv_; - - enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 }; - LazyConstructor > - request_vote_clients_[NumRequestVoteClients]; - - void trace(TraceEvent event, int64_t argument) { tracer_.onEvent(event, argument); } - - INode& getNode() { return append_entries_srv_.getNode(); } - const INode& getNode() const { return append_entries_srv_.getNode(); } - - void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); } - bool isActivityTimedOut() const; - - void handlePersistentStateUpdateError(int error); - - void updateFollower(); - void updateCandidate(); - void updateLeader(); - - void switchState(ServerState new_state); - void setActiveMode(bool new_active); - - void tryIncrementCurrentTermFromResponse(Term new_term); - - void propagateCommitIndex(); - - void handleAppendEntriesRequest(const ReceivedDataStructure& request, - ServiceResponseDataStructure& response); - - void handleAppendEntriesResponse(const ServiceCallResult& result); - - void handleRequestVoteRequest(const ReceivedDataStructure& request, - ServiceResponseDataStructure& response); - - void handleRequestVoteResponse(const ServiceCallResult& result); - - virtual void handleTimerEvent(const TimerEvent& event); - -public: - RaftCore(INode& node, - IDynamicNodeIDStorageBackend& storage, - IDynamicNodeIDAllocationServerEventTracer& tracer, - ILeaderLogCommitHandler& log_commit_handler, - MonotonicDuration update_interval = - MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS), - MonotonicDuration base_activity_timeout = - MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_BASE_ELECTION_TIMEOUT_MS)) - : TimerBase(node) - , update_interval_(update_interval) - , base_activity_timeout_(base_activity_timeout) - , tracer_(tracer) - , log_commit_handler_(log_commit_handler) - , 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) - , 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(node); - } - } - - /** - * Once started, the logic runs in the background until destructor is called. - * @param cluster_size If set, this value will be used and stored in the persistent storage. If not set, - * value from the persistent storage will be used. If not set and there's no such key - * in the persistent storage, initialization will fail. - */ - int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown); - - /** - * This function is mostly needed for testing. - */ - Log::Index getCommitIndex() const { return commit_index_; } - - /** - * Only the leader can call @ref appendLog(). - */ - bool isLeader() const { return server_state_ == ServerStateLeader; } - - /** - * Inserts one entry into log. - * Failures are tolerble because all operations are idempotent. - * This method will trigger an assertion failure and return error if the current node is not the leader. - */ - int appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id); - - /** - * This class is used to perform log searches. - */ - struct LogEntryInfo - { - Entry entry; - bool committed; - - LogEntryInfo(const Entry& arg_entry, bool arg_committed) - : entry(arg_entry) - , committed(arg_committed) - { } - }; - - /** - * This method is used by the allocator to query existence of certain entries in the Raft log. - * Predicate is a callable of the following prototype: - * bool (const LogEntryInfo& entry) - * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy - * contructor to the last visited entry; otherwise the constructor will not be initialized. In this case, lazy - * constructor is used as boost::optional. - * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. - */ - template - inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const - { - UAVCAN_ASSERT(try_implicit_cast(predicate, true)); - for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index >= 0; index--) - { - const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); - UAVCAN_ASSERT(entry != NULL); - const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_); - if (predicate(info)) - { - LazyConstructor ret; - ret.template construct(info); - return ret; - } - } - return LazyConstructor(); - } -}; - -/** - * The main allocator must implement this interface. - */ -class IAllocationRequestHandler -{ -public: - typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID; - - virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; - - virtual ~IAllocationRequestHandler() { } -}; - -/** - * This class manages communication with allocation clients. - * Three-stage unique ID exchange is implemented here, as well as response publication. - */ -class AllocationRequestManager -{ - typedef MethodBinder&)> - AllocationCallback; - - const MonotonicDuration stage_timeout_; - - bool active_; - MonotonicTime last_message_timestamp_; - protocol::dynamic_node_id::Allocation::FieldTypes::unique_id current_unique_id_; - - IAllocationRequestHandler& handler_; - - Subscriber allocation_sub_; - Publisher allocation_pub_; - - enum { InvalidStage = 0 }; - - static uint8_t detectRequestStage(const protocol::dynamic_node_id::Allocation& msg); - uint8_t getExpectedStage() const; - - void broadcastIntermediateAllocationResponse(); - - void handleAllocation(const ReceivedDataStructure& msg); - -public: - AllocationRequestManager(INode& node, IAllocationRequestHandler& handler) - : stage_timeout_(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS)) - , active_(false) - , handler_(handler) - , allocation_sub_(node) - , allocation_pub_(node) - { } - - int init(); - - void setActive(bool x); - bool isActive() const { return active_; } - - int broadcastAllocationResponse(const IAllocationRequestHandler::UniqueID& unique_id, NodeID allocated_node_id); -}; - -} // namespace dynamic_node_id_server_impl - -/** - * This class implements the top-level allocation logic and server API. - */ -class DynamicNodeIDAllocationServer : private dynamic_node_id_server_impl::IAllocationRequestHandler - , private dynamic_node_id_server_impl::ILeaderLogCommitHandler -{ - typedef MethodBinder&)> NodeStatusCallback; - - typedef MethodBinder&)> - GetNodeInfoResponseCallback; - - typedef Map PendingGetNodeInfoAttemptsMap; - - enum { MaxGetNodeInfoAttempts = 5 }; - - /* - * States - */ - PendingGetNodeInfoAttemptsMap pending_get_node_info_attempts_; - dynamic_node_id_server_impl::RaftCore raft_core_; - dynamic_node_id_server_impl::AllocationRequestManager allocation_request_manager_; - - /* - * Transport - */ - Subscriber node_status_sub_; - ServiceClient get_node_info_client_; - - INode& getNode() { return get_node_info_client_.getNode(); } - - bool isNodeIDTaken(const NodeID node_id) const; - NodeID findFreeNodeID(const NodeID node_id) const; - - void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id); - - virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id); - - virtual void onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry); - - virtual void onLeaderChange(bool local_node_is_leader); - - void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry); - -public: - DynamicNodeIDAllocationServer(INode& node, - IDynamicNodeIDStorageBackend& storage, - IDynamicNodeIDAllocationServerEventTracer& tracer) - : pending_get_node_info_attempts_(node.getAllocator()) - , raft_core_(node, storage, tracer, *this) - , allocation_request_manager_(node, *this) - , node_status_sub_(node) - , get_node_info_client_(node) - { } - - enum { ClusterSizeUnknown = dynamic_node_id_server_impl::ClusterManager::ClusterSizeUnknown }; - - int init(uint8_t cluster_size = ClusterSizeUnknown); -}; - -} - -#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_SERVER_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp new file mode 100644 index 0000000000..94630c1f06 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -0,0 +1,255 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * The main allocator must implement this interface. + */ +class IAllocationRequestHandler +{ +public: + virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; + + virtual ~IAllocationRequestHandler() { } +}; + +/** + * This class manages communication with allocation clients. + * Three-stage unique ID exchange is implemented here, as well as response publication. + */ +class AllocationRequestManager +{ + typedef MethodBinder&)> + AllocationCallback; + + const MonotonicDuration stage_timeout_; + + bool active_; + MonotonicTime last_message_timestamp_; + protocol::dynamic_node_id::Allocation::FieldTypes::unique_id current_unique_id_; + + IAllocationRequestHandler& handler_; + + Subscriber allocation_sub_; + Publisher allocation_pub_; + + enum { InvalidStage = 0 }; + + static uint8_t detectRequestStage(const protocol::dynamic_node_id::Allocation& msg) + { + const uint8_t max_bytes_per_request = protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + + if ((msg.unique_id.size() != max_bytes_per_request) && + (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) && + (msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD + { + return InvalidStage; + } + if (msg.first_part_of_unique_id) + { + return 1; // Note that CAN FD frames can deliver the unique ID in one stage! + } + if (msg.unique_id.size() == protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + if (msg.unique_id.size() < protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 3; + } + return InvalidStage; + } + + uint8_t getExpectedStage() const + { + if (current_unique_id_.empty()) + { + return 1; + } + if (current_unique_id_.size() >= (protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) + { + return 3; + } + if (current_unique_id_.size() >= protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + return InvalidStage; + } + + void broadcastIntermediateAllocationResponse() + { + UAVCAN_ASSERT(active_); + + protocol::dynamic_node_id::Allocation msg; + msg.unique_id = current_unique_id_; + UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); + + UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", + unsigned(msg.unique_id.size())); + + const int res = allocation_pub_.broadcast(msg); + if (res < 0) + { + allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast"); + } + } + + void handleAllocation(const ReceivedDataStructure& msg) + { + if (!msg.isAnonymousTransfer()) + { + return; // This is a response from another allocator, ignore + } + + if (!active_) + { + return; // The local node is not the leader, ignore + } + + /* + * Reset the expected stage on timeout + */ + if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_)) + { + UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); + current_unique_id_.clear(); + } + last_message_timestamp_ = msg.getMonotonicTimestamp(); + + /* + * Checking if request stage matches the expected stage + */ + const uint8_t request_stage = detectRequestStage(msg); + if (request_stage == InvalidStage) + { + return; // No way + } + + const uint8_t expected_stage = getExpectedStage(); + if (request_stage == InvalidStage) + { + current_unique_id_.clear(); + return; + } + + if (request_stage != expected_stage) + { + return; // Ignore - stage mismatch + } + + const uint8_t max_expected_bytes = static_cast(current_unique_id_.capacity() - current_unique_id_.size()); + UAVCAN_ASSERT(max_expected_bytes > 0); + if (msg.unique_id.size() > max_expected_bytes) + { + return; // Malformed request + } + + /* + * Updating the local state + */ + for (uint8_t i = 0; i < msg.unique_id.size(); i++) + { + current_unique_id_.push_back(msg.unique_id[i]); + } + + /* + * Proceeding with allocation if possible + */ + if (current_unique_id_.size() == current_unique_id_.capacity()) + { + UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d", + int(msg.node_id)); + + UniqueID unique_id; + copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); + current_unique_id_.clear(); + + handler_.handleAllocationRequest(unique_id, msg.node_id); + } + else + { + broadcastIntermediateAllocationResponse(); + } + } + +public: + AllocationRequestManager(INode& node, IAllocationRequestHandler& handler) + : stage_timeout_(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS)) + , active_(false) + , handler_(handler) + , allocation_sub_(node) + , allocation_pub_(node) + { } + + int init() + { + int res = allocation_pub_.init(); + if (res < 0) + { + return res; + } + (void)allocation_pub_.setPriority(TransferPriorityLow); + + res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation)); + if (res < 0) + { + return res; + } + allocation_sub_.allowAnonymousTransfers(); + + return 0; + } + + void setActive(bool x) + { + active_ = x; + if (!active_) + { + current_unique_id_.clear(); + } + } + + bool isActive() const { return active_; } + + int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id) + { + if (!active_) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + protocol::dynamic_node_id::Allocation msg; + + msg.unique_id.resize(msg.unique_id.capacity()); + copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); + + msg.node_id = allocated_node_id.get(); + + return allocation_pub_.broadcast(msg); + } +}; + +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp new file mode 100644 index 0000000000..3166962276 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -0,0 +1,445 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_CLUSTER_MANAGER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class maintains the cluster state. + */ +class ClusterManager : private TimerBase +{ +public: + enum { MaxClusterSize = Discovery::FieldTypes::known_nodes::MaxSize }; + +private: + typedef MethodBinder&)> + DiscoveryCallback; + + struct Server + { + NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + Server() + : next_index(0) + , match_index(0) + { } + + void resetIndices(const Log& log) + { + next_index = Log::Index(log.getLastIndex() + 1U); + match_index = 0; + } + }; + + IStorageBackend& storage_; + IEventTracer& tracer_; + const Log& log_; + + Subscriber discovery_sub_; + mutable Publisher discovery_pub_; + + Server servers_[MaxClusterSize - 1]; ///< Minus one because the local server is not listed there. + + 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(); } + const INode& getNode() const { return discovery_sub_.getNode(); } + + const Server* findServer(NodeID node_id) const { return const_cast(this)->findServer(node_id); } + Server* findServer(NodeID node_id) + { + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + if (servers_[i].node_id == node_id) + { + return &servers_[i]; + } + } + return NULL; + } + + void addServer(NodeID node_id) + { + UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); + if (!isKnownServer(node_id) && node_id.isUnicast()) + { + tracer_.onEvent(TraceNewServerDiscovered, node_id.get()); + servers_[num_known_servers_].node_id = node_id; + servers_[num_known_servers_].resetIndices(log_); + num_known_servers_ = static_cast(num_known_servers_ + 1U); + } + else + { + UAVCAN_ASSERT(0); + } + } + + virtual void handleTimerEvent(const TimerEvent&) + { + UAVCAN_ASSERT(num_known_servers_ < cluster_size_); + + tracer_.onEvent(TraceDiscoveryBroadcast, num_known_servers_); + + /* + * Filling the message + */ + Discovery msg; + msg.configured_cluster_size = cluster_size_; + + msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0 + + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + msg.known_nodes.push_back(servers_[i].node_id.get()); + } + + UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1)); + + /* + * Broadcasting + */ + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Broadcasting Discovery message; known nodes: %d of %d", + int(msg.known_nodes.size()), int(cluster_size_)); + + const int res = discovery_pub_.broadcast(msg); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcst failed: %d", res); + getNode().registerInternalFailure("Raft discovery broadcast"); + } + + /* + * Termination condition + */ + if (isClusterDiscovered()) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcasting timer stopped"); + stop(); + } + } + + void handleDiscovery(const ReceivedDataStructure& msg) + { + tracer_.onEvent(TraceDiscoveryReceived, msg.getSrcNodeID().get()); + + /* + * Validating cluster configuration + * If there's a case of misconfiguration, the message will be ignored. + */ + if (msg.configured_cluster_size != cluster_size_) + { + tracer_.onEvent(TraceInvalidClusterSizeReceived, msg.configured_cluster_size); + getNode().registerInternalFailure("Bad Raft cluster size"); + return; + } + + had_discovery_activity_ = true; + + /* + * Updating the set of known servers + */ + for (uint8_t i = 0; i < msg.known_nodes.size(); i++) + { + if (isClusterDiscovered()) + { + break; + } + + const NodeID node_id(msg.known_nodes[i]); + if (node_id.isUnicast() && !isKnownServer(node_id)) + { + addServer(node_id); + } + } + + /* + * Publishing a new Discovery request if the publishing server needs to learn about more servers. + */ + if (msg.configured_cluster_size > msg.known_nodes.size()) + { + startDiscoveryPublishingTimerIfNotRunning(); + } + } + + void startDiscoveryPublishingTimerIfNotRunning() + { + if (!isRunning()) + { + startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_INTERVAL_MS)); + } + } + +public: + enum { ClusterSizeUnknown = 0 }; + + /** + * @param node Needed to publish and subscribe to Discovery message + * @param storage Needed to read the cluster size parameter from the storage + * @param log Needed to initialize nextIndex[] values after elections + */ + ClusterManager(INode& node, IStorageBackend& storage, const Log& log, IEventTracer& tracer) + : TimerBase(node) + , storage_(storage) + , tracer_(tracer) + , log_(log) + , discovery_sub_(node) + , discovery_pub_(node) + , cluster_size_(0) + , num_known_servers_(0) + , had_discovery_activity_(false) + { } + + /** + * If cluster_size is set to ClusterSizeUnknown, the class will try to read this parameter from the + * storage backend using key 'cluster_size'. + * Returns negative error code. + */ + int init(uint8_t init_cluster_size = ClusterSizeUnknown) + { + /* + * Figuring out the cluster size + */ + if (init_cluster_size == ClusterSizeUnknown) + { + // Reading from the storage + StorageMarshaller io(storage_); + uint32_t value = 0; + int res = io.get(getStorageKeyForClusterSize(), value); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", + "Cluster size is neither configured nor stored in the storage"); + return res; + } + if ((value == 0) || (value > MaxClusterSize)) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster size is invalid"); + return -ErrFailure; + } + cluster_size_ = static_cast(value); + } + else + { + if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize)) + { + return -ErrInvalidParam; + } + cluster_size_ = init_cluster_size; + + // Writing the storage + StorageMarshaller io(storage_); + uint32_t value = init_cluster_size; + int res = io.setAndGetBack(getStorageKeyForClusterSize(), value); + if ((res < 0) || (value != init_cluster_size)) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Failed to store cluster size"); + return -ErrFailure; + } + } + + tracer_.onEvent(TraceClusterSizeInited, cluster_size_); + + UAVCAN_ASSERT(cluster_size_ > 0); + UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize); + + /* + * Initializing pub/sub and timer + */ + int res = discovery_pub_.init(); + if (res < 0) + { + return res; + } + (void)discovery_pub_.setPriority(TransferPriorityLow); + + res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery)); + if (res < 0) + { + return res; + } + + startDiscoveryPublishingTimerIfNotRunning(); + + /* + * Misc + */ + resetAllServerIndices(); + return 0; + } + + /** + * Whether such server has been discovered. + */ + bool isKnownServer(NodeID node_id) const + { + if (node_id == getNode().getNodeID()) + { + return true; + } + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); + if (servers_[i].node_id == node_id) + { + return true; + } + } + return false; + } + + /** + * An invalid node ID will be returned if there's no such server. + * The local server is not listed there. + */ + NodeID getRemoteServerNodeIDAtIndex(uint8_t index) const + { + if (index < num_known_servers_) + { + return servers_[index].node_id; + } + return NodeID(); + } + + /** + * See next_index[] in Raft paper. + */ + Log::Index getServerNextIndex(NodeID server_node_id) const + { + const Server* const s = findServer(server_node_id); + if (s != NULL) + { + return s->next_index; + } + UAVCAN_ASSERT(0); + return 0; + } + + void incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment) + { + Server* const s = findServer(server_node_id); + if (s != NULL) + { + s->next_index = Log::Index(s->next_index + increment); + } + else + { + UAVCAN_ASSERT(0); + } + } + + void decrementServerNextIndex(NodeID server_node_id) + { + Server* const s = findServer(server_node_id); + if (s != NULL) + { + s->next_index--; + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * See match_index[] in Raft paper. + */ + Log::Index getServerMatchIndex(NodeID server_node_id) const + { + const Server* const s = findServer(server_node_id); + if (s != NULL) + { + return s->match_index; + } + UAVCAN_ASSERT(0); + return 0; + } + + void setServerMatchIndex(NodeID server_node_id, Log::Index match_index) + { + Server* const s = findServer(server_node_id); + if (s != NULL) + { + s->match_index = match_index; + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This method must be called when the current server becomes leader. + */ + void resetAllServerIndices() + { + for (uint8_t i = 0; i < num_known_servers_; i++) + { + UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); + servers_[i].resetIndices(log_); + } + } + + /** + * 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. + */ + uint8_t getNumKnownServers() const { return num_known_servers_; } + + /** + * Cluster size and quorum size are constant. + */ + uint8_t getClusterSize() const { return cluster_size_; } + uint8_t getQuorumSize() const { return static_cast(cluster_size_ / 2U + 1U); } + + bool isClusterDiscovered() const { return num_known_servers_ == (cluster_size_ - 1); } +}; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp new file mode 100644 index 0000000000..a82c100ab3 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp @@ -0,0 +1,127 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_EVENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_EVENT_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * @ref IEventTracer. + * Event codes cannot be changed, only new ones can be added. + */ +enum TraceCode +{ + // Event name Argument + // 0 + TraceError, // error code (may be negated) + TraceLogLastIndexRestored, // recovered last index value + TraceLogAppend, // index of new entry + TraceLogRemove, // new last index value + TraceCurrentTermRestored, // current term + // 5 + TraceCurrentTermUpdate, // current term + TraceVotedForRestored, // value of votedFor + TraceVotedForUpdate, // value of votedFor + TraceDiscoveryBroadcast, // number of known servers + TraceNewServerDiscovered, // node ID of the new server + // 10 + TraceDiscoveryReceived, // node ID of the sender + TraceClusterSizeInited, // cluster size + TraceInvalidClusterSizeReceived, // received cluster size + TraceRaftCoreInited, // update interval in usec + TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader + // 15 + TraceRaftActiveSwitch, // 0 - Passive, 1 - Active + TraceRaftNewLogEntry, // node ID value + TraceRaftRequestIgnored, // node ID of the client + TraceRaftVoteRequestReceived, // node ID of the client + TraceRaftVoteRequestSucceeded, // node ID of the server + // 20 + TraceRaftVoteRequestInitiation, // node ID of the server + TraceRaftPersistStateUpdateError, // negative error code + TraceRaftCommitIndexUpdate, // new commit index value + TraceRaftNewerTermInResponse, // new term value + TraceRaftNewEntryCommitted, // new commit index value + // 25 + TraceRaftAppendEntriesCallFailure, // error code (may be negated) + + NumTraceCodes +}; + +/** + * This interface allows the application to trace events that happen in the server. + */ +class IEventTracer +{ +public: +#if UAVCAN_TOSTRING + /** + * It is safe to call this function with any argument. + * If the event code is out of range, an assertion failure will be triggered and an error text will be returned. + */ + static const char* getEventName(TraceCode code) + { + // import re + // make_strings = lambda s: ',\n'.join('"%s"' % x for x in re.findall(r'\ \ \ \ Trace([A-Za-z0-9]+),', s)) + static const char* const Strings[NumTraceCodes] = + { + "Error", + "LogLastIndexRestored", + "LogAppend", + "LogRemove", + "CurrentTermRestored", + "CurrentTermUpdate", + "VotedForRestored", + "VotedForUpdate", + "DiscoveryBroadcast", + "NewServerDiscovered", + "DiscoveryReceived", + "ClusterSizeInited", + "InvalidClusterSizeReceived", + "RaftCoreInited", + "RaftStateSwitch", + "RaftActiveSwitch", + "RaftNewLogEntry", + "RaftRequestIgnored", + "RaftVoteRequestReceived", + "RaftVoteRequestSucceeded", + "RaftVoteRequestInitiation", + "RaftPersistStateUpdateError", + "RaftCommitIndexUpdate", + "RaftNewerTermInResponse", + "RaftNewEntryCommitted", + "RaftAppendEntriesCallFailure" + }; + uavcan::StaticAssert::check(); + UAVCAN_ASSERT(code < NumTraceCodes); + return (code < NumTraceCodes) ? Strings[static_cast(code)] : "INVALID_EVENT_CODE"; + } +#endif + + /** + * The server invokes this method every time it believes that a noteworthy event has happened. + * The table of event codes can be found in the server sources. + * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures + * full backward compatibility. + * @param event_code Event code, see the sources for the enum with values. + * @param event_argument Value associated with the event; its meaning depends on the event code. + */ + virtual void onEvent(TraceCode event_code, int64_t event_argument) = 0; + + virtual ~IEventTracer() { } +}; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp new file mode 100644 index 0000000000..8b1e1668e3 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -0,0 +1,318 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_LOG_HPP_INCLUDED + +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * Raft log. + * This class transparently replicates its state to the storage backend, keeping the most recent state in memory. + * Writes are slow, reads are instantaneous. + */ +class Log +{ +public: + typedef uint8_t Index; + + enum { Capacity = NodeID::Max + 1 }; + +private: + IStorageBackend& storage_; + IEventTracer& tracer_; + Entry entries_[Capacity]; + Index last_index_; // Index zero always contains an empty entry + + static IStorageBackend::String getLastIndexKey() { return "log_last_index"; } + + static IStorageBackend::String makeEntryKey(Index index, const char* postfix) + { + IStorageBackend::String str; + // "log0_foobar" + str += "log"; + str.appendFormatted("%d", int(index)); + str += "_"; + str += postfix; + return str; + } + + int readEntryFromStorage(Index index, Entry& out_entry) + { + const StorageMarshaller io(storage_); + + // Term + if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0) + { + return -ErrFailure; + } + + // 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) + { + return -ErrFailure; + } + out_entry.node_id = static_cast(node_id); + + return 0; + } + + int writeEntryToStorage(Index index, const Entry& entry) + { + Entry temp = entry; + + StorageMarshaller io(storage_); + + // Term + if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0) + { + return -ErrFailure; + } + + // Unique ID + if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = entry.node_id; + if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + temp.node_id = static_cast(node_id); + + return (temp == entry) ? 0 : -ErrFailure; + } + + int initEmptyLogStorage() + { + StorageMarshaller io(storage_); + + /* + * Writing the zero entry - it must always be default-initialized + */ + entries_[0] = Entry(); + int res = writeEntryToStorage(0, entries_[0]); + if (res < 0) + { + return res; + } + + /* + * Initializing last index + * Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be + * left in an inconsistent state. + */ + last_index_ = 0; + uint32_t stored_index = 0; + res = io.setAndGetBack(getLastIndexKey(), stored_index); + if (res < 0) + { + return res; + } + if (stored_index != 0) + { + return -ErrFailure; + } + + return 0; + } + +public: + Log(IStorageBackend& storage, IEventTracer& tracer) + : storage_(storage) + , tracer_(tracer) + , 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_); + + // Reading max index + { + uint32_t value = 0; + if (io.get(getLastIndexKey(), value) < 0) + { + if (storage_.get(getLastIndexKey()).empty()) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Initializing empty storage"); + return initEmptyLogStorage(); + } + else + { + // There's some data in the storage, but it cannot be parsed - reporting an error + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read last index"); + return -ErrFailure; + } + } + if (value >= Capacity) + { + return -ErrFailure; + } + last_index_ = Index(value); + } + + tracer_.onEvent(TraceLogLastIndexRestored, last_index_); + + // Restoring log entries - note that index 0 always exists + for (Index index = 0; index <= last_index_; index++) + { + const int result = readEntryFromStorage(index, entries_[index]); + if (result < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read entry at index %u: %d", + unsigned(index), result); + return result; + } + } + + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Restored %u log entries", unsigned(last_index_)); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the entry was successfully appended. + */ + int append(const Entry& entry) + { + if ((last_index_ + 1) >= Capacity) + { + return -ErrLogic; + } + + tracer_.onEvent(TraceLogAppend, last_index_ + 1U); + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + int res = writeEntryToStorage(Index(last_index_ + 1), entry); + if (res < 0) + { + return res; + } + + // Updating the last index + StorageMarshaller io(storage_); + uint32_t new_last_index = last_index_ + 1U; + res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != last_index_ + 1U) + { + return -ErrFailure; + } + entries_[new_last_index] = entry; + last_index_ = Index(new_last_index); + + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "New entry, index %u, node ID %u, term %u", + unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term)); + return 0; + } + + /** + * This method invokes storage IO. + * Returned value indicates whether the requested operation has been carried out successfully. + */ + int removeEntriesWhereIndexGreaterOrEqual(Index index) + { + UAVCAN_ASSERT(last_index_ < Capacity); + + if (((index) >= Capacity) || (index <= 0)) + { + return -ErrLogic; + } + + uint32_t new_last_index = index - 1U; + + tracer_.onEvent(TraceLogRemove, new_last_index); + + if (new_last_index != last_index_) + { + StorageMarshaller io(storage_); + int res = io.setAndGetBack(getLastIndexKey(), new_last_index); + if (res < 0) + { + return res; + } + if (new_last_index != index - 1U) + { + return -ErrFailure; + } + UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Entries removed, last index %u --> %u", + unsigned(last_index_), unsigned(new_last_index)); + last_index_ = Index(new_last_index); + } + + // Removal operation leaves dangling entries in storage, it's OK + return 0; + } + + int removeEntriesWhereIndexGreater(Index index) + { + return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U)); + } + + /** + * Returns nullptr if there's no such index. + * This method does not use storage IO. + */ + const Entry* getEntryAtIndex(Index index) const + { + UAVCAN_ASSERT(last_index_ < Capacity); + return (index <= last_index_) ? &entries_[index] : NULL; + } + + Index getLastIndex() const { return last_index_; } + + bool isOtherLogUpToDate(Index other_last_index, Term other_last_term) const + { + UAVCAN_ASSERT(last_index_ < Capacity); + // Terms are different - the one with higher term is more up-to-date + if (other_last_term != entries_[last_index_].term) + { + return other_last_term > entries_[last_index_].term; + } + // Terms are equal - longer log wins + return other_last_index >= last_index_; + } +}; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp new file mode 100644 index 0000000000..33eb4673cd --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -0,0 +1,225 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_PERSISTENT_STATE_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class is a convenient container for persistent state variables defined by Raft. + * Writes are slow, reads are instantaneous. + */ +class PersistentState +{ + IStorageBackend& storage_; + IEventTracer& tracer_; + + Term current_term_; + NodeID voted_for_; + Log log_; + + static IStorageBackend::String getCurrentTermKey() { return "current_term"; } + static IStorageBackend::String getVotedForKey() { return "voted_for"; } + +public: + PersistentState(IStorageBackend& storage, IEventTracer& tracer) + : storage_(storage) + , tracer_(tracer) + , current_term_(0) + , log_(storage, tracer) + { } + + int init() + { + /* + * Reading log + */ + int res = log_.init(); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Log init failed: %d", res); + return res; + } + + const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); + if (last_entry == NULL) + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + + const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0); + + StorageMarshaller io(storage_); + + /* + * Reading currentTerm + */ + if (storage_.get(getCurrentTermKey()).empty() && log_is_empty) + { + // First initialization + current_term_ = 0; + res = io.setAndGetBack(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init current term: %d", res); + return res; + } + if (current_term_ != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + res = io.get(getCurrentTermKey(), current_term_); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read current term: %d", res); + return res; + } + } + + tracer_.onEvent(TraceCurrentTermRestored, current_term_); + + if (current_term_ < last_entry->term) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", + "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)", + unsigned(current_term_), unsigned(last_entry->term)); + return -ErrLogic; + } + + /* + * Reading votedFor + */ + if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0)) + { + // First initialization + voted_for_ = NodeID(0); + uint32_t stored_voted_for = 0; + res = io.setAndGetBack(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init votedFor: %d", res); + return res; + } + if (stored_voted_for != 0) + { + return -ErrFailure; + } + } + else + { + // Restoring + uint32_t stored_voted_for = 0; + res = io.get(getVotedForKey(), stored_voted_for); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read votedFor: %d", res); + return res; + } + if (stored_voted_for > NodeID::Max) + { + return -ErrFailure; + } + voted_for_ = NodeID(uint8_t(stored_voted_for)); + } + + tracer_.onEvent(TraceVotedForRestored, voted_for_.get()); + + return 0; + } + + Term getCurrentTerm() const { return current_term_; } + + NodeID getVotedFor() const { return voted_for_; } + bool isVotedForSet() const { return voted_for_.isUnicast(); } + + Log& getLog() { return log_; } + const Log& getLog() const { return log_; } + + /** + * Invokes storage IO. + */ + int setCurrentTerm(Term term) + { + if (term < current_term_) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + tracer_.onEvent(TraceCurrentTermUpdate, term); + + StorageMarshaller io(storage_); + + Term tmp = term; + int res = io.setAndGetBack(getCurrentTermKey(), tmp); + if (res < 0) + { + return res; + } + + if (tmp != term) + { + return -ErrFailure; + } + + current_term_ = term; + return 0; + } + + /** + * Invokes storage IO. + */ + int setVotedFor(NodeID node_id) + { + if (!node_id.isValid()) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + tracer_.onEvent(TraceVotedForUpdate, node_id.get()); + + StorageMarshaller io(storage_); + + uint32_t tmp = node_id.get(); + int res = io.setAndGetBack(getVotedForKey(), tmp); + if (res < 0) + { + return res; + } + + if (node_id.get() != tmp) + { + return -ErrFailure; + } + + voted_for_ = node_id; + return 0; + } + + int resetVotedFor() { return setVotedFor(NodeID(0)); } +}; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp new file mode 100644 index 0000000000..87fa73dd65 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -0,0 +1,860 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#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 +#include +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log. + */ +class ILeaderLogCommitHandler +{ +public: + /** + * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). + */ + virtual void onEntryCommitted(const Entry& entry) = 0; + + /** + * Assume false by default. + */ + virtual void onLeaderChange(bool local_node_is_leader) = 0; + + virtual ~ILeaderLogCommitHandler() { } +}; + +/** + * This class implements log replication and voting. + * It does not implement client-server interaction at all; instead it just exposes a public method for adding + * allocation entries. + */ +class RaftCore : private TimerBase +{ + typedef MethodBinder&, + ServiceResponseDataStructure&)> + AppendEntriesCallback; + + typedef MethodBinder&)> + AppendEntriesResponseCallback; + + typedef MethodBinder&, + ServiceResponseDataStructure&)> + RequestVoteCallback; + + typedef MethodBinder&)> + RequestVoteResponseCallback; + + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + + struct PendingAppendEntriesFields + { + Log::Index prev_log_index; + Log::Index num_entries; + + PendingAppendEntriesFields() + : prev_log_index(0) + , num_entries(0) + { } + }; + + /* + * Constants + */ + const MonotonicDuration update_interval_; ///< AE requests will be issued at this rate + const MonotonicDuration base_activity_timeout_; + + IEventTracer& tracer_; + ILeaderLogCommitHandler& log_commit_handler_; + + /* + * States + */ + PersistentState persistent_state_; + ClusterManager cluster_; + Log::Index commit_index_; + + MonotonicTime last_activity_timestamp_; + bool active_mode_; + ServerState server_state_; + + uint8_t next_server_index_; ///< Next server to query AE from + uint8_t num_votes_received_in_this_campaign_; + + PendingAppendEntriesFields pending_append_entries_fields_; + + /* + * Transport + */ + ServiceServer append_entries_srv_; + ServiceClient append_entries_client_; + ServiceServer request_vote_srv_; + + enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 }; + LazyConstructor > + request_vote_clients_[NumRequestVoteClients]; + + /* + * Methods + */ + void trace(TraceCode event, int64_t argument) { tracer_.onEvent(event, argument); } + + INode& getNode() { return append_entries_srv_.getNode(); } + const INode& getNode() const { return append_entries_srv_.getNode(); } + + void registerActivity() { last_activity_timestamp_ = getNode().getMonotonicTime(); } + + bool isActivityTimedOut() const + { + const int multiplier = static_cast(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); + } + + void handlePersistentStateUpdateError(int error) + { + UAVCAN_ASSERT(error < 0); + trace(TraceRaftPersistStateUpdateError, error); + switchState(ServerStateFollower); + setActiveMode(false); // Goodnight sweet prince + registerActivity(); // Deferring reelections + } + + void updateFollower() + { + if (active_mode_ && isActivityTimedOut()) + { + switchState(ServerStateCandidate); + registerActivity(); + } + } + + void updateCandidate() + { + UAVCAN_ASSERT(active_mode_); + + if (num_votes_received_in_this_campaign_ > 0) + { + const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); + + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Election complete, won: %d", int(won)); + + switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader + } + else + { + // Set votedFor, abort on failure + int res = persistent_state_.setVotedFor(getNode().getNodeID()); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + // Increment current term, abort on failure + res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U); + if (res < 0) + { + handlePersistentStateUpdateError(res); + return; + } + + num_votes_received_in_this_campaign_ = 1; // Voting for self + + RequestVote::Request req; + req.last_log_index = persistent_state_.getLog().getLastIndex(); + 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++) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + if (!node_id.isUnicast()) + { + break; + } + + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Requesting vote from %d", int(node_id.get())); + trace(TraceRaftVoteRequestInitiation, node_id.get()); + + res = request_vote_clients_[i]->call(node_id, req); + if (res < 0) + { + trace(TraceError, res); + } + } + } + } + + void updateLeader() + { + if (cluster_.getClusterSize() == 1) + { + setActiveMode(false); // Haha + } + + if (active_mode_ || (next_server_index_ > 0)) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); + UAVCAN_ASSERT(node_id.isUnicast()); + + next_server_index_++; + if (next_server_index_ >= cluster_.getNumKnownServers()) + { + next_server_index_ = 0; + } + + AppendEntries::Request req; + req.term = persistent_state_.getCurrentTerm(); + req.leader_commit = commit_index_; + + req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); + + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); + if (entry == NULL) + { + UAVCAN_ASSERT(0); + handlePersistentStateUpdateError(-ErrLogic); + return; + } + + req.prev_log_term = entry->term; + + for (Log::Index index = cluster_.getServerNextIndex(node_id); + index <= persistent_state_.getLog().getLastIndex(); + index++) + { + req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); + if (req.entries.size() == req.entries.capacity()) + { + break; + } + } + + pending_append_entries_fields_.num_entries = req.entries.size(); + pending_append_entries_fields_.prev_log_index = req.prev_log_index; + + const int res = append_entries_client_.call(node_id, req); + if (res < 0) + { + trace(TraceRaftAppendEntriesCallFailure, res); + } + } + + propagateCommitIndex(); + } + + void switchState(ServerState new_state) + { + if (server_state_ != new_state) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "State switch: %d --> %d", + int(server_state_), int(new_state)); + trace(TraceRaftStateSwitch, new_state); + + if ((ServerStateLeader == server_state_) || + (ServerStateLeader == new_state)) + { + log_commit_handler_.onLeaderChange(ServerStateLeader == new_state); + } + + server_state_ = new_state; + + cluster_.resetAllServerIndices(); + + 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(); + } + } + + void setActiveMode(bool new_active) + { + if (active_mode_ != new_active) + { + UAVCAN_TRACE("dynamic_node_id_server_impl::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); + const int res = persistent_state_.setCurrentTerm(new_term); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + registerActivity(); // Deferring future elections + switchState(ServerStateFollower); + setActiveMode(false); + } + + void propagateCommitIndex() + { + // Objective is to estimate whether we can safely increment commit index value + UAVCAN_ASSERT(server_state_ == ServerStateLeader); + UAVCAN_ASSERT(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); + + uint8_t num_nodes_with_next_log_entry_available = 1; // Local node + for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) + { + const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); + if (match_index > commit_index_) + { + num_nodes_with_next_log_entry_available++; + } + } + + if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize()) + { + commit_index_++; + UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed + trace(TraceRaftNewEntryCommitted, commit_index_); + + // AT THIS POINT ALLOCATION IS COMPLETE + log_commit_handler_.onEntryCommitted(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); + } + } + } + + void handleAppendEntriesRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + return; + } + + registerActivity(); + + UAVCAN_ASSERT(response.isResponseEnabled()); // This is default + + /* + * 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()) + { + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + switchState(ServerStateFollower); + setActiveMode(false); + + if (!response.isResponseEnabled()) + { + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + response.success = false; + + /* + * Step 1 (see Raft paper) + * Reject the request if the leader has stale term number. + */ + if (request.term < persistent_state_.getCurrentTerm()) + { + response.setResponseEnabled(true); + return; + } + + switchState(ServerStateFollower); + setActiveMode(false); + + /* + * Step 2 + * Reject the request if the assumed log index does not exist on the local node. + */ + const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index); + if (prev_entry == NULL) + { + response.setResponseEnabled(true); + return; + } + + /* + * Step 3 + * Drop log entries if term number does not match. + * Ignore the request if the persistent state cannot be updated. + */ + if (prev_entry->term != request.prev_log_term) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index); + response.setResponseEnabled(res >= 0); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + } + return; + } + + /* + * Step 4 + * Update the log with new entries - this will possibly require to rewrite existing entries. + * Ignore the request if the persistent state cannot be updated. + */ + if (request.prev_log_index != persistent_state_.getLog().getLastIndex()) + { + const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + + for (uint8_t i = 0; i < request.entries.size(); i++) + { + const int res = persistent_state_.getLog().append(request.entries[i]); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; // Response will not be sent, the server will assume that we're dead + } + } + + /* + * Step 5 + * Update the commit index. + */ + if (request.leader_commit > commit_index_) + { + commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex()); + trace(TraceRaftCommitIndexUpdate, commit_index_); + } + + response.setResponseEnabled(true); + response.success = true; + } + + void handleAppendEntriesResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + + if (!result.isSuccessful()) + { + return; + } + + if (result.response.term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.response.term); + } + else + { + if (result.response.success) + { + cluster_.incrementServerNextIndexBy(result.server_node_id, pending_append_entries_fields_.num_entries); + cluster_.setServerMatchIndex(result.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); + } + } + + pending_append_entries_fields_ = PendingAppendEntriesFields(); + // Rest of the logic is implemented in periodic update handlers. + } + + void handleRequestVoteRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + return; + } + + 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()) + { + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + response.setResponseEnabled(false); + trace(TraceRaftPersistStateUpdateError, res); + } + + switchState(ServerStateFollower); + + if (!response.isResponseEnabled()) + { + return; + } + } + + /* + * Preparing the response + */ + response.term = persistent_state_.getCurrentTerm(); + + if (request.term < response.term) + { + response.vote_granted = false; + } + else + { + const bool can_vote = !persistent_state_.isVotedForSet() || + (persistent_state_.getVotedFor() == request.getSrcNodeID()); + const bool log_is_up_to_date = + persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term); + + response.vote_granted = can_vote && log_is_up_to_date; + + if (response.vote_granted) + { + registerActivity(); // This is necessary to avoid excessive elections + + const int res = persistent_state_.setVotedFor(request.getSrcNodeID()); + if (res < 0) + { + trace(TraceRaftPersistStateUpdateError, res); + response.setResponseEnabled(false); + return; + } + } + } + } + + void handleRequestVoteResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled + + if (!result.isSuccessful()) + { + return; + } + + trace(TraceRaftVoteRequestSucceeded, result.server_node_id.get()); + + if (result.response.term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.response.term); + } + else + { + if (result.response.vote_granted) + { + num_votes_received_in_this_campaign_++; + } + } + // Rest of the logic is implemented in periodic update handlers. + // I'm no fan of asynchronous programming. At all. + } + + virtual void handleTimerEvent(const TimerEvent&) + { + if (cluster_.hadDiscoveryActivity() && isLeader()) + { + setActiveMode(true); + } + + switch (server_state_) + { + case ServerStateFollower: + { + updateFollower(); + break; + } + case ServerStateCandidate: + { + updateCandidate(); + break; + } + case ServerStateLeader: + { + updateLeader(); + break; + } + default: + { + UAVCAN_ASSERT(0); + break; + } + } + } + +public: + RaftCore(INode& node, + IStorageBackend& storage, + IEventTracer& tracer, + ILeaderLogCommitHandler& log_commit_handler, + MonotonicDuration update_interval = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS), + MonotonicDuration base_activity_timeout = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_BASE_ELECTION_TIMEOUT_MS)) + : TimerBase(node) + , update_interval_(update_interval) + , base_activity_timeout_(base_activity_timeout) + , tracer_(tracer) + , log_commit_handler_(log_commit_handler) + , 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) + , 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(node); + } + } + + /** + * Once started, the logic runs in the background until destructor is called. + * @param cluster_size If set, this value will be used and stored in the persistent storage. If not set, + * value from the persistent storage will be used. If not set and there's no such key + * in the persistent storage, initialization will fail. + */ + int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) + { + /* + * 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; + + /* + * Initializing internals + */ + int res = persistent_state_.init(); + if (res < 0) + { + return res; + } + + res = cluster_.init(cluster_size); + if (res < 0) + { + return res; + } + + res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest)); + if (res < 0) + { + return res; + } + + res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest)); + if (res < 0) + { + return res; + } + + res = append_entries_client_.init(); + if (res < 0) + { + return res; + } + 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_clients_[i]->init(); + if (res < 0) + { + return res; + } + request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); + request_vote_clients_[i]->setRequestTimeout(update_interval_); + } + + startPeriodic(update_interval_); + + trace(TraceRaftCoreInited, update_interval_.toUSec()); + + UAVCAN_ASSERT(res >= 0); + return 0; + } + + /** + * This function is mostly needed for testing. + */ + Log::Index getCommitIndex() const { return commit_index_; } + + /** + * Only the leader can call @ref appendLog(). + */ + bool isLeader() const { return server_state_ == ServerStateLeader; } + + /** + * Inserts one entry into log. + * Failures are tolerble because all operations are idempotent. + * This method will trigger an assertion failure and return error if the current node is not the leader. + */ + int appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) + { + if (isLeader()) + { + Entry entry; + entry.node_id = node_id.get(); + entry.unique_id = unique_id; + entry.term = persistent_state_.getCurrentTerm(); + + trace(TraceRaftNewLogEntry, entry.node_id); + return persistent_state_.getLog().append(entry); + } + else + { + UAVCAN_ASSERT(0); + return -ErrLogic; + } + } + + /** + * This class is used to perform log searches. + */ + struct LogEntryInfo + { + Entry entry; + bool committed; + + LogEntryInfo(const Entry& arg_entry, bool arg_committed) + : entry(arg_entry) + , committed(arg_committed) + { } + }; + + /** + * This method is used by the allocator to query existence of certain entries in the Raft log. + * Predicate is a callable of the following prototype: + * bool (const LogEntryInfo& entry) + * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy + * contructor to the last visited entry; otherwise the constructor will not be initialized. In this case, lazy + * constructor is used as boost::optional. + * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. + */ + template + inline LazyConstructor traverseLogFromEndUntil(const Predicate& predicate) const + { + UAVCAN_ASSERT(try_implicit_cast(predicate, true)); + for (int index = static_cast(persistent_state_.getLog().getLastIndex()); index >= 0; index--) + { + const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(Log::Index(index)); + UAVCAN_ASSERT(entry != NULL); + const LogEntryInfo info(*entry, Log::Index(index) <= commit_index_); + if (predicate(info)) + { + LazyConstructor ret; + ret.template construct(info); + return ret; + } + } + return LazyConstructor(); + } +}; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp new file mode 100644 index 0000000000..24b4b99cff --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -0,0 +1,201 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class implements the top-level allocation logic and server API. + */ +class Server : private IAllocationRequestHandler + , private ILeaderLogCommitHandler +{ + struct UniqueIDLogPredicate + { + const UniqueID unique_id; + + UniqueIDLogPredicate(const UniqueID& uid) + : unique_id(uid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.unique_id == unique_id; + } + }; + + struct NodeIDLogPredicate + { + const NodeID node_id; + + NodeIDLogPredicate(const NodeID& nid) + : node_id(nid) + { } + + bool operator()(const RaftCore::LogEntryInfo& info) const + { + return info.entry.node_id == node_id.get(); + } + }; + + /* + * States + */ + INode& node_; + RaftCore raft_core_; + AllocationRequestManager allocation_request_manager_; + + /* + * Methods + */ + bool isNodeIDTaken(const NodeID node_id) const + { + UAVCAN_TRACE("DynamicNodeIDServer", "Testing if node ID %d is taken", int(node_id.get())); + return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); + } + + NodeID findFreeNodeID(const NodeID preferred_node_id) const + { + uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + + // Up + while (candidate <= NodeID::Max) + { + if (!isNodeIDTaken(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + candidate--; // This has been tested already + + // Down + while (candidate > 0) + { + if (!isNodeIDTaken(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); + } + + void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) + { + const NodeID allocated_node_id = findFreeNodeID(preferred_node_id); + if (!allocated_node_id.isUnicast()) + { + UAVCAN_TRACE("DynamicNodeIDServer", "Request ignored - no free node ID left"); + return; + } + + UAVCAN_TRACE("DynamicNodeIDServer", "New node ID allocated: %d", int(allocated_node_id.get())); + const int res = raft_core_.appendLog(unique_id, allocated_node_id); + if (res < 0) + { + node_.registerInternalFailure("Raft log append"); + } + } + + virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) + { + // TODO: allocation requests must not be served if the list of unidentified nodes is not empty + + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id)); + + if (result.isConstructed()) + { + if (result->committed) + { + tryPublishAllocationResult(result->entry); + UAVCAN_TRACE("DynamicNodeIDServer", + "Allocation request served with existing allocation; node ID %d", + int(result->entry.node_id)); + } + else + { + UAVCAN_TRACE("DynamicNodeIDServer", + "Allocation request ignored - allocation exists but not committed yet; node ID %d", + int(result->entry.node_id)); + } + } + else + { + allocateNewNode(unique_id, preferred_node_id); + } + } + + virtual void onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry) + { + tryPublishAllocationResult(entry); + } + + virtual void onLeaderChange(bool local_node_is_leader) + { + UAVCAN_TRACE("DynamicNodeIDServer", "I am leader: %d", int(local_node_is_leader)); + allocation_request_manager_.setActive(local_node_is_leader); + } + + void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) + { + const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); + if (res < 0) + { + node_.registerInternalFailure("Dynamic allocation final broadcast"); + } + } + +public: + Server(INode& node, + IStorageBackend& storage, + IEventTracer& tracer) + : node_(node) + , raft_core_(node, storage, tracer, *this) + , allocation_request_manager_(node, *this) + { } + + int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) + { + int res = raft_core_.init(cluster_size); + if (res < 0) + { + return res; + } + + res = allocation_request_manager_.init(); + if (res < 0) + { + return res; + } + + // TODO Initialize the node info transport + + return 0; + } +}; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp new file mode 100644 index 0000000000..ed3ba2f873 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/types.hpp @@ -0,0 +1,29 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_TYPES_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ + +using namespace ::uavcan::protocol::dynamic_node_id::server; + +/** + * Raft term + */ +typedef StorageType::Type Term; + +} +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp new file mode 100644 index 0000000000..efc3459a68 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp @@ -0,0 +1,62 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_BACKEND_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * This interface is used by the server to read and write stable storage. + * The storage is represented as a key-value container, where keys and values are ASCII strings up to 32 + * characters long, not including the termination byte. Fixed block size allows for absolutely straightforward + * 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 +{ +public: + /** + * Maximum length of keys and values. One pair takes twice as much space. + */ + enum { MaxStringLength = 32 }; + + /** + * It is guaranteed that the server will never require more than this number of key/value pairs. + * Total storage space needed is (MaxKeyValuePairs * MaxStringLength * 2), not including storage overhead. + */ + enum { MaxKeyValuePairs = 512 }; + + /** + * 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, ArrayModeDynamic, MaxStringLength> String; + + /** + * Read one value from the storage. + * If such key does not exist, or if read failed, an empty string will be returned. + * This method should not block for more than 50 ms. + */ + virtual String get(const String& key) const = 0; + + /** + * Create or update value for the given key. Empty value should be regarded as a request to delete the key. + * This method should not block for more than 50 ms. + * Failures will be ignored. + */ + virtual void set(const String& key, const String& value) = 0; + + virtual ~IStorageBackend() { } +}; + +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp new file mode 100644 index 0000000000..26c937db50 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp @@ -0,0 +1,181 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_STORAGE_MARSHALLER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_STORAGE_MARSHALLER_HPP_INCLUDED + +#include +#include +#include +#include +#include + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 +# include +#endif + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * This class extends the storage backend interface with serialization/deserialization functionality. + */ +class StorageMarshaller +{ + IStorageBackend& storage_; + + static uint8_t convertLowerCaseHexCharToNibble(char ch) + { + const uint8_t ret = (ch > '9') ? static_cast(ch - 'a' + 10) : static_cast(ch - '0'); + UAVCAN_ASSERT(ret < 16); + return ret; + } + +public: + StorageMarshaller(IStorageBackend& storage) + : storage_(storage) + { } + + /** + * These methods set the value and then immediately read it back. + * 1. Serialize the value. + * 2. Update the value on the backend. + * 3. Call get() with the same value argument. + * The caller then is supposed to check whether the argument has the desired value. + */ + int setAndGetBack(const IStorageBackend::String& key, uint32_t& inout_value) + { + IStorageBackend::String serialized; + serialized.appendFormatted("%llu", static_cast(inout_value)); + + UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); + } + + int setAndGetBack(const IStorageBackend::String& key, UniqueID& inout_value) + { + IStorageBackend::String serialized; + for (uint8_t i = 0; i < UniqueID::MaxSize; i++) + { + serialized.appendFormatted("%02x", inout_value.at(i)); + } + UAVCAN_ASSERT(serialized.size() == UniqueID::MaxSize * 2); + + UAVCAN_TRACE("StorageMarshaller", "Set %s = %s", key.c_str(), serialized.c_str()); + storage_.set(key, serialized); + + return get(key, inout_value); + } + + /** + * Getters simply read and deserialize the value. + * 1. Read the value back from the backend; return false if read fails. + * 2. Deserealize the newly read value; return false if deserialization fails. + * 3. Update the argument with deserialized value. + * 4. Return true. + */ + int get(const IStorageBackend::String& key, uint32_t& out_value) const + { + /* + * Reading the storage + */ + const IStorageBackend::String val = storage_.get(key); + if (val.empty()) + { + return -ErrFailure; + } + + /* + * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. + * The value must contain only numeric characters. + */ + for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if (static_cast(*it) < '0' || static_cast(*it) > '9') + { + return -ErrFailure; + } + } + + if (val.size() > 10) // len(str(0xFFFFFFFF)) + { + return -ErrFailure; + } + + /* + * Conversion is carried out here + */ +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + errno = 0; +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + const unsigned long long x = std::strtoull(val.c_str(), NULL, 10); +#else + // There was no strtoull() before C++11, so we need to resort to strtoul() + StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check(); + const unsigned long x = std::strtoul(val.c_str(), NULL, 10); +#endif + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + if (errno != 0) + { + return -ErrFailure; + } +#endif + + out_value = static_cast(x); + return 0; + } + + int get(const IStorageBackend::String& key, UniqueID& out_value) const + { + static const uint8_t NumBytes = UniqueID::MaxSize; + + /* + * Reading the storage + */ + IStorageBackend::String val = storage_.get(key); + if (val.size() != NumBytes * 2) + { + return -ErrFailure; + } + + /* + * The value must contain only hexadecimal numbers. + */ + val.convertToLowerCaseASCII(); + for (IStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) + { + if ((static_cast(*it) < '0' || static_cast(*it) > '9') && + (static_cast(*it) < 'a' || static_cast(*it) > 'f')) + { + return -ErrFailure; + } + } + + /* + * Conversion is carried out here + */ + IStorageBackend::String::const_iterator it = val.begin(); + + for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++) + { + out_value[byte_index] = + static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) << 4); + out_value[byte_index] = + static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) | out_value[byte_index]); + } + + return 0; + } +}; + +} +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp new file mode 100644 index 0000000000..8ae2b6e9f6 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp @@ -0,0 +1,27 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_TYPES_HPP_INCLUDED + +#include +// UAVCAN types +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +using namespace ::uavcan::protocol::dynamic_node_id; + +/** + * Node Unique ID + */ +typedef protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id UniqueID; + +} +} + +#endif // Include guard diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp deleted file mode 100644 index 16eb6b7d70..0000000000 --- a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_server.cpp +++ /dev/null @@ -1,1893 +0,0 @@ -/* - * Copyright (C) 2015 Pavel Kirienko - */ - -#include -#include - -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 -# include -#endif - -#ifndef UAVCAN_CPP_VERSION -# error UAVCAN_CPP_VERSION -#endif - -namespace uavcan -{ -/* - * IDynamicNodeIDAllocationServerEventTracer - */ -#if UAVCAN_TOSTRING -const char* IDynamicNodeIDAllocationServerEventTracer::getEventName(uint16_t code) -{ - using namespace dynamic_node_id_server_impl; - - // import re - // make_strings = lambda s: ',\n'.join('"%s"' % x for x in re.findall(r'\ \ \ \ Trace([A-Za-z0-9]+),', s)) - static const char* const Strings[NumTraceEventCodes] = - { - "Error", - "LogLastIndexRestored", - "LogAppend", - "LogRemove", - "CurrentTermRestored", - "CurrentTermUpdate", - "VotedForRestored", - "VotedForUpdate", - "DiscoveryBroadcast", - "NewServerDiscovered", - "DiscoveryReceived", - "ClusterSizeInited", - "InvalidClusterSizeReceived", - "RaftCoreInited", - "RaftStateSwitch", - "RaftActiveSwitch", - "RaftNewLogEntry", - "RaftRequestIgnored", - "RaftVoteRequestReceived", - "RaftVoteRequestSucceeded", - "RaftVoteRequestInitiation", - "RaftPersistStateUpdateError", - "RaftCommitIndexUpdate", - "RaftNewerTermInResponse", - "RaftNewEntryCommitted", - "RaftAppendEntriesCallFailure" - }; - uavcan::StaticAssert::check(); - UAVCAN_ASSERT(code < NumTraceEventCodes); - return (code < NumTraceEventCodes) ? Strings[code] : "INVALID_EVENT_CODE"; -} -#endif - -namespace dynamic_node_id_server_impl -{ -/* - * MarshallingStorageDecorator - */ -uint8_t MarshallingStorageDecorator::convertLowerCaseHexCharToNibble(char ch) -{ - const uint8_t ret = (ch > '9') ? static_cast(ch - 'a' + 10) : static_cast(ch - '0'); - UAVCAN_ASSERT(ret < 16); - return ret; -} - -int MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, uint32_t& inout_value) -{ - IDynamicNodeIDStorageBackend::String serialized; - serialized.appendFormatted("%llu", static_cast(inout_value)); - - UAVCAN_TRACE("MarshallingStorageDecorator", "Set %s = %s", key.c_str(), serialized.c_str()); - storage_.set(key, serialized); - - return get(key, inout_value); -} - -int MarshallingStorageDecorator::setAndGetBack(const IDynamicNodeIDStorageBackend::String& key, - Entry::FieldTypes::unique_id& inout_value) -{ - IDynamicNodeIDStorageBackend::String serialized; - for (uint8_t i = 0; i < Entry::FieldTypes::unique_id::MaxSize; i++) - { - serialized.appendFormatted("%02x", inout_value.at(i)); - } - UAVCAN_ASSERT(serialized.size() == Entry::FieldTypes::unique_id::MaxSize * 2); - - UAVCAN_TRACE("MarshallingStorageDecorator", "Set %s = %s", key.c_str(), serialized.c_str()); - storage_.set(key, serialized); - - return get(key, inout_value); -} - -int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, uint32_t& out_value) const -{ - /* - * Reading the storage - */ - const IDynamicNodeIDStorageBackend::String val = storage_.get(key); - if (val.empty()) - { - return -ErrFailure; - } - - /* - * Per MISRA C++ recommendations, checking the inputs instead of relying solely on errno. - * The value must contain only numeric characters. - */ - for (IDynamicNodeIDStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) - { - if (static_cast(*it) < '0' || static_cast(*it) > '9') - { - return -ErrFailure; - } - } - - if (val.size() > 10) // len(str(0xFFFFFFFF)) - { - return -ErrFailure; - } - - /* - * Conversion is carried out here - */ -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - errno = 0; -#endif - -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - const unsigned long long x = std::strtoull(val.c_str(), NULL, 10); -#else - // There was no strtoull() before C++11, so we need to resort to strtoul() - StaticAssert<(sizeof(unsigned long) >= sizeof(uint32_t))>::check(); - const unsigned long x = std::strtoul(val.c_str(), NULL, 10); -#endif - -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - if (errno != 0) - { - return -ErrFailure; - } -#endif - - out_value = static_cast(x); - return 0; -} - -int MarshallingStorageDecorator::get(const IDynamicNodeIDStorageBackend::String& key, - Entry::FieldTypes::unique_id& out_value) const -{ - static const uint8_t NumBytes = Entry::FieldTypes::unique_id::MaxSize; - - /* - * Reading the storage - */ - IDynamicNodeIDStorageBackend::String val = storage_.get(key); - if (val.size() != NumBytes * 2) - { - return -ErrFailure; - } - - /* - * The value must contain only hexadecimal numbers. - */ - val.convertToLowerCaseASCII(); - for (IDynamicNodeIDStorageBackend::String::const_iterator it = val.begin(); it != val.end(); ++it) - { - if ((static_cast(*it) < '0' || static_cast(*it) > '9') && - (static_cast(*it) < 'a' || static_cast(*it) > 'f')) - { - return -ErrFailure; - } - } - - /* - * Conversion is carried out here - */ - IDynamicNodeIDStorageBackend::String::const_iterator it = val.begin(); - - for (uint8_t byte_index = 0; byte_index < NumBytes; byte_index++) - { - out_value[byte_index] = static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) << 4); - out_value[byte_index] = static_cast(convertLowerCaseHexCharToNibble(static_cast(*it++)) | - out_value[byte_index]); - } - - return 0; -} - -/* - * Log - */ -IDynamicNodeIDStorageBackend::String Log::makeEntryKey(Index index, const char* postfix) -{ - IDynamicNodeIDStorageBackend::String str; - // "log0_foobar" - str += "log"; - str.appendFormatted("%d", int(index)); - str += "_"; - str += postfix; - return str; -} - -int Log::readEntryFromStorage(Index index, Entry& out_entry) -{ - const MarshallingStorageDecorator io(storage_); - - // Term - if (io.get(makeEntryKey(index, "term"), out_entry.term) < 0) - { - return -ErrFailure; - } - - // 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) - { - return -ErrFailure; - } - out_entry.node_id = static_cast(node_id); - - return 0; -} - -int Log::writeEntryToStorage(Index index, const Entry& entry) -{ - Entry temp = entry; - - MarshallingStorageDecorator io(storage_); - - // Term - if (io.setAndGetBack(makeEntryKey(index, "term"), temp.term) < 0) - { - return -ErrFailure; - } - - // Unique ID - if (io.setAndGetBack(makeEntryKey(index, "unique_id"), temp.unique_id) < 0) - { - return -ErrFailure; - } - - // Node ID - uint32_t node_id = entry.node_id; - if (io.setAndGetBack(makeEntryKey(index, "node_id"), node_id) < 0) - { - return -ErrFailure; - } - temp.node_id = static_cast(node_id); - - return (temp == entry) ? 0 : -ErrFailure; -} - -int Log::initEmptyLogStorage() -{ - MarshallingStorageDecorator io(storage_); - - /* - * Writing the zero entry - it must always be default-initialized - */ - entries_[0] = Entry(); - int res = writeEntryToStorage(0, entries_[0]); - if (res < 0) - { - return res; - } - - /* - * Initializing last index - * Last index must be written AFTER the zero entry, otherwise if the write fails here the storage will be - * left in an inconsistent state. - */ - last_index_ = 0; - uint32_t stored_index = 0; - res = io.setAndGetBack(getLastIndexKey(), stored_index); - if (res < 0) - { - return res; - } - if (stored_index != 0) - { - return -ErrFailure; - } - - return 0; -} - -int Log::init() -{ - MarshallingStorageDecorator io(storage_); - - // Reading max index - { - uint32_t value = 0; - if (io.get(getLastIndexKey(), value) < 0) - { - if (storage_.get(getLastIndexKey()).empty()) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Initializing empty storage"); - return initEmptyLogStorage(); - } - else - { - // There's some data in the storage, but it cannot be parsed - reporting an error - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read last index"); - return -ErrFailure; - } - } - if (value >= Capacity) - { - return -ErrFailure; - } - last_index_ = Index(value); - } - - tracer_.onEvent(TraceLogLastIndexRestored, last_index_); - - // Restoring log entries - note that index 0 always exists - for (Index index = 0; index <= last_index_; index++) - { - const int result = readEntryFromStorage(index, entries_[index]); - if (result < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read entry at index %u: %d", - unsigned(index), result); - return result; - } - } - - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Restored %u log entries", unsigned(last_index_)); - return 0; -} - -int Log::append(const Entry& entry) -{ - if ((last_index_ + 1) >= Capacity) - { - return -ErrLogic; - } - - tracer_.onEvent(TraceLogAppend, last_index_ + 1U); - - // If next operations fail, we'll get a dangling entry, but it's absolutely OK. - int res = writeEntryToStorage(Index(last_index_ + 1), entry); - if (res < 0) - { - return res; - } - - // Updating the last index - MarshallingStorageDecorator io(storage_); - uint32_t new_last_index = last_index_ + 1U; - res = io.setAndGetBack(getLastIndexKey(), new_last_index); - if (res < 0) - { - return res; - } - if (new_last_index != last_index_ + 1U) - { - return -ErrFailure; - } - entries_[new_last_index] = entry; - last_index_ = Index(new_last_index); - - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "New entry, index %u, node ID %u, term %u", - unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term)); - return 0; -} - -int Log::removeEntriesWhereIndexGreaterOrEqual(Index index) -{ - UAVCAN_ASSERT(last_index_ < Capacity); - - if (((index) >= Capacity) || (index <= 0)) - { - return -ErrLogic; - } - - uint32_t new_last_index = index - 1U; - - tracer_.onEvent(TraceLogRemove, new_last_index); - - if (new_last_index != last_index_) - { - MarshallingStorageDecorator io(storage_); - int res = io.setAndGetBack(getLastIndexKey(), new_last_index); - if (res < 0) - { - return res; - } - if (new_last_index != index - 1U) - { - return -ErrFailure; - } - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Entries removed, last index %u --> %u", - unsigned(last_index_), unsigned(new_last_index)); - last_index_ = Index(new_last_index); - } - - // Removal operation leaves dangling entries in storage, it's OK - return 0; -} - -int Log::removeEntriesWhereIndexGreater(Index index) -{ - return removeEntriesWhereIndexGreaterOrEqual(Index(index + 1U)); -} - -const Entry* Log::getEntryAtIndex(Index index) const -{ - UAVCAN_ASSERT(last_index_ < Capacity); - return (index <= last_index_) ? &entries_[index] : NULL; -} - -bool Log::isOtherLogUpToDate(Log::Index other_last_index, Term other_last_term) const -{ - UAVCAN_ASSERT(last_index_ < Capacity); - // Terms are different - the one with higher term is more up-to-date - if (other_last_term != entries_[last_index_].term) - { - return other_last_term > entries_[last_index_].term; - } - // Terms are equal - longer log wins - return other_last_index >= last_index_; -} - -/* - * PersistentState - */ -int PersistentState::init() -{ - /* - * Reading log - */ - int res = log_.init(); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Log init failed: %d", res); - return res; - } - - const Entry* const last_entry = log_.getEntryAtIndex(log_.getLastIndex()); - if (last_entry == NULL) - { - UAVCAN_ASSERT(0); - return -ErrLogic; - } - - const bool log_is_empty = (log_.getLastIndex() == 0) && (last_entry->term == 0); - - MarshallingStorageDecorator io(storage_); - - /* - * Reading currentTerm - */ - if (storage_.get(getCurrentTermKey()).empty() && log_is_empty) - { - // First initialization - current_term_ = 0; - res = io.setAndGetBack(getCurrentTermKey(), current_term_); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init current term: %d", res); - return res; - } - if (current_term_ != 0) - { - return -ErrFailure; - } - } - else - { - // Restoring - res = io.get(getCurrentTermKey(), current_term_); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read current term: %d", res); - return res; - } - } - - tracer_.onEvent(TraceCurrentTermRestored, current_term_); - - if (current_term_ < last_entry->term) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", - "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)", - unsigned(current_term_), unsigned(last_entry->term)); - return -ErrLogic; - } - - /* - * Reading votedFor - */ - if (storage_.get(getVotedForKey()).empty() && log_is_empty && (current_term_ == 0)) - { - // First initialization - voted_for_ = NodeID(0); - uint32_t stored_voted_for = 0; - res = io.setAndGetBack(getVotedForKey(), stored_voted_for); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init votedFor: %d", res); - return res; - } - if (stored_voted_for != 0) - { - return -ErrFailure; - } - } - else - { - // Restoring - uint32_t stored_voted_for = 0; - res = io.get(getVotedForKey(), stored_voted_for); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read votedFor: %d", res); - return res; - } - if (stored_voted_for > NodeID::Max) - { - return -ErrFailure; - } - voted_for_ = NodeID(uint8_t(stored_voted_for)); - } - - tracer_.onEvent(TraceVotedForRestored, voted_for_.get()); - - return 0; -} - -int PersistentState::setCurrentTerm(const Term term) -{ - if (term < current_term_) - { - UAVCAN_ASSERT(0); - return -ErrInvalidParam; - } - - tracer_.onEvent(TraceCurrentTermUpdate, term); - - MarshallingStorageDecorator io(storage_); - - Term tmp = term; - int res = io.setAndGetBack(getCurrentTermKey(), tmp); - if (res < 0) - { - return res; - } - - if (tmp != term) - { - return -ErrFailure; - } - - current_term_ = term; - return 0; -} - -int PersistentState::setVotedFor(const NodeID node_id) -{ - if (!node_id.isValid()) - { - UAVCAN_ASSERT(0); - return -ErrInvalidParam; - } - - tracer_.onEvent(TraceVotedForUpdate, node_id.get()); - - MarshallingStorageDecorator io(storage_); - - uint32_t tmp = node_id.get(); - int res = io.setAndGetBack(getVotedForKey(), tmp); - if (res < 0) - { - return res; - } - - if (node_id.get() != tmp) - { - return -ErrFailure; - } - - voted_for_ = node_id; - return 0; -} - -/* - * ClusterManager - */ -void ClusterManager::Server::resetIndices(const Log& log) -{ - next_index = Log::Index(log.getLastIndex() + 1U); - match_index = 0; -} - -ClusterManager::Server* ClusterManager::findServer(NodeID node_id) -{ - for (uint8_t i = 0; i < num_known_servers_; i++) - { - UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - if (servers_[i].node_id == node_id) - { - return &servers_[i]; - } - } - return NULL; -} - -const ClusterManager::Server* ClusterManager::findServer(NodeID node_id) const -{ - return const_cast(this)->findServer(node_id); -} - -void ClusterManager::addServer(NodeID node_id) -{ - UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); - if (!isKnownServer(node_id) && node_id.isUnicast()) - { - tracer_.onEvent(TraceNewServerDiscovered, node_id.get()); - servers_[num_known_servers_].node_id = node_id; - servers_[num_known_servers_].resetIndices(log_); - num_known_servers_ = static_cast(num_known_servers_ + 1U); - } - else - { - UAVCAN_ASSERT(0); - } -} - -void ClusterManager::handleTimerEvent(const TimerEvent&) -{ - UAVCAN_ASSERT(num_known_servers_ < cluster_size_); - - tracer_.onEvent(TraceDiscoveryBroadcast, num_known_servers_); - - /* - * Filling the message - */ - Discovery msg; - msg.configured_cluster_size = cluster_size_; - - msg.known_nodes.push_back(getNode().getNodeID().get()); // Putting ourselves at index 0 - - for (uint8_t i = 0; i < num_known_servers_; i++) - { - UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - msg.known_nodes.push_back(servers_[i].node_id.get()); - } - - UAVCAN_ASSERT(msg.known_nodes.size() == (num_known_servers_ + 1)); - - /* - * Broadcasting - */ - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Broadcasting Discovery message; known nodes: %d of %d", - int(msg.known_nodes.size()), int(cluster_size_)); - - const int res = discovery_pub_.broadcast(msg); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcst failed: %d", res); - getNode().registerInternalFailure("Raft discovery broadcast"); - } - - /* - * Termination condition - */ - if (isClusterDiscovered()) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcasting timer stopped"); - stop(); - } -} - -void ClusterManager::handleDiscovery(const ReceivedDataStructure& msg) -{ - tracer_.onEvent(TraceDiscoveryReceived, msg.getSrcNodeID().get()); - - /* - * Validating cluster configuration - * If there's a case of misconfiguration, the message will be ignored. - */ - if (msg.configured_cluster_size != cluster_size_) - { - tracer_.onEvent(TraceInvalidClusterSizeReceived, msg.configured_cluster_size); - getNode().registerInternalFailure("Bad Raft cluster size"); - return; - } - - had_discovery_activity_ = true; - - /* - * Updating the set of known servers - */ - for (uint8_t i = 0; i < msg.known_nodes.size(); i++) - { - if (isClusterDiscovered()) - { - break; - } - - const NodeID node_id(msg.known_nodes[i]); - if (node_id.isUnicast() && !isKnownServer(node_id)) - { - addServer(node_id); - } - } - - /* - * Publishing a new Discovery request if the publishing server needs to learn about more servers. - */ - if (msg.configured_cluster_size > msg.known_nodes.size()) - { - startDiscoveryPublishingTimerIfNotRunning(); - } -} - -void ClusterManager::startDiscoveryPublishingTimerIfNotRunning() -{ - if (!isRunning()) - { - startPeriodic(MonotonicDuration::fromMSec(Discovery::BROADCASTING_INTERVAL_MS)); - } -} - -int ClusterManager::init(const uint8_t init_cluster_size) -{ - /* - * Figuring out the cluster size - */ - if (init_cluster_size == ClusterSizeUnknown) - { - // Reading from the storage - MarshallingStorageDecorator io(storage_); - uint32_t value = 0; - int res = io.get(getStorageKeyForClusterSize(), value); - if (res < 0) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", - "Cluster size is neither configured nor stored in the storage"); - return res; - } - if ((value == 0) || (value > MaxClusterSize)) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster size is invalid"); - return -ErrFailure; - } - cluster_size_ = static_cast(value); - } - else - { - if ((init_cluster_size == 0) || (init_cluster_size > MaxClusterSize)) - { - return -ErrInvalidParam; - } - cluster_size_ = init_cluster_size; - - // Writing the storage - MarshallingStorageDecorator io(storage_); - uint32_t value = init_cluster_size; - int res = io.setAndGetBack(getStorageKeyForClusterSize(), value); - if ((res < 0) || (value != init_cluster_size)) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Failed to store cluster size"); - return -ErrFailure; - } - } - - tracer_.onEvent(TraceClusterSizeInited, cluster_size_); - - UAVCAN_ASSERT(cluster_size_ > 0); - UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize); - - /* - * Initializing pub/sub and timer - */ - int res = discovery_pub_.init(); - if (res < 0) - { - return res; - } - (void)discovery_pub_.setPriority(TransferPriorityLow); - - res = discovery_sub_.start(DiscoveryCallback(this, &ClusterManager::handleDiscovery)); - if (res < 0) - { - return res; - } - - startDiscoveryPublishingTimerIfNotRunning(); - - /* - * Misc - */ - resetAllServerIndices(); - return 0; -} - -bool ClusterManager::isKnownServer(NodeID node_id) const -{ - if (node_id == getNode().getNodeID()) - { - return true; - } - for (uint8_t i = 0; i < num_known_servers_; i++) - { - UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - UAVCAN_ASSERT(servers_[i].node_id != getNode().getNodeID()); - if (servers_[i].node_id == node_id) - { - return true; - } - } - return false; -} - -NodeID ClusterManager::getRemoteServerNodeIDAtIndex(uint8_t index) const -{ - if (index < num_known_servers_) - { - return servers_[index].node_id; - } - return NodeID(); -} - -Log::Index ClusterManager::getServerNextIndex(NodeID server_node_id) const -{ - const Server* const s = findServer(server_node_id); - if (s != NULL) - { - return s->next_index; - } - UAVCAN_ASSERT(0); - return 0; -} - -void ClusterManager::incrementServerNextIndexBy(NodeID server_node_id, Log::Index increment) -{ - Server* const s = findServer(server_node_id); - if (s != NULL) - { - s->next_index = Log::Index(s->next_index + increment); - } - else - { - UAVCAN_ASSERT(0); - } -} - -void ClusterManager::decrementServerNextIndex(NodeID server_node_id) -{ - Server* const s = findServer(server_node_id); - if (s != NULL) - { - s->next_index--; - } - else - { - UAVCAN_ASSERT(0); - } -} - -Log::Index ClusterManager::getServerMatchIndex(NodeID server_node_id) const -{ - const Server* const s = findServer(server_node_id); - if (s != NULL) - { - return s->match_index; - } - UAVCAN_ASSERT(0); - return 0; -} - -void ClusterManager::setServerMatchIndex(NodeID server_node_id, Log::Index match_index) -{ - Server* const s = findServer(server_node_id); - if (s != NULL) - { - s->match_index = match_index; - } - else - { - UAVCAN_ASSERT(0); - } -} - -void ClusterManager::resetAllServerIndices() -{ - for (uint8_t i = 0; i < num_known_servers_; i++) - { - UAVCAN_ASSERT(servers_[i].node_id.isUnicast()); - servers_[i].resetIndices(log_); - } -} - -/* - * RaftCore - */ -bool RaftCore::isActivityTimedOut() const -{ - const int multiplier = static_cast(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); -} - -void RaftCore::handlePersistentStateUpdateError(int error) -{ - UAVCAN_ASSERT(error < 0); - trace(TraceRaftPersistStateUpdateError, error); - switchState(ServerStateFollower); - setActiveMode(false); // Goodnight sweet prince - registerActivity(); // Deferring reelections -} - -void RaftCore::updateFollower() -{ - if (active_mode_ && isActivityTimedOut()) - { - switchState(ServerStateCandidate); - registerActivity(); - } -} - -void RaftCore::updateCandidate() -{ - UAVCAN_ASSERT(active_mode_); - - if (num_votes_received_in_this_campaign_ > 0) - { - const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); - - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Election complete, won: %d", int(won)); - - switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader - } - else - { - // Set votedFor, abort on failure - int res = persistent_state_.setVotedFor(getNode().getNodeID()); - if (res < 0) - { - handlePersistentStateUpdateError(res); - return; - } - - // Increment current term, abort on failure - res = persistent_state_.setCurrentTerm(persistent_state_.getCurrentTerm() + 1U); - if (res < 0) - { - handlePersistentStateUpdateError(res); - return; - } - - num_votes_received_in_this_campaign_ = 1; // Voting for self - - RequestVote::Request req; - req.last_log_index = persistent_state_.getLog().getLastIndex(); - 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++) - { - const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); - if (!node_id.isUnicast()) - { - break; - } - - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Requesting vote from %d", int(node_id.get())); - trace(TraceRaftVoteRequestInitiation, node_id.get()); - - res = request_vote_clients_[i]->call(node_id, req); - if (res < 0) - { - trace(TraceError, res); - } - } - } -} - -void RaftCore::updateLeader() -{ - if (cluster_.getClusterSize() == 1) - { - setActiveMode(false); // Haha - } - - if (active_mode_ || (next_server_index_ > 0)) - { - const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); - UAVCAN_ASSERT(node_id.isUnicast()); - - next_server_index_++; - if (next_server_index_ >= cluster_.getNumKnownServers()) - { - next_server_index_ = 0; - } - - AppendEntries::Request req; - req.term = persistent_state_.getCurrentTerm(); - req.leader_commit = commit_index_; - - req.prev_log_index = Log::Index(cluster_.getServerNextIndex(node_id) - 1U); - - const Entry* const entry = persistent_state_.getLog().getEntryAtIndex(req.prev_log_index); - if (entry == NULL) - { - UAVCAN_ASSERT(0); - handlePersistentStateUpdateError(-ErrLogic); - return; - } - - req.prev_log_term = entry->term; - - for (Log::Index index = cluster_.getServerNextIndex(node_id); - index <= persistent_state_.getLog().getLastIndex(); - index++) - { - req.entries.push_back(*persistent_state_.getLog().getEntryAtIndex(index)); - if (req.entries.size() == req.entries.capacity()) - { - break; - } - } - - pending_append_entries_fields_.num_entries = req.entries.size(); - pending_append_entries_fields_.prev_log_index = req.prev_log_index; - - const int res = append_entries_client_.call(node_id, req); - if (res < 0) - { - trace(TraceRaftAppendEntriesCallFailure, res); - } - } - - propagateCommitIndex(); -} - -void RaftCore::switchState(const ServerState new_state) -{ - if (server_state_ != new_state) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "State switch: %d --> %d", - int(server_state_), int(new_state)); - trace(TraceRaftStateSwitch, new_state); - - if ((ServerStateLeader == server_state_) || - (ServerStateLeader == new_state)) - { - log_commit_handler_.onLeaderChange(ServerStateLeader == new_state); - } - - server_state_ = new_state; - - cluster_.resetAllServerIndices(); - - 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(); - } -} - -void RaftCore::setActiveMode(const bool new_active) -{ - if (active_mode_ != new_active) - { - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Active switch: %d --> %d", - int(active_mode_), int(new_active)); - trace(TraceRaftActiveSwitch, new_active); - - active_mode_ = new_active; - } -} - -void RaftCore::tryIncrementCurrentTermFromResponse(Term new_term) -{ - trace(TraceRaftNewerTermInResponse, new_term); - const int res = persistent_state_.setCurrentTerm(new_term); - if (res < 0) - { - trace(TraceRaftPersistStateUpdateError, res); - } - registerActivity(); // Deferring future elections - switchState(ServerStateFollower); - setActiveMode(false); -} - -void RaftCore::propagateCommitIndex() -{ - // Objective is to estimate whether we can safely increment commit index value - UAVCAN_ASSERT(server_state_ == ServerStateLeader); - UAVCAN_ASSERT(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); - - uint8_t num_nodes_with_next_log_entry_available = 1; // Local node - for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) - { - const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); - if (match_index > commit_index_) - { - num_nodes_with_next_log_entry_available++; - } - } - - if (num_nodes_with_next_log_entry_available >= cluster_.getQuorumSize()) - { - commit_index_++; - UAVCAN_ASSERT(commit_index_ > 0); // Index 0 is always committed - trace(TraceRaftNewEntryCommitted, commit_index_); - - // AT THIS POINT ALLOCATION IS COMPLETE - log_commit_handler_.onEntryCommitted(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); - } - } -} - -void RaftCore::handleAppendEntriesRequest(const ReceivedDataStructure& request, - ServiceResponseDataStructure& response) -{ - if (!cluster_.isKnownServer(request.getSrcNodeID())) - { - trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); - return; - } - - registerActivity(); - - UAVCAN_ASSERT(response.isResponseEnabled()); // This is default - - /* - * 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()) - { - int res = persistent_state_.setCurrentTerm(request.term); - if (res < 0) - { - response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); - } - - res = persistent_state_.resetVotedFor(); - if (res < 0) - { - response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); - } - - switchState(ServerStateFollower); - setActiveMode(false); - - if (!response.isResponseEnabled()) - { - return; - } - } - - /* - * Preparing the response - */ - response.term = persistent_state_.getCurrentTerm(); - response.success = false; - - /* - * Step 1 (see Raft paper) - * Reject the request if the leader has stale term number. - */ - if (request.term < persistent_state_.getCurrentTerm()) - { - response.setResponseEnabled(true); - return; - } - - switchState(ServerStateFollower); - setActiveMode(false); - - /* - * Step 2 - * Reject the request if the assumed log index does not exist on the local node. - */ - const Entry* const prev_entry = persistent_state_.getLog().getEntryAtIndex(request.prev_log_index); - if (prev_entry == NULL) - { - response.setResponseEnabled(true); - return; - } - - /* - * Step 3 - * Drop log entries if term number does not match. - * Ignore the request if the persistent state cannot be updated. - */ - if (prev_entry->term != request.prev_log_term) - { - const int res = persistent_state_.getLog().removeEntriesWhereIndexGreaterOrEqual(request.prev_log_index); - response.setResponseEnabled(res >= 0); - if (res < 0) - { - trace(TraceRaftPersistStateUpdateError, res); - } - return; - } - - /* - * Step 4 - * Update the log with new entries - this will possibly require to rewrite existing entries. - * Ignore the request if the persistent state cannot be updated. - */ - if (request.prev_log_index != persistent_state_.getLog().getLastIndex()) - { - const int res = persistent_state_.getLog().removeEntriesWhereIndexGreater(request.prev_log_index); - if (res < 0) - { - trace(TraceRaftPersistStateUpdateError, res); - response.setResponseEnabled(false); - return; - } - } - - for (uint8_t i = 0; i < request.entries.size(); i++) - { - const int res = persistent_state_.getLog().append(request.entries[i]); - if (res < 0) - { - trace(TraceRaftPersistStateUpdateError, res); - response.setResponseEnabled(false); - return; // Response will not be sent, the server will assume that we're dead - } - } - - /* - * Step 5 - * Update the commit index. - */ - if (request.leader_commit > commit_index_) - { - commit_index_ = min(request.leader_commit, persistent_state_.getLog().getLastIndex()); - trace(TraceRaftCommitIndexUpdate, commit_index_); - } - - response.setResponseEnabled(true); - response.success = true; -} - -void RaftCore::handleAppendEntriesResponse(const ServiceCallResult& result) -{ - UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled - - if (!result.isSuccessful()) - { - return; - } - - if (result.response.term > persistent_state_.getCurrentTerm()) - { - tryIncrementCurrentTermFromResponse(result.response.term); - } - else - { - if (result.response.success) - { - cluster_.incrementServerNextIndexBy(result.server_node_id, pending_append_entries_fields_.num_entries); - cluster_.setServerMatchIndex(result.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); - } - } - - pending_append_entries_fields_ = PendingAppendEntriesFields(); - // Rest of the logic is implemented in periodic update handlers. -} - -void RaftCore::handleRequestVoteRequest(const ReceivedDataStructure& request, - ServiceResponseDataStructure& response) -{ - trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); - - if (!cluster_.isKnownServer(request.getSrcNodeID())) - { - trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); - return; - } - - 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()) - { - int res = persistent_state_.setCurrentTerm(request.term); - if (res < 0) - { - response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); - } - - res = persistent_state_.resetVotedFor(); - if (res < 0) - { - response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); - } - - switchState(ServerStateFollower); - - if (!response.isResponseEnabled()) - { - return; - } - } - - /* - * Preparing the response - */ - response.term = persistent_state_.getCurrentTerm(); - - if (request.term < response.term) - { - response.vote_granted = false; - } - else - { - const bool can_vote = !persistent_state_.isVotedForSet() || - (persistent_state_.getVotedFor() == request.getSrcNodeID()); - const bool log_is_up_to_date = - persistent_state_.getLog().isOtherLogUpToDate(request.last_log_index, request.last_log_term); - - response.vote_granted = can_vote && log_is_up_to_date; - - if (response.vote_granted) - { - registerActivity(); // This is necessary to avoid excessive elections - - const int res = persistent_state_.setVotedFor(request.getSrcNodeID()); - if (res < 0) - { - trace(TraceRaftPersistStateUpdateError, res); - response.setResponseEnabled(false); - return; - } - } - } -} - -void RaftCore::handleRequestVoteResponse(const ServiceCallResult& result) -{ - UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled - - if (!result.isSuccessful()) - { - return; - } - - trace(TraceRaftVoteRequestSucceeded, result.server_node_id.get()); - - if (result.response.term > persistent_state_.getCurrentTerm()) - { - tryIncrementCurrentTermFromResponse(result.response.term); - } - else - { - if (result.response.vote_granted) - { - num_votes_received_in_this_campaign_++; - } - } - // Rest of the logic is implemented in periodic update handlers. - // I'm no fan of asynchronous programming. At all. -} - -void RaftCore::handleTimerEvent(const TimerEvent&) -{ - if (cluster_.hadDiscoveryActivity() && isLeader()) - { - setActiveMode(true); - } - - switch (server_state_) - { - case ServerStateFollower: - { - updateFollower(); - break; - } - case ServerStateCandidate: - { - updateCandidate(); - break; - } - case ServerStateLeader: - { - updateLeader(); - break; - } - default: - { - UAVCAN_ASSERT(0); - break; - } - } -} - -int RaftCore::init(uint8_t cluster_size) -{ - /* - * 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; - - /* - * Initializing internals - */ - int res = persistent_state_.init(); - if (res < 0) - { - return res; - } - - res = cluster_.init(cluster_size); - if (res < 0) - { - return res; - } - - res = append_entries_srv_.start(AppendEntriesCallback(this, &RaftCore::handleAppendEntriesRequest)); - if (res < 0) - { - return res; - } - - res = request_vote_srv_.start(RequestVoteCallback(this, &RaftCore::handleRequestVoteRequest)); - if (res < 0) - { - return res; - } - - res = append_entries_client_.init(); - if (res < 0) - { - return res; - } - 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_clients_[i]->init(); - if (res < 0) - { - return res; - } - request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); - request_vote_clients_[i]->setRequestTimeout(update_interval_); - } - - startPeriodic(update_interval_); - - trace(TraceRaftCoreInited, update_interval_.toUSec()); - - UAVCAN_ASSERT(res >= 0); - return 0; -} - -int RaftCore::appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) -{ - if (isLeader()) - { - Entry entry; - entry.node_id = node_id.get(); - entry.unique_id = unique_id; - entry.term = persistent_state_.getCurrentTerm(); - - trace(TraceRaftNewLogEntry, entry.node_id); - return persistent_state_.getLog().append(entry); - } - else - { - UAVCAN_ASSERT(0); - return -ErrLogic; - } -} - -/* - * AllocationRequestManager - */ -uint8_t AllocationRequestManager::detectRequestStage(const protocol::dynamic_node_id::Allocation& msg) -{ - const uint8_t max_bytes_per_request = protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; - - if ((msg.unique_id.size() != max_bytes_per_request) && - (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) && - (msg.unique_id.size() != msg.unique_id.capacity())) // Future proofness for CAN FD - { - return InvalidStage; - } - if (msg.first_part_of_unique_id) - { - return 1; // Note that CAN FD frames can deliver the unique ID in one stage! - } - if (msg.unique_id.size() == protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) - { - return 2; - } - if (msg.unique_id.size() < protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) - { - return 3; - } - return InvalidStage; -} - -uint8_t AllocationRequestManager::getExpectedStage() const -{ - if (current_unique_id_.empty()) - { - return 1; - } - if (current_unique_id_.size() >= (protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) - { - return 3; - } - if (current_unique_id_.size() >= protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) - { - return 2; - } - return InvalidStage; -} - -void AllocationRequestManager::broadcastIntermediateAllocationResponse() -{ - UAVCAN_ASSERT(active_); - - protocol::dynamic_node_id::Allocation msg; - msg.unique_id = current_unique_id_; - UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); - - UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", - unsigned(msg.unique_id.size())); - - const int res = allocation_pub_.broadcast(msg); - if (res < 0) - { - allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast"); - } -} - -void AllocationRequestManager::handleAllocation(const ReceivedDataStructure& msg) -{ - if (!msg.isAnonymousTransfer()) - { - return; // This is a response from another allocator, ignore - } - - if (!active_) - { - return; // The local node is not the leader, ignore - } - - /* - * Reset the expected stage on timeout - */ - if (msg.getMonotonicTimestamp() > (last_message_timestamp_ + stage_timeout_)) - { - UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); - current_unique_id_.clear(); - } - last_message_timestamp_ = msg.getMonotonicTimestamp(); - - /* - * Checking if request stage matches the expected stage - */ - const uint8_t request_stage = detectRequestStage(msg); - if (request_stage == InvalidStage) - { - return; // No way - } - - const uint8_t expected_stage = getExpectedStage(); - if (request_stage == InvalidStage) - { - current_unique_id_.clear(); - return; - } - - if (request_stage != expected_stage) - { - return; // Ignore - stage mismatch - } - - const uint8_t max_expected_bytes = static_cast(current_unique_id_.capacity() - current_unique_id_.size()); - UAVCAN_ASSERT(max_expected_bytes > 0); - if (msg.unique_id.size() > max_expected_bytes) - { - return; // Malformed request - } - - /* - * Updating the local state - */ - for (uint8_t i = 0; i < msg.unique_id.size(); i++) - { - current_unique_id_.push_back(msg.unique_id[i]); - } - - /* - * Proceeding with allocation if possible - */ - if (current_unique_id_.size() == current_unique_id_.capacity()) - { - UAVCAN_TRACE("AllocationRequestManager", "Allocation request received; preferred node ID: %d", - int(msg.node_id)); - - IAllocationRequestHandler::UniqueID unique_id; - copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); - current_unique_id_.clear(); - - handler_.handleAllocationRequest(unique_id, msg.node_id); - } - else - { - broadcastIntermediateAllocationResponse(); - } -} - -int AllocationRequestManager::init() -{ - int res = allocation_pub_.init(); - if (res < 0) - { - return res; - } - (void)allocation_pub_.setPriority(TransferPriorityLow); - - res = allocation_sub_.start(AllocationCallback(this, &AllocationRequestManager::handleAllocation)); - if (res < 0) - { - return res; - } - allocation_sub_.allowAnonymousTransfers(); - - return 0; -} - -void AllocationRequestManager::setActive(bool x) -{ - active_ = x; - if (!active_) - { - current_unique_id_.clear(); - } -} - -int AllocationRequestManager::broadcastAllocationResponse(const IAllocationRequestHandler::UniqueID& unique_id, - NodeID allocated_node_id) -{ - if (!active_) - { - UAVCAN_ASSERT(0); - return -ErrLogic; - } - - protocol::dynamic_node_id::Allocation msg; - - msg.unique_id.resize(msg.unique_id.capacity()); - copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); - - msg.node_id = allocated_node_id.get(); - - return allocation_pub_.broadcast(msg); -} - -/* - * There's no reason to remove these types from the class scope, except that otherwise the half-broken Eclipse CDT - * indexer goes bananas. - */ -struct UniqueIDLogPredicate -{ - const IAllocationRequestHandler::UniqueID unique_id; - - UniqueIDLogPredicate(const IAllocationRequestHandler::UniqueID& uid) - : unique_id(uid) - { } - - bool operator()(const RaftCore::LogEntryInfo& info) const - { - return info.entry.unique_id == unique_id; - } -}; - -struct NodeIDLogPredicate -{ - const NodeID node_id; - - NodeIDLogPredicate(const NodeID& nid) - : node_id(nid) - { } - - bool operator()(const RaftCore::LogEntryInfo& info) const - { - return info.entry.node_id == node_id.get(); - } -}; - -} // dynamic_node_id_server_impl - -/* - * DynamicNodeIDAllocationServer - */ -bool DynamicNodeIDAllocationServer::isNodeIDTaken(const NodeID node_id) const -{ - UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Testing if node ID %d is taken", int(node_id.get())); - return raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::NodeIDLogPredicate(node_id)); -} - -NodeID DynamicNodeIDAllocationServer::findFreeNodeID(const NodeID preferred_node_id) const -{ - uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; - - // Up - while (candidate <= NodeID::Max) - { - if (!isNodeIDTaken(candidate)) - { - return candidate; - } - candidate++; - } - - candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; - candidate--; // This has been tested already - - // Down - while (candidate > 0) - { - if (!isNodeIDTaken(candidate)) - { - return candidate; - } - candidate--; - } - - return NodeID(); -} - -void DynamicNodeIDAllocationServer::allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) -{ - const NodeID allocated_node_id = findFreeNodeID(preferred_node_id); - if (!allocated_node_id.isUnicast()) - { - UAVCAN_TRACE("DynamicNodeIDAllocationServer", "Request ignored - no free node ID left"); - return; - } - - UAVCAN_TRACE("DynamicNodeIDAllocationServer", "New node ID allocated: %d", int(allocated_node_id.get())); - const int res = raft_core_.appendLog(unique_id, allocated_node_id); - if (res < 0) - { - getNode().registerInternalFailure("Raft log append"); - } -} - -void DynamicNodeIDAllocationServer::handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) -{ - // TODO: allocation requests must not be served if the list of unidentified nodes is not empty - - const LazyConstructor result = - raft_core_.traverseLogFromEndUntil(dynamic_node_id_server_impl::UniqueIDLogPredicate(unique_id)); - - if (result.isConstructed()) - { - if (result->committed) - { - tryPublishAllocationResult(result->entry); - UAVCAN_TRACE("DynamicNodeIDAllocationServer", - "Allocation request served with existing allocation; node ID %d", - int(result->entry.node_id)); - } - else - { - UAVCAN_TRACE("DynamicNodeIDAllocationServer", - "Allocation request ignored - allocation exists but not committed yet; node ID %d", - int(result->entry.node_id)); - } - } - else - { - allocateNewNode(unique_id, preferred_node_id); - } -} - -void DynamicNodeIDAllocationServer::onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry) -{ - tryPublishAllocationResult(entry); -} - -void DynamicNodeIDAllocationServer::onLeaderChange(bool local_node_is_leader) -{ - UAVCAN_TRACE("DynamicNodeIDAllocationServer", "I am leader: %d", int(local_node_is_leader)); - allocation_request_manager_.setActive(local_node_is_leader); -} - -void DynamicNodeIDAllocationServer::tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) -{ - const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); - if (res < 0) - { - getNode().registerInternalFailure("Dynamic allocation final broadcast"); - } -} - -int DynamicNodeIDAllocationServer::init(uint8_t cluster_size) -{ - int res = raft_core_.init(cluster_size); - if (res < 0) - { - return res; - } - - res = allocation_request_manager_.init(); - if (res < 0) - { - return res; - } - - // TODO Initialize the node info transport - - return 0; -} - -} diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/server.cpp similarity index 86% rename from libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp rename to libuavcan/test/protocol/dynamic_node_id_server/server.cpp index 64254fc143..ee06b71573 100644 --- a/libuavcan/test/protocol/dynamic_node_id_allocation_server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/server.cpp @@ -10,11 +10,13 @@ #include #include #include -#include +#include #include -#include "helpers.hpp" +#include "../helpers.hpp" -class StorageBackend : public uavcan::IDynamicNodeIDStorageBackend +using uavcan::dynamic_node_id_server::UniqueID; + +class StorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend { typedef std::map Container; Container container_; @@ -60,11 +62,11 @@ public: }; -class EventTracer : public uavcan::IDynamicNodeIDAllocationServerEventTracer +class EventTracer : public uavcan::dynamic_node_id_server::distributed::IEventTracer { const std::string id_; - virtual void onEvent(uavcan::uint16_t code, uavcan::int64_t argument) + virtual void onEvent(uavcan::dynamic_node_id_server::distributed::TraceCode code, uavcan::int64_t argument) { std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; } @@ -76,7 +78,7 @@ public: }; -class CommitHandler : public uavcan::dynamic_node_id_server_impl::ILeaderLogCommitHandler +class CommitHandler : public uavcan::dynamic_node_id_server::distributed::ILeaderLogCommitHandler { const std::string id_; @@ -95,7 +97,7 @@ public: }; -class AllocationRequestHandler : public uavcan::dynamic_node_id_server_impl::IAllocationRequestHandler +class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler { std::vector > requests_; @@ -139,13 +141,13 @@ public: static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry -TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) +TEST(DynamicNodeIDServer, StorageMarshaller) { StorageBackend st; - uavcan::dynamic_node_id_server_impl::MarshallingStorageDecorator marshaler(st); + uavcan::dynamic_node_id_server::StorageMarshaller marshaler(st); - uavcan::IDynamicNodeIDStorageBackend::String key; + uavcan::dynamic_node_id_server::IStorageBackend::String key; /* * uint32 @@ -221,13 +223,15 @@ TEST(DynamicNodeIDAllocationServer, MarshallingStorageDecorator) } -TEST(DynamicNodeIDAllocationServer, LogInitialization) +TEST(DynamicNodeIDServer, LogInitialization) { + using namespace uavcan::dynamic_node_id_server::distributed; + EventTracer tracer; // No log data in the storage - initializing empty log { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer); ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, log.init()); @@ -241,7 +245,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) // Nonempty storage, one item { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); storage.set("log_last_index", "0"); ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found @@ -258,7 +262,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) // Nonempty storage, broken data { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); storage.set("log_last_index", "foobar"); ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value @@ -281,7 +285,7 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) // Nonempty storage, many items { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); storage.set("log_last_index", "1"); // 2 items - 0, 1 storage.set("log0_term", "0"); @@ -317,11 +321,13 @@ TEST(DynamicNodeIDAllocationServer, LogInitialization) } -TEST(DynamicNodeIDAllocationServer, LogAppend) +TEST(DynamicNodeIDServer, LogAppend) { + using namespace uavcan::dynamic_node_id_server::distributed; + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, log.init()); @@ -390,11 +396,13 @@ TEST(DynamicNodeIDAllocationServer, LogAppend) } -TEST(DynamicNodeIDAllocationServer, LogRemove) +TEST(DynamicNodeIDServer, LogRemove) { + using namespace uavcan::dynamic_node_id_server::distributed; + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); /* * Filling the log fully @@ -446,15 +454,17 @@ TEST(DynamicNodeIDAllocationServer, LogRemove) } -TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) +TEST(DynamicNodeIDServer, PersistentStorageInitialization) { + using namespace uavcan::dynamic_node_id_server::distributed; + EventTracer tracer; /* * First initialization */ { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); + PersistentState pers(storage, tracer); ASSERT_EQ(0, storage.getNumKeys()); ASSERT_LE(0, pers.init()); @@ -472,12 +482,12 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { // This log is used to initialize the storage - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); ASSERT_LE(0, log.init()); } ASSERT_LE(1, storage.getNumKeys()); - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); + PersistentState pers(storage, tracer); ASSERT_LE(0, pers.init()); @@ -494,14 +504,14 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { // This log is used to initialize the storage - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); ASSERT_LE(0, log.init()); } ASSERT_LE(1, storage.getNumKeys()); storage.set("current_term", "1"); - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); + PersistentState pers(storage, tracer); ASSERT_GT(0, pers.init()); // Fails because current term is not zero @@ -522,7 +532,7 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) { // This log is used to initialize the storage - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); ASSERT_LE(0, log.init()); uavcan::protocol::dynamic_node_id::server::Entry entry; @@ -533,7 +543,7 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) } ASSERT_LE(4, storage.getNumKeys()); - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); + PersistentState pers(storage, tracer); ASSERT_GT(0, pers.init()); // Fails because log is not empty @@ -556,11 +566,13 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorageInitialization) } -TEST(DynamicNodeIDAllocationServer, PersistentStorage) +TEST(DynamicNodeIDServer, PersistentStorage) { + using namespace uavcan::dynamic_node_id_server::distributed; + EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::PersistentState pers(storage, tracer); + PersistentState pers(storage, tracer); /* * Initializing @@ -643,8 +655,10 @@ TEST(DynamicNodeIDAllocationServer, PersistentStorage) } -TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) +TEST(DynamicNodeIDServer, ClusterManagerInitialization) { + using namespace uavcan::dynamic_node_id_server::distributed; + const unsigned MaxClusterSize = uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize; @@ -658,10 +672,10 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) */ { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); + ClusterManager mgr(nodes.a, storage, log, tracer); // Too big ASSERT_GT(0, mgr.init(MaxClusterSize + 1)); @@ -683,10 +697,10 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) */ { StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); + ClusterManager mgr(nodes.a, storage, log, tracer); // Not configured ASSERT_GT(0, mgr.init()); @@ -700,17 +714,19 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerInitialization) } -TEST(DynamicNodeIDAllocationServer, ClusterManagerOneServer) +TEST(DynamicNodeIDServer, ClusterManagerOneServer) { + using namespace uavcan::dynamic_node_id_server::distributed; + uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); + ClusterManager mgr(nodes.a, storage, log, tracer); /* * Pub and sub @@ -775,17 +791,19 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerOneServer) } -TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) +TEST(DynamicNodeIDServer, ClusterManagerThreeServers) { + using namespace uavcan::dynamic_node_id_server::distributed; + uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; EventTracer tracer; StorageBackend storage; - uavcan::dynamic_node_id_server_impl::Log log(storage, tracer); + Log log(storage, tracer); InterlinkedTestNodesWithSysClock nodes; - uavcan::dynamic_node_id_server_impl::ClusterManager mgr(nodes.a, storage, log, tracer); + ClusterManager mgr(nodes.a, storage, log, tracer); /* * Pub and sub @@ -891,9 +909,9 @@ TEST(DynamicNodeIDAllocationServer, ClusterManagerThreeServers) } -TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) +TEST(DynamicNodeIDServer, RaftCoreBasic) { - using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::dynamic_node_id_server::distributed; using namespace uavcan::protocol::dynamic_node_id::server; uavcan::GlobalDataTypeRegistry::instance().reset(); @@ -970,28 +988,24 @@ TEST(DynamicNodeIDAllocationServer, RaftCoreBasic) } -TEST(DynamicNodeIDAllocationServer, EventCodeToString) +TEST(DynamicNodeIDServer, EventCodeToString) { - using uavcan::IDynamicNodeIDAllocationServerEventTracer; - using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::dynamic_node_id_server::distributed; + using namespace uavcan::dynamic_node_id_server; // Simply checking some error codes - ASSERT_STREQ("Error", - IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceError)); - ASSERT_STREQ("RaftActiveSwitch", - IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftActiveSwitch)); - ASSERT_STREQ("RaftAppendEntriesCallFailure", - IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); - ASSERT_STREQ("DiscoveryReceived", - IDynamicNodeIDAllocationServerEventTracer::getEventName(TraceDiscoveryReceived)); + ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError)); + ASSERT_STREQ("RaftActiveSwitch", IEventTracer::getEventName(TraceRaftActiveSwitch)); + ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); + ASSERT_STREQ("DiscoveryReceived", IEventTracer::getEventName(TraceDiscoveryReceived)); } -TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) +TEST(DynamicNodeIDServer, AllocationRequestManager) { using namespace uavcan::protocol::dynamic_node_id; using namespace uavcan::protocol::dynamic_node_id::server; - using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::dynamic_node_id_server; uavcan::GlobalDataTypeRegistry::instance().reset(); uavcan::DefaultDataTypeRegistrator _reg1; @@ -1045,9 +1059,9 @@ TEST(DynamicNodeIDAllocationServer, AllocationRequestManager) } -TEST(DynamicNodeIDAllocationServer, Main) +TEST(DynamicNodeIDServer, Main) { - using namespace uavcan::dynamic_node_id_server_impl; + using namespace uavcan::dynamic_node_id_server; using namespace uavcan::protocol::dynamic_node_id; using namespace uavcan::protocol::dynamic_node_id::server; @@ -1066,7 +1080,7 @@ TEST(DynamicNodeIDAllocationServer, Main) /* * Server */ - uavcan::DynamicNodeIDAllocationServer server(nodes.a, storage, tracer); + distributed::Server server(nodes.a, storage, tracer); ASSERT_LE(0, server.init(1)); @@ -1092,22 +1106,14 @@ TEST(DynamicNodeIDAllocationServer, Main) } -TEST(DynamicNodeIDAllocationServer, ObjectSizes) +TEST(DynamicNodeIDServer, ObjectSizes) { - std::cout << "Log: " - << sizeof(uavcan::dynamic_node_id_server_impl::Log) << std::endl; + using namespace uavcan::dynamic_node_id_server; - std::cout << "PersistentState: " - << sizeof(uavcan::dynamic_node_id_server_impl::PersistentState) << std::endl; - - std::cout << "ClusterManager: " - << sizeof(uavcan::dynamic_node_id_server_impl::ClusterManager) << std::endl; - - std::cout << "RaftCore: " - << sizeof(uavcan::dynamic_node_id_server_impl::RaftCore) << std::endl; - - std::cout << "AllocationRequestManager: " - << sizeof(uavcan::dynamic_node_id_server_impl::AllocationRequestManager) << std::endl; - - std::cout << "DynamicNodeIDAllocationServer: " << sizeof(uavcan::DynamicNodeIDAllocationServer) << std::endl; + std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl; + std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl; + std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl; + std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl; + std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl; + std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl; } From 696d97880d597e061a96d0d2dbc971c18abf8cde Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 16:05:15 +0300 Subject: [PATCH 054/268] Include guard fix --- .../dynamic_node_id_server/allocation_request_manager.hpp | 4 ++-- .../protocol/dynamic_node_id_server/storage_marshaller.hpp | 4 ++-- .../include/uavcan/protocol/dynamic_node_id_server/types.hpp | 4 ++-- 3 files changed, 6 insertions(+), 6 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 94630c1f06..795bbae43f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_ALLOCATION_REQUEST_MANAGER_HPP_INCLUDED #include #include diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp index 26c937db50..09870d9192 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_marshaller.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_STORAGE_MARSHALLER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_STORAGE_MARSHALLER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_STORAGE_MARSHALLER_HPP_INCLUDED #include #include diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp index 8ae2b6e9f6..360a93aee6 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/types.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_TYPES_HPP_INCLUDED -#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_INTERNAL_TYPES_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_TYPES_HPP_INCLUDED #include // UAVCAN types From 0f10d85f7ed2c825a5a6e824aa436473c318ac43 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 16:15:41 +0300 Subject: [PATCH 055/268] Node ID selection logic extracted into a class --- .../distributed/server.hpp | 34 +-------- .../node_id_selector.hpp | 72 +++++++++++++++++++ 2 files changed, 75 insertions(+), 31 deletions(-) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 24b4b99cff..b15dc9df39 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -12,6 +12,7 @@ #include #include #include +#include namespace uavcan { @@ -69,39 +70,10 @@ class Server : private IAllocationRequestHandler return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); } - NodeID findFreeNodeID(const NodeID preferred_node_id) const - { - uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; - - // Up - while (candidate <= NodeID::Max) - { - if (!isNodeIDTaken(candidate)) - { - return candidate; - } - candidate++; - } - - candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; - candidate--; // This has been tested already - - // Down - while (candidate > 0) - { - if (!isNodeIDTaken(candidate)) - { - return candidate; - } - candidate--; - } - - return NodeID(); - } - void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) { - const NodeID allocated_node_id = findFreeNodeID(preferred_node_id); + const NodeID allocated_node_id = + NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); if (!allocated_node_id.isUnicast()) { UAVCAN_TRACE("DynamicNodeIDServer", "Request ignored - no free node ID left"); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp new file mode 100644 index 0000000000..2af730a67e --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp @@ -0,0 +1,72 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_ID_SELECTOR_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * Node ID allocation logic + */ +template +class NodeIDSelector +{ + typedef bool (Owner::*IsNodeIDTakenMethod)(const NodeID node_id) const; + + const Owner* const owner_; + const IsNodeIDTakenMethod is_node_id_taken_; + +public: + NodeIDSelector(const Owner* owner, IsNodeIDTakenMethod is_node_id_taken) + : owner_(owner) + , is_node_id_taken_(is_node_id_taken) + { + UAVCAN_ASSERT(owner_ != NULL); + UAVCAN_ASSERT(is_node_id_taken_ != NULL); + } + + /** + * Reutrns a default-constructed (invalid) node ID if a free one could not be found. + */ + NodeID findFreeNodeID(const NodeID preferred_node_id) const + { + uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + + // Up + while (candidate <= NodeID::Max) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + candidate--; // This has been tested already + + // Down + while (candidate > 0) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); + } +}; + +} +} + +#endif // Include guard From 567d16764069d8db8534e2a9707cb4132cf95c1c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 16:21:50 +0300 Subject: [PATCH 056/268] Updated default service timeout --- libuavcan/include/uavcan/node/service_client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b4c82774ba..067d3b890c 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -110,7 +110,7 @@ public: /** * It's not recommended to override default timeouts. */ - static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(500); } static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); } static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); } From 2c36e91c54d7b07a592f6b812387ddc63a606bfb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 16:29:31 +0300 Subject: [PATCH 057/268] Fixed calls to UAVCAN_TRACE() --- .../distributed/cluster_manager.hpp | 14 ++++++++------ .../dynamic_node_id_server/distributed/log.hpp | 12 ++++++------ .../distributed/persistent_state.hpp | 16 ++++++++++------ .../distributed/raft_core.hpp | 9 +++++---- 4 files changed, 29 insertions(+), 22 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 3166962276..79ec57e1ac 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -128,13 +128,14 @@ private: /* * Broadcasting */ - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Broadcasting Discovery message; known nodes: %d of %d", + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Broadcasting Discovery message; known nodes: %d of %d", int(msg.known_nodes.size()), int(cluster_size_)); const int res = discovery_pub_.broadcast(msg); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcst failed: %d", res); + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Discovery broadcst failed: %d", res); getNode().registerInternalFailure("Raft discovery broadcast"); } @@ -143,7 +144,8 @@ private: */ if (isClusterDiscovered()) { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Discovery broadcasting timer stopped"); + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Discovery broadcasting timer stopped"); stop(); } } @@ -237,13 +239,13 @@ public: int res = io.get(getStorageKeyForClusterSize(), value); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is neither configured nor stored in the storage"); return res; } if ((value == 0) || (value > MaxClusterSize)) { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Cluster size is invalid"); + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is invalid"); return -ErrFailure; } cluster_size_ = static_cast(value); @@ -262,7 +264,7 @@ public: int res = io.setAndGetBack(getStorageKeyForClusterSize(), value); if ((res < 0) || (value != init_cluster_size)) { - UAVCAN_TRACE("dynamic_node_id_server_impl::ClusterManager", "Failed to store cluster size"); + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Failed to store cluster size"); return -ErrFailure; } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp index 8b1e1668e3..85be7d6e4b 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -168,13 +168,13 @@ public: { if (storage_.get(getLastIndexKey()).empty()) { - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Initializing empty storage"); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Initializing empty storage"); return initEmptyLogStorage(); } else { // There's some data in the storage, but it cannot be parsed - reporting an error - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read last index"); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read last index"); return -ErrFailure; } } @@ -193,13 +193,13 @@ public: const int result = readEntryFromStorage(index, entries_[index]); if (result < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Failed to read entry at index %u: %d", + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Failed to read entry at index %u: %d", unsigned(index), result); return result; } } - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Restored %u log entries", unsigned(last_index_)); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Restored %u log entries", unsigned(last_index_)); return 0; } @@ -238,7 +238,7 @@ public: entries_[new_last_index] = entry; last_index_ = Index(new_last_index); - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "New entry, index %u, node ID %u, term %u", + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "New entry, index %u, node ID %u, term %u", unsigned(last_index_), unsigned(entry.node_id), unsigned(entry.term)); return 0; } @@ -272,7 +272,7 @@ public: { return -ErrFailure; } - UAVCAN_TRACE("dynamic_node_id_server_impl::Log", "Entries removed, last index %u --> %u", + UAVCAN_TRACE("dynamic_node_id_server::distributed::Log", "Entries removed, last index %u --> %u", unsigned(last_index_), unsigned(new_last_index)); last_index_ = Index(new_last_index); } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp index 33eb4673cd..580bab219c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -50,7 +50,7 @@ public: int res = log_.init(); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Log init failed: %d", res); + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Log init failed: %d", res); return res; } @@ -75,7 +75,8 @@ public: res = io.setAndGetBack(getCurrentTermKey(), current_term_); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init current term: %d", res); + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to init current term: %d", res); return res; } if (current_term_ != 0) @@ -89,7 +90,8 @@ public: res = io.get(getCurrentTermKey(), current_term_); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read current term: %d", res); + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to read current term: %d", res); return res; } } @@ -98,7 +100,7 @@ public: if (current_term_ < last_entry->term) { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", "Persistent storage is damaged: current term is less than term of the last log entry (%u < %u)", unsigned(current_term_), unsigned(last_entry->term)); return -ErrLogic; @@ -115,7 +117,8 @@ public: res = io.setAndGetBack(getVotedForKey(), stored_voted_for); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to init votedFor: %d", res); + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to init votedFor: %d", res); return res; } if (stored_voted_for != 0) @@ -130,7 +133,8 @@ public: res = io.get(getVotedForKey(), stored_voted_for); if (res < 0) { - UAVCAN_TRACE("dynamic_node_id_server_impl::PersistentState", "Failed to read votedFor: %d", res); + UAVCAN_TRACE("dynamic_node_id_server::distributed::PersistentState", + "Failed to read votedFor: %d", res); return res; } if (stored_voted_for > NodeID::Max) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 87fa73dd65..dcf4c534e1 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -165,7 +165,7 @@ class RaftCore : private TimerBase { const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Election complete, won: %d", int(won)); + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Election complete, won: %d", int(won)); switchState(won ? ServerStateLeader : ServerStateFollower); // Start over or become leader } @@ -202,7 +202,8 @@ class RaftCore : private TimerBase break; } - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Requesting vote from %d", int(node_id.get())); + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Requesting vote from %d", int(node_id.get())); trace(TraceRaftVoteRequestInitiation, node_id.get()); res = request_vote_clients_[i]->call(node_id, req); @@ -276,7 +277,7 @@ class RaftCore : private TimerBase { if (server_state_ != new_state) { - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "State switch: %d --> %d", + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d", int(server_state_), int(new_state)); trace(TraceRaftStateSwitch, new_state); @@ -305,7 +306,7 @@ class RaftCore : private TimerBase { if (active_mode_ != new_active) { - UAVCAN_TRACE("dynamic_node_id_server_impl::RaftCore", "Active switch: %d --> %d", + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Active switch: %d --> %d", int(active_mode_), int(new_active)); trace(TraceRaftActiveSwitch, new_active); From ea3886b04835d1d7973185a0aeedb01a6d96a547 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 16:55:52 +0300 Subject: [PATCH 058/268] Refactored tests of node ID allocation server --- .../distributed/cluster_manager.hpp | 2 + .../allocation_request_manager.cpp | 109 ++ .../distributed/cluster_manager.cpp | 263 ++++ .../distributed/event.cpp | 19 + .../distributed/event_tracer.hpp | 25 + .../distributed/log.cpp | 243 ++++ .../distributed/persistent_state.cpp | 209 +++ .../distributed/server.cpp | 176 +++ .../memory_storage_backend.hpp | 53 + .../dynamic_node_id_server/server.cpp | 1119 ----------------- .../storage_marshaller.cpp | 88 ++ 11 files changed, 1187 insertions(+), 1119 deletions(-) create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp delete mode 100644 libuavcan/test/protocol/dynamic_node_id_server/server.cpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 79ec57e1ac..52328fd23d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -7,9 +7,11 @@ #include #include +#include #include #include #include +#include #include #include #include diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp new file mode 100644 index 0000000000..a11969b088 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -0,0 +1,109 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "../helpers.hpp" + + +using uavcan::dynamic_node_id_server::UniqueID; + +class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler +{ + std::vector > requests_; + +public: + virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + requests_.push_back(std::pair(unique_id, preferred_node_id)); + } + + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) + { + if (requests_.empty()) + { + std::cout << "No pending requests" << std::endl; + return false; + } + + const std::pair pair = requests_.at(requests_.size() - 1U); + requests_.pop_back(); + + if (pair.first != unique_id) + { + std::cout << "Unique ID mismatch" << std::endl; + return false; + } + + if (pair.second != preferred_node_id) + { + std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")" + << std::endl; + return false; + } + + return true; + } + + void reset() { requests_.clear(); } +}; + + +TEST(dynamic_node_id_server_AllocationRequestManager, Basic) +{ + using namespace uavcan::protocol::dynamic_node_id; + using namespace uavcan::protocol::dynamic_node_id::server; + using namespace uavcan::dynamic_node_id_server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDClient client(nodes.b); + + /* + * Client initialization + */ + 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)); + + /* + * Request manager initialization + */ + AllocationRequestHandler handler; + + AllocationRequestManager manager(nodes.a, handler); + + ASSERT_LE(0, manager.init()); + + ASSERT_FALSE(manager.isActive()); + manager.setActive(true); + ASSERT_TRUE(manager.isActive()); + + /* + * Allocation + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(handler.matchAndPopLastRequest(hwver.unique_id, PreferredNodeID)); + + ASSERT_LE(0, manager.broadcastAllocationResponse(hwver.unique_id, PreferredNodeID)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + /* + * Checking the client + */ + ASSERT_TRUE(client.isAllocationComplete()); + + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp new file mode 100644 index 0000000000..1cc9c1e885 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp @@ -0,0 +1,263 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_ClusterManager, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + const unsigned MaxClusterSize = + uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + + /* + * Simple initialization + */ + { + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + // Too big + ASSERT_GT(0, mgr.init(MaxClusterSize + 1)); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + ASSERT_LE(0, mgr.init(5)); + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ("5", storage.get("cluster_size")); + + // Testing other states + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_EQ(5, mgr.getClusterSize()); + ASSERT_EQ(3, mgr.getQuorumSize()); + ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid()); + } + /* + * Recovery from the storage + */ + { + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + // Not configured + ASSERT_GT(0, mgr.init()); + ASSERT_EQ(0, storage.getNumKeys()); + + // OK + storage.set("cluster_size", "5"); + ASSERT_LE(0, mgr.init()); + ASSERT_EQ(1, storage.getNumKeys()); + } +} + + +TEST(dynamic_node_id_server_ClusterManager, OneServer) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(1)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_TRUE(mgr.isClusterDiscovered()); + + ASSERT_EQ(0, nodes.a.internal_failure_count); + + /* + * Broadcasting discovery with wrong cluster size, it will be reported as internal failure + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 2; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + + ASSERT_EQ(1, nodes.a.internal_failure_count); + + /* + * Discovery rate limiting test + */ + ASSERT_FALSE(sub.collector.msg.get()); + + msg = uavcan::protocol::dynamic_node_id::server::Discovery(); + msg.configured_cluster_size = 1; // Correct value + ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + // Rinse repeat + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); +} + + +TEST(dynamic_node_id_server_ClusterManager, ThreeServers) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + InterlinkedTestNodesWithSysClock nodes; + + ClusterManager mgr(nodes.a, storage, log, tracer); + + /* + * Pub and sub + */ + SubscriberWithCollector sub(nodes.b); + uavcan::Publisher pub(nodes.b); + + ASSERT_LE(0, sub.start()); + ASSERT_LE(0, pub.init()); + + /* + * Starting + */ + ASSERT_LE(0, mgr.init(3)); + + ASSERT_EQ(0, mgr.getNumKnownServers()); + ASSERT_FALSE(mgr.isClusterDiscovered()); + + /* + * Discovery publishing rate check + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + ASSERT_FALSE(sub.collector.msg.get()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + sub.collector.msg.reset(); + + /* + * Discovering other nodes + */ + uavcan::protocol::dynamic_node_id::server::Discovery msg; + msg.configured_cluster_size = 3; + msg.known_nodes.push_back(2U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(2, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + sub.collector.msg.reset(); + + ASSERT_FALSE(mgr.isClusterDiscovered()); + + // This will complete the discovery + msg.known_nodes.push_back(127U); + ASSERT_LE(0, pub.broadcast(msg)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); + ASSERT_TRUE(sub.collector.msg.get()); + ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); + ASSERT_EQ(3, sub.collector.msg->known_nodes.size()); + ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); + ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); + ASSERT_EQ(127, sub.collector.msg->known_nodes[2]); + sub.collector.msg.reset(); + + // Making sure discovery is now terminated + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(sub.collector.msg.get()); + + /* + * Checking Raft states + */ + ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0)); + ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1)); + ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2)); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); + + mgr.setServerMatchIndex(2, 10); + ASSERT_EQ(10, mgr.getServerMatchIndex(2)); + + mgr.incrementServerNextIndexBy(2, 5); + ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2)); + mgr.decrementServerNextIndex(2); + ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2)); + + mgr.resetAllServerIndices(); + + ASSERT_EQ(0, mgr.getServerMatchIndex(2)); + ASSERT_EQ(0, mgr.getServerMatchIndex(127)); + + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); + ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp new file mode 100644 index 0000000000..40033726e5 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp @@ -0,0 +1,19 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include + + +TEST(DynamicNodeIDServer, EventCodeToString) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + using namespace uavcan::dynamic_node_id_server; + + // 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("DiscoveryReceived", IEventTracer::getEventName(TraceDiscoveryReceived)); +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp new file mode 100644 index 0000000000..20f8f024c6 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp @@ -0,0 +1,25 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include + + +class EventTracer : public uavcan::dynamic_node_id_server::distributed::IEventTracer +{ + const std::string id_; + + virtual void onEvent(uavcan::dynamic_node_id_server::distributed::TraceCode code, uavcan::int64_t argument) + { + std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; + } + +public: + EventTracer() { } + + EventTracer(const std::string& id) : id_(id) { } +}; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp new file mode 100644 index 0000000000..a836c90302 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp @@ -0,0 +1,243 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry + + +TEST(dynamic_node_id_server_Log, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + // No log data in the storage - initializing empty log + { + MemoryStorageBackend storage; + uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + } + // Nonempty storage, one item + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + } + // Nonempty storage, broken data + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "foobar"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "128"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value + + storage.set("log_last_index", "0"); + ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "128"); // Bad value (127 max) + ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(4, storage.getNumKeys()); + } + // Nonempty storage, many items + { + MemoryStorageBackend storage; + Log log(storage, tracer); + + storage.set("log_last_index", "1"); // 2 items - 0, 1 + storage.set("log0_term", "0"); + storage.set("log0_unique_id", "00000000000000000000000000000000"); + storage.set("log0_node_id", "0"); + storage.set("log1_term", "1"); + storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef"); + storage.set("log1_node_id", "127"); + + ASSERT_LE(0, log.init()); // OK now + ASSERT_EQ(7, storage.getNumKeys()); + ASSERT_EQ(1, log.getLastIndex()); + + ASSERT_EQ(0, log.getEntryAtIndex(0)->term); + ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); + ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), + log.getEntryAtIndex(0)->unique_id); + + ASSERT_EQ(1, log.getEntryAtIndex(1)->term); + ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id); + 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_EQ(uid, log.getEntryAtIndex(1)->unique_id); + } +} + + +TEST(dynamic_node_id_server_Log, Append) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, log.init()); + storage.print(); + ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); + + /* + * Entry at the index 0 always exists, and it's always zero-initialized. + */ + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("log0_term")); + ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id")); + ASSERT_EQ("0", storage.get("log0_node_id")); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("log1_term")); + ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id")); + ASSERT_EQ("1", storage.get("log1_node_id")); + + ASSERT_EQ(1, log.getLastIndex()); + ASSERT_TRUE(entry == *log.getEntryAtIndex(1)); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(7, storage.getNumKeys()); + + entry.term = 2; + entry.node_id = 2; + entry.unique_id[0] = 2; + ASSERT_GT(0, log.append(entry)); + + ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, log.getLastIndex()); + + /* + * Making sure append() fails when the log is full + */ + storage.failOnSetCalls(false); + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, log.append(entry)); // Failing because full + + storage.print(); +} + + +TEST(dynamic_node_id_server_Log, Remove) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + Log log(storage, tracer); + + /* + * Filling the log fully + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + + while (log.getLastIndex() < (log.Capacity - 1)) + { + ASSERT_LE(0, log.append(entry)); + ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); + + entry.term += 1; + entry.node_id = uint8_t(entry.node_id + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + /* + * Removal will fail as the storage is failing to update + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + /* + * Now removal must work + */ + storage.failOnSetCalls(false); + + ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); + ASSERT_EQ(59, log.getLastIndex()); + ASSERT_EQ("59", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30)); + ASSERT_EQ(30, log.getLastIndex()); + ASSERT_EQ("30", storage.get("log_last_index")); + + ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1)); + ASSERT_EQ(0, log.getLastIndex()); + ASSERT_EQ("0", storage.get("log_last_index")); + + storage.print(); +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp new file mode 100644 index 0000000000..94cad3a11a --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp @@ -0,0 +1,209 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "event_tracer.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_PersistentState, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + /* + * First initialization + */ + { + MemoryStorageBackend storage; + PersistentState pers(storage, tracer); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - only empty log is recovered + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + PersistentState pers(storage, tracer); + + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Partial recovery - log and current term are recovered + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + } + ASSERT_LE(1, storage.getNumKeys()); + + storage.set("current_term", "1"); + + PersistentState pers(storage, tracer); + + ASSERT_GT(0, pers.init()); // Fails because current term is not zero + + storage.set("current_term", "0"); + + ASSERT_LE(0, pers.init()); // OK now + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } + /* + * Full recovery + */ + { + MemoryStorageBackend storage; + + { + // This log is used to initialize the storage + Log log(storage, tracer); + ASSERT_LE(0, log.init()); + + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, log.append(entry)); + } + ASSERT_LE(4, storage.getNumKeys()); + + PersistentState pers(storage, tracer); + + ASSERT_GT(0, pers.init()); // Fails because log is not empty + + storage.set("current_term", "0"); + storage.set("voted_for", "0"); + ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm + + storage.set("current_term", "1"); // OK + storage.set("voted_for", "128"); // Invalid value + ASSERT_GT(0, pers.init()); // Fails because of bad votedFor + + storage.set("voted_for", "0"); // OK now + ASSERT_LE(0, pers.init()); + + ASSERT_LE(3, storage.getNumKeys()); + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("1", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + } +} + + +TEST(dynamic_node_id_server_PersistentState, Basic) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + + EventTracer tracer; + MemoryStorageBackend storage; + PersistentState pers(storage, tracer); + + /* + * Initializing + */ + ASSERT_LE(0, pers.init()); + + ASSERT_EQ("0", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Inserting some log entries + */ + uavcan::protocol::dynamic_node_id::server::Entry entry; + entry.term = 1; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, pers.getLog().append(entry)); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("0", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing current term + */ + ASSERT_EQ(0, pers.getCurrentTerm()); + ASSERT_LE(0, pers.setCurrentTerm(2)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("0", storage.get("voted_for")); + + /* + * Changing votedFor + */ + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(0)); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_TRUE(pers.isVotedForSet()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + ASSERT_LE(0, pers.resetVotedFor()); + ASSERT_EQ(0, pers.getVotedFor().get()); + ASSERT_FALSE(pers.isVotedForSet()); + ASSERT_EQ("0", storage.get("voted_for")); + + ASSERT_LE(0, pers.setVotedFor(45)); + ASSERT_TRUE(pers.isVotedForSet()); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Handling errors + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(2, pers.getCurrentTerm()); + ASSERT_GT(0, pers.setCurrentTerm(7893)); + ASSERT_EQ(2, pers.getCurrentTerm()); + + ASSERT_EQ(45, pers.getVotedFor().get()); + ASSERT_GT(0, pers.setVotedFor(78)); + ASSERT_EQ(45, pers.getVotedFor().get()); + + ASSERT_EQ("1", storage.get("log_last_index")); + ASSERT_EQ("2", storage.get("current_term")); + ASSERT_EQ("45", storage.get("voted_for")); + + /* + * Final checks + */ + ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp new file mode 100644 index 0000000000..b5e6ed6068 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + +#include +#include +#include +#include +#include "event_tracer.hpp" +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + +using uavcan::dynamic_node_id_server::UniqueID; + + +class CommitHandler : public uavcan::dynamic_node_id_server::distributed::ILeaderLogCommitHandler +{ + const std::string id_; + + virtual void onEntryCommitted(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + { + std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; + } + + virtual void onLeaderChange(bool local_node_is_leader) + { + std::cout << "I AM LEADER: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + } + +public: + CommitHandler(const std::string& id) : id_(id) { } +}; + + +TEST(DynamicNodeIDServer, RaftCoreBasic) +{ + using namespace uavcan::dynamic_node_id_server::distributed; + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + EventTracer tracer_a("a"); + EventTracer tracer_b("b"); + MemoryStorageBackend storage_a; + MemoryStorageBackend storage_b; + CommitHandler commit_handler_a("a"); + CommitHandler commit_handler_b("b"); + + InterlinkedTestNodesWithSysClock nodes; + + std::auto_ptr raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + std::auto_ptr raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b)); + + /* + * Initialization + */ + ASSERT_LE(0, raft_a->init(2)); + ASSERT_LE(0, raft_b->init(2)); + + /* + * Running and trying not to fall + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + // The one with lower node ID must become a leader + ASSERT_TRUE(raft_a->isLeader()); + ASSERT_FALSE(raft_b->isLeader()); + + ASSERT_EQ(0, raft_a->getCommitIndex()); + ASSERT_EQ(0, raft_b->getCommitIndex()); + + /* + * Adding some stuff + */ + Entry::FieldTypes::unique_id unique_id; + uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); + + ASSERT_LE(0, raft_a->appendLog(unique_id, uavcan::NodeID(1))); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); + + /* + * Terminating the leader - the Follower will continue to sleep + */ + raft_a.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + /* + * Reinitializing the leader - current Follower will become the new Leader + */ + storage_a.reset(); + + raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); + ASSERT_LE(0, raft_a->init(2)); + ASSERT_EQ(0, raft_a->getCommitIndex()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + ASSERT_FALSE(raft_a->isLeader()); + ASSERT_TRUE(raft_b->isLeader()); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); +} + + +TEST(DynamicNodeIDServer, Main) +{ + 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 _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + + EventTracer tracer; + MemoryStorageBackend storage; + + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + /* + * Server + */ + distributed::Server server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(1)); + + /* + * 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(4000)); + + ASSERT_TRUE(client.isAllocationComplete()); + ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); +} + + +TEST(DynamicNodeIDServer, ObjectSizes) +{ + using namespace uavcan::dynamic_node_id_server; + + std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl; + std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl; + std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl; + std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl; + std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl; + std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl; +} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp b/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp new file mode 100644 index 0000000000..516c33e994 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/memory_storage_backend.hpp @@ -0,0 +1,53 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include + +class MemoryStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend +{ + typedef std::map Container; + Container container_; + + bool fail_; + +public: + MemoryStorageBackend() + : fail_(false) + { } + + virtual String get(const String& key) const + { + const Container::const_iterator it = container_.find(key); + if (it == container_.end()) + { + return String(); + } + return it->second; + } + + virtual void set(const String& key, const String& value) + { + if (!fail_) + { + container_[key] = value; + } + } + + void failOnSetCalls(bool really) { fail_ = really; } + + void reset() { container_.clear(); } + + unsigned getNumKeys() const { return unsigned(container_.size()); } + + void print() const + { + for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it) + { + std::cout << it->first.c_str() << "\t" << it->second.c_str() << std::endl; + } + } +}; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/server.cpp deleted file mode 100644 index ee06b71573..0000000000 --- a/libuavcan/test/protocol/dynamic_node_id_server/server.cpp +++ /dev/null @@ -1,1119 +0,0 @@ -/* - * Copyright (C) 2015 Pavel Kirienko - */ - -#if __GNUC__ -// We need auto_ptr for compatibility reasons -# pragma GCC diagnostic ignored "-Wdeprecated-declarations" -#endif - -#include -#include -#include -#include -#include -#include "../helpers.hpp" - -using uavcan::dynamic_node_id_server::UniqueID; - -class StorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend -{ - typedef std::map Container; - Container container_; - - bool fail_; - -public: - StorageBackend() - : fail_(false) - { } - - virtual String get(const String& key) const - { - const Container::const_iterator it = container_.find(key); - if (it == container_.end()) - { - return String(); - } - return it->second; - } - - virtual void set(const String& key, const String& value) - { - if (!fail_) - { - container_[key] = value; - } - } - - void failOnSetCalls(bool really) { fail_ = really; } - - void reset() { container_.clear(); } - - unsigned getNumKeys() const { return unsigned(container_.size()); } - - void print() const - { - for (Container::const_iterator it = container_.begin(); it != container_.end(); ++it) - { - std::cout << it->first.c_str() << "\t" << it->second.c_str() << std::endl; - } - } -}; - - -class EventTracer : public uavcan::dynamic_node_id_server::distributed::IEventTracer -{ - const std::string id_; - - virtual void onEvent(uavcan::dynamic_node_id_server::distributed::TraceCode code, uavcan::int64_t argument) - { - std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; - } - -public: - EventTracer() { } - - EventTracer(const std::string& id) : id_(id) { } -}; - - -class CommitHandler : public uavcan::dynamic_node_id_server::distributed::ILeaderLogCommitHandler -{ - const std::string id_; - - virtual void onEntryCommitted(const uavcan::protocol::dynamic_node_id::server::Entry& entry) - { - std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; - } - - virtual void onLeaderChange(bool local_node_is_leader) - { - std::cout << "I AM LEADER: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; - } - -public: - CommitHandler(const std::string& id) : id_(id) { } -}; - - -class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler -{ - std::vector > requests_; - -public: - virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) - { - requests_.push_back(std::pair(unique_id, preferred_node_id)); - } - - bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) - { - if (requests_.empty()) - { - std::cout << "No pending requests" << std::endl; - return false; - } - - const std::pair pair = requests_.at(requests_.size() - 1U); - requests_.pop_back(); - - if (pair.first != unique_id) - { - std::cout << "Unique ID mismatch" << std::endl; - return false; - } - - if (pair.second != preferred_node_id) - { - std::cout << "Node ID mismatch (" << pair.second.get() << ", " << preferred_node_id.get() << ")" - << std::endl; - return false; - } - - return true; - } - - void reset() { requests_.clear(); } -}; - - -static const unsigned NumEntriesInStorageWithEmptyLog = 4; // last index + 3 items per log entry - - -TEST(DynamicNodeIDServer, StorageMarshaller) -{ - StorageBackend st; - - uavcan::dynamic_node_id_server::StorageMarshaller marshaler(st); - - uavcan::dynamic_node_id_server::IStorageBackend::String key; - - /* - * uint32 - */ - uint32_t u32 = 0; - - key = "foo"; - u32 = 0; - ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); - ASSERT_EQ(0, u32); - - key = "bar"; - u32 = 0xFFFFFFFF; - ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); - ASSERT_EQ(0xFFFFFFFF, u32); - ASSERT_LE(0, marshaler.get(key, u32)); - ASSERT_EQ(0xFFFFFFFF, u32); - - key = "foo"; - ASSERT_LE(0, marshaler.get(key, u32)); - ASSERT_EQ(0, u32); - - key = "the_cake_is_a_lie"; - ASSERT_GT(0, marshaler.get(key, u32)); - ASSERT_EQ(0, u32); - - /* - * uint8[16] - */ - uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id array; - - key = "a"; - // Set zero - ASSERT_LE(0, marshaler.setAndGetBack(key, array)); - for (uint8_t i = 0; i < 16; i++) - { - ASSERT_EQ(0, array[i]); - } - - // Make sure this will not be interpreted as uint32 - ASSERT_GT(0, marshaler.get(key, u32)); - ASSERT_EQ(0, u32); - - // Set pattern - for (uint8_t i = 0; i < 16; i++) - { - array[i] = uint8_t(i + 1); - } - ASSERT_LE(0, marshaler.setAndGetBack(key, array)); - for (uint8_t i = 0; i < 16; i++) - { - ASSERT_EQ(i + 1, array[i]); - } - - // Set another pattern - for (uint8_t i = 0; i < 16; i++) - { - array[i] = uint8_t(i | (i << 4)); - } - ASSERT_LE(0, marshaler.setAndGetBack(key, array)); - for (uint8_t i = 0; i < 16; i++) - { - ASSERT_EQ(uint8_t(i | (i << 4)), array[i]); - } - - // Make sure uint32 cannot be interpreted as an array - key = "foo"; - ASSERT_GT(0, marshaler.get(key, array)); - - // Nonexistent key - key = "the_cake_is_a_lie"; - ASSERT_GT(0, marshaler.get(key, array)); -} - - -TEST(DynamicNodeIDServer, LogInitialization) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - EventTracer tracer; - // No log data in the storage - initializing empty log - { - StorageBackend storage; - uavcan::dynamic_node_id_server::distributed::Log log(storage, tracer); - - ASSERT_EQ(0, storage.getNumKeys()); - ASSERT_LE(0, log.init()); - ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); - ASSERT_EQ(0, log.getLastIndex()); - ASSERT_EQ(0, log.getEntryAtIndex(0)->term); - ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); - ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), - log.getEntryAtIndex(0)->unique_id); - } - // Nonempty storage, one item - { - StorageBackend storage; - Log log(storage, tracer); - - storage.set("log_last_index", "0"); - ASSERT_LE(-uavcan::ErrFailure, log.init()); // Expected one entry, none found - ASSERT_EQ(1, storage.getNumKeys()); - - storage.set("log0_term", "0"); - storage.set("log0_unique_id", "00000000000000000000000000000000"); - storage.set("log0_node_id", "0"); - ASSERT_LE(0, log.init()); // OK now - ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); - ASSERT_EQ(0, log.getLastIndex()); - ASSERT_EQ(0, log.getEntryAtIndex(0)->term); - } - // Nonempty storage, broken data - { - StorageBackend storage; - Log log(storage, tracer); - - storage.set("log_last_index", "foobar"); - ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value - - storage.set("log_last_index", "128"); - ASSERT_LE(-uavcan::ErrFailure, log.init()); // Bad value - - storage.set("log_last_index", "0"); - ASSERT_LE(-uavcan::ErrFailure, log.init()); // No log items - ASSERT_EQ(1, storage.getNumKeys()); - - storage.set("log0_term", "0"); - storage.set("log0_unique_id", "00000000000000000000000000000000"); - storage.set("log0_node_id", "128"); // Bad value (127 max) - ASSERT_LE(-uavcan::ErrFailure, log.init()); // Failed - ASSERT_EQ(0, log.getLastIndex()); - ASSERT_EQ(0, log.getEntryAtIndex(0)->term); - ASSERT_EQ(4, storage.getNumKeys()); - } - // Nonempty storage, many items - { - StorageBackend storage; - Log log(storage, tracer); - - storage.set("log_last_index", "1"); // 2 items - 0, 1 - storage.set("log0_term", "0"); - storage.set("log0_unique_id", "00000000000000000000000000000000"); - storage.set("log0_node_id", "0"); - storage.set("log1_term", "1"); - storage.set("log1_unique_id", "0123456789abcdef0123456789abcdef"); - storage.set("log1_node_id", "127"); - - ASSERT_LE(0, log.init()); // OK now - ASSERT_EQ(7, storage.getNumKeys()); - ASSERT_EQ(1, log.getLastIndex()); - - ASSERT_EQ(0, log.getEntryAtIndex(0)->term); - ASSERT_EQ(0, log.getEntryAtIndex(0)->node_id); - ASSERT_EQ(uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id(), - log.getEntryAtIndex(0)->unique_id); - - ASSERT_EQ(1, log.getEntryAtIndex(1)->term); - ASSERT_EQ(127, log.getEntryAtIndex(1)->node_id); - 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_EQ(uid, log.getEntryAtIndex(1)->unique_id); - } -} - - -TEST(DynamicNodeIDServer, LogAppend) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - EventTracer tracer; - StorageBackend storage; - Log log(storage, tracer); - - ASSERT_EQ(0, storage.getNumKeys()); - ASSERT_LE(0, log.init()); - storage.print(); - ASSERT_EQ(NumEntriesInStorageWithEmptyLog, storage.getNumKeys()); - - /* - * Entry at the index 0 always exists, and it's always zero-initialized. - */ - ASSERT_EQ("0", storage.get("log_last_index")); - ASSERT_EQ("0", storage.get("log0_term")); - ASSERT_EQ("00000000000000000000000000000000", storage.get("log0_unique_id")); - ASSERT_EQ("0", storage.get("log0_node_id")); - - /* - * Adding one entry to the log, making sure it appears in the storage - */ - uavcan::protocol::dynamic_node_id::server::Entry entry; - entry.term = 1; - entry.node_id = 1; - entry.unique_id[0] = 1; - ASSERT_LE(0, log.append(entry)); - - ASSERT_EQ("1", storage.get("log_last_index")); - ASSERT_EQ("1", storage.get("log1_term")); - ASSERT_EQ("01000000000000000000000000000000", storage.get("log1_unique_id")); - ASSERT_EQ("1", storage.get("log1_node_id")); - - ASSERT_EQ(1, log.getLastIndex()); - ASSERT_TRUE(entry == *log.getEntryAtIndex(1)); - - /* - * Adding another entry while storage is failing - */ - storage.failOnSetCalls(true); - - ASSERT_EQ(7, storage.getNumKeys()); - - entry.term = 2; - entry.node_id = 2; - entry.unique_id[0] = 2; - ASSERT_GT(0, log.append(entry)); - - ASSERT_EQ(7, storage.getNumKeys()); // No new entries, we failed - - ASSERT_EQ(1, log.getLastIndex()); - - /* - * Making sure append() fails when the log is full - */ - storage.failOnSetCalls(false); - - while (log.getLastIndex() < (log.Capacity - 1)) - { - ASSERT_LE(0, log.append(entry)); - ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); - - entry.term += 1; - entry.node_id = uint8_t(entry.node_id + 1U); - entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); - } - - ASSERT_GT(0, log.append(entry)); // Failing because full - - storage.print(); -} - - -TEST(DynamicNodeIDServer, LogRemove) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - EventTracer tracer; - StorageBackend storage; - Log log(storage, tracer); - - /* - * Filling the log fully - */ - uavcan::protocol::dynamic_node_id::server::Entry entry; - entry.term = 1; - entry.node_id = 1; - entry.unique_id[0] = 1; - - while (log.getLastIndex() < (log.Capacity - 1)) - { - ASSERT_LE(0, log.append(entry)); - ASSERT_TRUE(entry == *log.getEntryAtIndex(log.getLastIndex())); - - entry.term += 1; - entry.node_id = uint8_t(entry.node_id + 1U); - entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); - } - - /* - * Removal will fail as the storage is failing to update - */ - storage.failOnSetCalls(true); - - ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); - ASSERT_GT(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); // Failing - ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); - - /* - * Now removal must work - */ - storage.failOnSetCalls(false); - - ASSERT_EQ(log.Capacity - 1, log.getLastIndex()); - - ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(60)); - ASSERT_EQ(59, log.getLastIndex()); - ASSERT_EQ("59", storage.get("log_last_index")); - - ASSERT_LE(0, log.removeEntriesWhereIndexGreater(30)); - ASSERT_EQ(30, log.getLastIndex()); - ASSERT_EQ("30", storage.get("log_last_index")); - - ASSERT_LE(0, log.removeEntriesWhereIndexGreaterOrEqual(1)); - ASSERT_EQ(0, log.getLastIndex()); - ASSERT_EQ("0", storage.get("log_last_index")); - - storage.print(); -} - - -TEST(DynamicNodeIDServer, PersistentStorageInitialization) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - EventTracer tracer; - /* - * First initialization - */ - { - StorageBackend storage; - PersistentState pers(storage, tracer); - - ASSERT_EQ(0, storage.getNumKeys()); - ASSERT_LE(0, pers.init()); - - ASSERT_LE(3, storage.getNumKeys()); - ASSERT_EQ("0", storage.get("log_last_index")); - ASSERT_EQ("0", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - } - /* - * Partial recovery - only empty log is recovered - */ - { - StorageBackend storage; - - { - // This log is used to initialize the storage - Log log(storage, tracer); - ASSERT_LE(0, log.init()); - } - ASSERT_LE(1, storage.getNumKeys()); - - PersistentState pers(storage, tracer); - - ASSERT_LE(0, pers.init()); - - ASSERT_LE(3, storage.getNumKeys()); - ASSERT_EQ("0", storage.get("log_last_index")); - ASSERT_EQ("0", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - } - /* - * Partial recovery - log and current term are recovered - */ - { - StorageBackend storage; - - { - // This log is used to initialize the storage - Log log(storage, tracer); - ASSERT_LE(0, log.init()); - } - ASSERT_LE(1, storage.getNumKeys()); - - storage.set("current_term", "1"); - - PersistentState pers(storage, tracer); - - ASSERT_GT(0, pers.init()); // Fails because current term is not zero - - storage.set("current_term", "0"); - - ASSERT_LE(0, pers.init()); // OK now - - ASSERT_LE(3, storage.getNumKeys()); - ASSERT_EQ("0", storage.get("log_last_index")); - ASSERT_EQ("0", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - } - /* - * Full recovery - */ - { - StorageBackend storage; - - { - // This log is used to initialize the storage - Log log(storage, tracer); - ASSERT_LE(0, log.init()); - - uavcan::protocol::dynamic_node_id::server::Entry entry; - entry.term = 1; - entry.node_id = 1; - entry.unique_id[0] = 1; - ASSERT_LE(0, log.append(entry)); - } - ASSERT_LE(4, storage.getNumKeys()); - - PersistentState pers(storage, tracer); - - ASSERT_GT(0, pers.init()); // Fails because log is not empty - - storage.set("current_term", "0"); - storage.set("voted_for", "0"); - ASSERT_GT(0, pers.init()); // Fails because of bad currentTerm - - storage.set("current_term", "1"); // OK - storage.set("voted_for", "128"); // Invalid value - ASSERT_GT(0, pers.init()); // Fails because of bad votedFor - - storage.set("voted_for", "0"); // OK now - ASSERT_LE(0, pers.init()); - - ASSERT_LE(3, storage.getNumKeys()); - ASSERT_EQ("1", storage.get("log_last_index")); - ASSERT_EQ("1", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - } -} - - -TEST(DynamicNodeIDServer, PersistentStorage) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - EventTracer tracer; - StorageBackend storage; - PersistentState pers(storage, tracer); - - /* - * Initializing - */ - ASSERT_LE(0, pers.init()); - - ASSERT_EQ("0", storage.get("log_last_index")); - ASSERT_EQ("0", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - - /* - * Inserting some log entries - */ - uavcan::protocol::dynamic_node_id::server::Entry entry; - entry.term = 1; - entry.node_id = 1; - entry.unique_id[0] = 1; - ASSERT_LE(0, pers.getLog().append(entry)); - - ASSERT_EQ("1", storage.get("log_last_index")); - ASSERT_EQ("0", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - - /* - * Changing current term - */ - ASSERT_EQ(0, pers.getCurrentTerm()); - ASSERT_LE(0, pers.setCurrentTerm(2)); - ASSERT_EQ(2, pers.getCurrentTerm()); - - ASSERT_EQ("1", storage.get("log_last_index")); - ASSERT_EQ("2", storage.get("current_term")); - ASSERT_EQ("0", storage.get("voted_for")); - - /* - * Changing votedFor - */ - ASSERT_FALSE(pers.isVotedForSet()); - ASSERT_EQ(0, pers.getVotedFor().get()); - ASSERT_LE(0, pers.setVotedFor(0)); - ASSERT_EQ(0, pers.getVotedFor().get()); - ASSERT_LE(0, pers.setVotedFor(45)); - ASSERT_EQ(45, pers.getVotedFor().get()); - ASSERT_TRUE(pers.isVotedForSet()); - - ASSERT_EQ("1", storage.get("log_last_index")); - ASSERT_EQ("2", storage.get("current_term")); - ASSERT_EQ("45", storage.get("voted_for")); - - ASSERT_LE(0, pers.resetVotedFor()); - ASSERT_EQ(0, pers.getVotedFor().get()); - ASSERT_FALSE(pers.isVotedForSet()); - ASSERT_EQ("0", storage.get("voted_for")); - - ASSERT_LE(0, pers.setVotedFor(45)); - ASSERT_TRUE(pers.isVotedForSet()); - ASSERT_EQ("45", storage.get("voted_for")); - - /* - * Handling errors - */ - storage.failOnSetCalls(true); - - ASSERT_EQ(2, pers.getCurrentTerm()); - ASSERT_GT(0, pers.setCurrentTerm(7893)); - ASSERT_EQ(2, pers.getCurrentTerm()); - - ASSERT_EQ(45, pers.getVotedFor().get()); - ASSERT_GT(0, pers.setVotedFor(78)); - ASSERT_EQ(45, pers.getVotedFor().get()); - - ASSERT_EQ("1", storage.get("log_last_index")); - ASSERT_EQ("2", storage.get("current_term")); - ASSERT_EQ("45", storage.get("voted_for")); - - /* - * Final checks - */ - ASSERT_GT(10, storage.getNumKeys()); // Making sure there's some sane number of keys in the storage -} - - -TEST(DynamicNodeIDServer, ClusterManagerInitialization) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - const unsigned MaxClusterSize = - uavcan::protocol::dynamic_node_id::server::Discovery::FieldTypes::known_nodes::MaxSize; - - uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; - - EventTracer tracer; - - /* - * Simple initialization - */ - { - StorageBackend storage; - Log log(storage, tracer); - InterlinkedTestNodesWithSysClock nodes; - - ClusterManager mgr(nodes.a, storage, log, tracer); - - // Too big - ASSERT_GT(0, mgr.init(MaxClusterSize + 1)); - ASSERT_EQ(0, storage.getNumKeys()); - - // OK - ASSERT_LE(0, mgr.init(5)); - ASSERT_EQ(1, storage.getNumKeys()); - ASSERT_EQ("5", storage.get("cluster_size")); - - // Testing other states - ASSERT_EQ(0, mgr.getNumKnownServers()); - ASSERT_EQ(5, mgr.getClusterSize()); - ASSERT_EQ(3, mgr.getQuorumSize()); - ASSERT_FALSE(mgr.getRemoteServerNodeIDAtIndex(0).isValid()); - } - /* - * Recovery from the storage - */ - { - StorageBackend storage; - Log log(storage, tracer); - InterlinkedTestNodesWithSysClock nodes; - - ClusterManager mgr(nodes.a, storage, log, tracer); - - // Not configured - ASSERT_GT(0, mgr.init()); - ASSERT_EQ(0, storage.getNumKeys()); - - // OK - storage.set("cluster_size", "5"); - ASSERT_LE(0, mgr.init()); - ASSERT_EQ(1, storage.getNumKeys()); - } -} - - -TEST(DynamicNodeIDServer, ClusterManagerOneServer) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; - - EventTracer tracer; - StorageBackend storage; - Log log(storage, tracer); - InterlinkedTestNodesWithSysClock nodes; - - ClusterManager mgr(nodes.a, storage, log, tracer); - - /* - * Pub and sub - */ - SubscriberWithCollector sub(nodes.b); - uavcan::Publisher pub(nodes.b); - - ASSERT_LE(0, sub.start()); - ASSERT_LE(0, pub.init()); - - /* - * Starting - */ - ASSERT_LE(0, mgr.init(1)); - - ASSERT_EQ(0, mgr.getNumKnownServers()); - ASSERT_TRUE(mgr.isClusterDiscovered()); - - ASSERT_EQ(0, nodes.a.internal_failure_count); - - /* - * Broadcasting discovery with wrong cluster size, it will be reported as internal failure - */ - uavcan::protocol::dynamic_node_id::server::Discovery msg; - msg.configured_cluster_size = 2; - msg.known_nodes.push_back(2U); - ASSERT_LE(0, pub.broadcast(msg)); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); - - ASSERT_EQ(1, nodes.a.internal_failure_count); - - /* - * Discovery rate limiting test - */ - ASSERT_FALSE(sub.collector.msg.get()); - - msg = uavcan::protocol::dynamic_node_id::server::Discovery(); - msg.configured_cluster_size = 1; // Correct value - ASSERT_LE(0, pub.broadcast(msg)); // List of known nodes is empty, intentionally - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); - ASSERT_FALSE(sub.collector.msg.get()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); - ASSERT_TRUE(sub.collector.msg.get()); - ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); - ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); - ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); - sub.collector.msg.reset(); - - // Rinse repeat - ASSERT_LE(0, pub.broadcast(msg)); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); - ASSERT_FALSE(sub.collector.msg.get()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); - ASSERT_TRUE(sub.collector.msg.get()); - ASSERT_EQ(1, sub.collector.msg->configured_cluster_size); - ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); - ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); - sub.collector.msg.reset(); -} - - -TEST(DynamicNodeIDServer, ClusterManagerThreeServers) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - - uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; - - EventTracer tracer; - StorageBackend storage; - Log log(storage, tracer); - InterlinkedTestNodesWithSysClock nodes; - - ClusterManager mgr(nodes.a, storage, log, tracer); - - /* - * Pub and sub - */ - SubscriberWithCollector sub(nodes.b); - uavcan::Publisher pub(nodes.b); - - ASSERT_LE(0, sub.start()); - ASSERT_LE(0, pub.init()); - - /* - * Starting - */ - ASSERT_LE(0, mgr.init(3)); - - ASSERT_EQ(0, mgr.getNumKnownServers()); - ASSERT_FALSE(mgr.isClusterDiscovered()); - - /* - * Discovery publishing rate check - */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); - ASSERT_FALSE(sub.collector.msg.get()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); - ASSERT_TRUE(sub.collector.msg.get()); - ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); - ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); - ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); - sub.collector.msg.reset(); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); - ASSERT_FALSE(sub.collector.msg.get()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); - ASSERT_TRUE(sub.collector.msg.get()); - ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); - ASSERT_EQ(1, sub.collector.msg->known_nodes.size()); - ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); - sub.collector.msg.reset(); - - /* - * Discovering other nodes - */ - uavcan::protocol::dynamic_node_id::server::Discovery msg; - msg.configured_cluster_size = 3; - msg.known_nodes.push_back(2U); - ASSERT_LE(0, pub.broadcast(msg)); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); - ASSERT_TRUE(sub.collector.msg.get()); - ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); - ASSERT_EQ(2, sub.collector.msg->known_nodes.size()); - ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); - ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); - sub.collector.msg.reset(); - - ASSERT_FALSE(mgr.isClusterDiscovered()); - - // This will complete the discovery - msg.known_nodes.push_back(127U); - ASSERT_LE(0, pub.broadcast(msg)); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1050)); - ASSERT_TRUE(sub.collector.msg.get()); - ASSERT_EQ(3, sub.collector.msg->configured_cluster_size); - ASSERT_EQ(3, sub.collector.msg->known_nodes.size()); - ASSERT_EQ(1, sub.collector.msg->known_nodes[0]); - ASSERT_EQ(2, sub.collector.msg->known_nodes[1]); - ASSERT_EQ(127, sub.collector.msg->known_nodes[2]); - sub.collector.msg.reset(); - - // Making sure discovery is now terminated - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); - ASSERT_FALSE(sub.collector.msg.get()); - - /* - * Checking Raft states - */ - ASSERT_EQ(uavcan::NodeID(2), mgr.getRemoteServerNodeIDAtIndex(0)); - ASSERT_EQ(uavcan::NodeID(127), mgr.getRemoteServerNodeIDAtIndex(1)); - ASSERT_EQ(uavcan::NodeID(), mgr.getRemoteServerNodeIDAtIndex(2)); - - ASSERT_EQ(0, mgr.getServerMatchIndex(2)); - ASSERT_EQ(0, mgr.getServerMatchIndex(127)); - - ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); - ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); - - mgr.setServerMatchIndex(2, 10); - ASSERT_EQ(10, mgr.getServerMatchIndex(2)); - - mgr.incrementServerNextIndexBy(2, 5); - ASSERT_EQ(log.getLastIndex() + 1 + 5, mgr.getServerNextIndex(2)); - mgr.decrementServerNextIndex(2); - ASSERT_EQ(log.getLastIndex() + 1 + 5 - 1, mgr.getServerNextIndex(2)); - - mgr.resetAllServerIndices(); - - ASSERT_EQ(0, mgr.getServerMatchIndex(2)); - ASSERT_EQ(0, mgr.getServerMatchIndex(127)); - - ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(2)); - ASSERT_EQ(log.getLastIndex() + 1, mgr.getServerNextIndex(127)); -} - - -TEST(DynamicNodeIDServer, RaftCoreBasic) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - using namespace uavcan::protocol::dynamic_node_id::server; - - uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; - uavcan::DefaultDataTypeRegistrator _reg2; - uavcan::DefaultDataTypeRegistrator _reg3; - - EventTracer tracer_a("a"); - EventTracer tracer_b("b"); - StorageBackend storage_a; - StorageBackend storage_b; - CommitHandler commit_handler_a("a"); - CommitHandler commit_handler_b("b"); - - InterlinkedTestNodesWithSysClock nodes; - - std::auto_ptr raft_a(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); - std::auto_ptr raft_b(new RaftCore(nodes.b, storage_b, tracer_b, commit_handler_b)); - - /* - * Initialization - */ - ASSERT_LE(0, raft_a->init(2)); - ASSERT_LE(0, raft_b->init(2)); - - /* - * Running and trying not to fall - */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); - - // The one with lower node ID must become a leader - ASSERT_TRUE(raft_a->isLeader()); - ASSERT_FALSE(raft_b->isLeader()); - - ASSERT_EQ(0, raft_a->getCommitIndex()); - ASSERT_EQ(0, raft_b->getCommitIndex()); - - /* - * Adding some stuff - */ - Entry::FieldTypes::unique_id unique_id; - uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); - - ASSERT_LE(0, raft_a->appendLog(unique_id, uavcan::NodeID(1))); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); - - ASSERT_EQ(1, raft_a->getCommitIndex()); - ASSERT_EQ(1, raft_b->getCommitIndex()); - - /* - * Terminating the leader - the Follower will continue to sleep - */ - raft_a.reset(); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); - - /* - * Reinitializing the leader - current Follower will become the new Leader - */ - storage_a.reset(); - - raft_a.reset(new RaftCore(nodes.a, storage_a, tracer_a, commit_handler_a)); - ASSERT_LE(0, raft_a->init(2)); - ASSERT_EQ(0, raft_a->getCommitIndex()); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); - - ASSERT_FALSE(raft_a->isLeader()); - ASSERT_TRUE(raft_b->isLeader()); - - ASSERT_EQ(1, raft_a->getCommitIndex()); - ASSERT_EQ(1, raft_b->getCommitIndex()); -} - - -TEST(DynamicNodeIDServer, EventCodeToString) -{ - using namespace uavcan::dynamic_node_id_server::distributed; - using namespace uavcan::dynamic_node_id_server; - - // 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("DiscoveryReceived", IEventTracer::getEventName(TraceDiscoveryReceived)); -} - - -TEST(DynamicNodeIDServer, AllocationRequestManager) -{ - using namespace uavcan::protocol::dynamic_node_id; - using namespace uavcan::protocol::dynamic_node_id::server; - using namespace uavcan::dynamic_node_id_server; - - uavcan::GlobalDataTypeRegistry::instance().reset(); - uavcan::DefaultDataTypeRegistrator _reg1; - - // Node A is Allocator, Node B is Allocatee - InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); - - uavcan::DynamicNodeIDClient client(nodes.b); - - /* - * Client initialization - */ - 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)); - - /* - * Request manager initialization - */ - AllocationRequestHandler handler; - - AllocationRequestManager manager(nodes.a, handler); - - ASSERT_LE(0, manager.init()); - - ASSERT_FALSE(manager.isActive()); - manager.setActive(true); - ASSERT_TRUE(manager.isActive()); - - /* - * Allocation - */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); - - ASSERT_TRUE(handler.matchAndPopLastRequest(hwver.unique_id, PreferredNodeID)); - - ASSERT_LE(0, manager.broadcastAllocationResponse(hwver.unique_id, PreferredNodeID)); - - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); - - /* - * Checking the client - */ - ASSERT_TRUE(client.isAllocationComplete()); - - ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); -} - - -TEST(DynamicNodeIDServer, Main) -{ - 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 _reg1; - uavcan::DefaultDataTypeRegistrator _reg2; - uavcan::DefaultDataTypeRegistrator _reg3; - uavcan::DefaultDataTypeRegistrator _reg4; - - EventTracer tracer; - StorageBackend storage; - - // Node A is Allocator, Node B is Allocatee - InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); - - /* - * Server - */ - distributed::Server server(nodes.a, storage, tracer); - - ASSERT_LE(0, server.init(1)); - - /* - * 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(4000)); - - ASSERT_TRUE(client.isAllocationComplete()); - ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); -} - - -TEST(DynamicNodeIDServer, ObjectSizes) -{ - using namespace uavcan::dynamic_node_id_server; - - std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl; - std::cout << "distributed::PersistentState: " << sizeof(distributed::PersistentState) << std::endl; - std::cout << "distributed::ClusterManager: " << sizeof(distributed::ClusterManager) << std::endl; - std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl; - std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl; - std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl; -} diff --git a/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp b/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp new file mode 100644 index 0000000000..53be558259 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/storage_marshaller.cpp @@ -0,0 +1,88 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "memory_storage_backend.hpp" + +TEST(dynamic_node_id_server_StorageMarshaller, Basic) +{ + MemoryStorageBackend st; + + uavcan::dynamic_node_id_server::StorageMarshaller marshaler(st); + + uavcan::dynamic_node_id_server::IStorageBackend::String key; + + /* + * uint32 + */ + uint32_t u32 = 0; + + key = "foo"; + u32 = 0; + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0, u32); + + key = "bar"; + u32 = 0xFFFFFFFF; + ASSERT_LE(0, marshaler.setAndGetBack(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + ASSERT_LE(0, marshaler.get(key, u32)); + ASSERT_EQ(0xFFFFFFFF, u32); + + key = "foo"; + ASSERT_LE(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + key = "the_cake_is_a_lie"; + ASSERT_GT(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + /* + * uint8[16] + */ + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id array; + + key = "a"; + // Set zero + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(0, array[i]); + } + + // Make sure this will not be interpreted as uint32 + ASSERT_GT(0, marshaler.get(key, u32)); + ASSERT_EQ(0, u32); + + // Set pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i + 1); + } + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(i + 1, array[i]); + } + + // Set another pattern + for (uint8_t i = 0; i < 16; i++) + { + array[i] = uint8_t(i | (i << 4)); + } + ASSERT_LE(0, marshaler.setAndGetBack(key, array)); + for (uint8_t i = 0; i < 16; i++) + { + ASSERT_EQ(uint8_t(i | (i << 4)), array[i]); + } + + // Make sure uint32 cannot be interpreted as an array + key = "foo"; + ASSERT_GT(0, marshaler.get(key, array)); + + // Nonexistent key + key = "the_cake_is_a_lie"; + ASSERT_GT(0, marshaler.get(key, array)); +} From d980b5e655eb69f1d56bb7eebbc530cf422c492d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 17:04:14 +0300 Subject: [PATCH 059/268] Typo in UAVCAN_TRACE() --- .../dynamic_node_id_server/distributed/server.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index b15dc9df39..23668c12dc 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -66,7 +66,8 @@ class Server : private IAllocationRequestHandler */ bool isNodeIDTaken(const NodeID node_id) const { - UAVCAN_TRACE("DynamicNodeIDServer", "Testing if node ID %d is taken", int(node_id.get())); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Testing if node ID %d is taken", int(node_id.get())); return raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); } @@ -76,11 +77,12 @@ class Server : private IAllocationRequestHandler NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); if (!allocated_node_id.isUnicast()) { - UAVCAN_TRACE("DynamicNodeIDServer", "Request ignored - no free node ID left"); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left"); return; } - UAVCAN_TRACE("DynamicNodeIDServer", "New node ID allocated: %d", int(allocated_node_id.get())); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d", + int(allocated_node_id.get())); const int res = raft_core_.appendLog(unique_id, allocated_node_id); if (res < 0) { @@ -100,13 +102,13 @@ class Server : private IAllocationRequestHandler if (result->committed) { tryPublishAllocationResult(result->entry); - UAVCAN_TRACE("DynamicNodeIDServer", + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Allocation request served with existing allocation; node ID %d", int(result->entry.node_id)); } else { - UAVCAN_TRACE("DynamicNodeIDServer", + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Allocation request ignored - allocation exists but not committed yet; node ID %d", int(result->entry.node_id)); } @@ -124,7 +126,7 @@ class Server : private IAllocationRequestHandler virtual void onLeaderChange(bool local_node_is_leader) { - UAVCAN_TRACE("DynamicNodeIDServer", "I am leader: %d", int(local_node_is_leader)); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "I am leader: %d", int(local_node_is_leader)); allocation_request_manager_.setActive(local_node_is_leader); } From ab0017f870ae71484ae54f5aa906092bf7741e04 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 23:06:58 +0300 Subject: [PATCH 060/268] Top level header for distributed server --- .../protocol/dynamic_node_id_server/distributed.hpp | 10 ++++++++++ .../dynamic_node_id_server/distributed/server.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp new file mode 100644 index 0000000000..eef49ccba1 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp @@ -0,0 +1,10 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED + +#include + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index b5e6ed6068..38563e87be 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include "event_tracer.hpp" #include "../../helpers.hpp" From 68c36924e3b0ba93980a502f6c6e0ba30b7175d1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 23:17:22 +0300 Subject: [PATCH 061/268] Better naming --- .../protocol/dynamic_node_id_server/distributed/raft_core.hpp | 4 ++-- .../protocol/dynamic_node_id_server/distributed/server.hpp | 2 +- .../protocol/dynamic_node_id_server/distributed/server.cpp | 4 ++-- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index dcf4c534e1..af2c1e83e6 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -34,7 +34,7 @@ public: /** * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). */ - virtual void onEntryCommitted(const Entry& entry) = 0; + virtual void onEntryCommit(const Entry& entry) = 0; /** * Assume false by default. @@ -373,7 +373,7 @@ class RaftCore : private TimerBase trace(TraceRaftNewEntryCommitted, commit_index_); // AT THIS POINT ALLOCATION IS COMPLETE - log_commit_handler_.onEntryCommitted(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); + log_commit_handler_.onEntryCommit(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); } } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 23668c12dc..c13a0f5224 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -119,7 +119,7 @@ class Server : private IAllocationRequestHandler } } - virtual void onEntryCommitted(const protocol::dynamic_node_id::server::Entry& entry) + virtual void onEntryCommit(const protocol::dynamic_node_id::server::Entry& entry) { tryPublishAllocationResult(entry); } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 38563e87be..c0f3e2f820 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -22,14 +22,14 @@ class CommitHandler : public uavcan::dynamic_node_id_server::distributed::ILeade { const std::string id_; - virtual void onEntryCommitted(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + virtual void onEntryCommit(const uavcan::protocol::dynamic_node_id::server::Entry& entry) { std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; } virtual void onLeaderChange(bool local_node_is_leader) { - std::cout << "I AM LEADER: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; } public: From 1a763a824cbb0dd48ecaaa74d25698d827183757 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 23:33:27 +0300 Subject: [PATCH 062/268] Better naming --- .../distributed/raft_core.hpp | 18 +++++++++--------- .../distributed/server.hpp | 6 +++--- .../distributed/server.cpp | 6 +++--- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index af2c1e83e6..cd79f352c4 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -28,20 +28,20 @@ namespace distributed /** * Allocator has to implement this interface so the RaftCore can inform it when a new entry gets committed to the log. */ -class ILeaderLogCommitHandler +class IRaftLeaderMonitor { public: /** * This method will be invoked when a new log entry is committed (only if the local server is the current Leader). */ - virtual void onEntryCommit(const Entry& entry) = 0; + virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0; /** * Assume false by default. */ - virtual void onLeaderChange(bool local_node_is_leader) = 0; + virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0; - virtual ~ILeaderLogCommitHandler() { } + virtual ~IRaftLeaderMonitor() { } }; /** @@ -90,7 +90,7 @@ class RaftCore : private TimerBase const MonotonicDuration base_activity_timeout_; IEventTracer& tracer_; - ILeaderLogCommitHandler& log_commit_handler_; + IRaftLeaderMonitor& leader_monitor_; /* * States @@ -284,7 +284,7 @@ class RaftCore : private TimerBase if ((ServerStateLeader == server_state_) || (ServerStateLeader == new_state)) { - log_commit_handler_.onLeaderChange(ServerStateLeader == new_state); + leader_monitor_.handleLocalLeadershipChange(ServerStateLeader == new_state); } server_state_ = new_state; @@ -373,7 +373,7 @@ class RaftCore : private TimerBase trace(TraceRaftNewEntryCommitted, commit_index_); // AT THIS POINT ALLOCATION IS COMPLETE - log_commit_handler_.onEntryCommit(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); + leader_monitor_.handleLogCommitOnLeader(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); } } } @@ -677,7 +677,7 @@ public: RaftCore(INode& node, IStorageBackend& storage, IEventTracer& tracer, - ILeaderLogCommitHandler& log_commit_handler, + IRaftLeaderMonitor& leader_monitor, MonotonicDuration update_interval = MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_REQUEST_TIMEOUT_MS), MonotonicDuration base_activity_timeout = @@ -686,7 +686,7 @@ public: , update_interval_(update_interval) , base_activity_timeout_(base_activity_timeout) , tracer_(tracer) - , log_commit_handler_(log_commit_handler) + , 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 diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index c13a0f5224..a3f1e225e9 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -24,7 +24,7 @@ namespace distributed * This class implements the top-level allocation logic and server API. */ class Server : private IAllocationRequestHandler - , private ILeaderLogCommitHandler + , private IRaftLeaderMonitor { struct UniqueIDLogPredicate { @@ -119,12 +119,12 @@ class Server : private IAllocationRequestHandler } } - virtual void onEntryCommit(const protocol::dynamic_node_id::server::Entry& entry) + virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry) { tryPublishAllocationResult(entry); } - virtual void onLeaderChange(bool local_node_is_leader) + virtual void handleLocalLeadershipChange(bool local_node_is_leader) { UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "I am leader: %d", int(local_node_is_leader)); allocation_request_manager_.setActive(local_node_is_leader); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index c0f3e2f820..eb93787b51 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -18,16 +18,16 @@ using uavcan::dynamic_node_id_server::UniqueID; -class CommitHandler : public uavcan::dynamic_node_id_server::distributed::ILeaderLogCommitHandler +class CommitHandler : public uavcan::dynamic_node_id_server::distributed::IRaftLeaderMonitor { const std::string id_; - virtual void onEntryCommit(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + virtual void handleLogCommitOnLeader(const uavcan::protocol::dynamic_node_id::server::Entry& entry) { std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; } - virtual void onLeaderChange(bool local_node_is_leader) + virtual void handleLocalLeadershipChange(bool local_node_is_leader) { std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; } From 67b33a712e78bc4dc65bd8f95958afbee2e84bab Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 9 May 2015 23:52:51 +0300 Subject: [PATCH 063/268] Improved logic of allocation request manager --- .../dynamic_node_id/1010.Allocation.uavcan | 5 +++++ .../allocation_request_manager.hpp | 15 +++++++++------ 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan index c4ecd05bf2..4793ac71f5 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/1010.Allocation.uavcan @@ -39,6 +39,11 @@ # uint16 DEFAULT_REQUEST_PERIOD_MS = 1000 +# +# Server will reset its state if there was no follow-up request in this amount of time. +# +uint16 FOLLOWUP_TIMEOUT_MS = 500 + # # Any request message can accommodate no more than this number of bytes of unique ID. # This limitation is needed to ensure that all request transfers are single-frame. diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 795bbae43f..65cd62a0fb 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -133,7 +133,6 @@ class AllocationRequestManager UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); current_unique_id_.clear(); } - last_message_timestamp_ = msg.getMonotonicTimestamp(); /* * Checking if request stage matches the expected stage @@ -141,26 +140,25 @@ class AllocationRequestManager const uint8_t request_stage = detectRequestStage(msg); if (request_stage == InvalidStage) { - return; // No way + return; // Malformed request - ignore without resetting } const uint8_t expected_stage = getExpectedStage(); if (request_stage == InvalidStage) { - current_unique_id_.clear(); return; } if (request_stage != expected_stage) { - return; // Ignore - stage mismatch + return; // Ignore - stage mismatch } const uint8_t max_expected_bytes = static_cast(current_unique_id_.capacity() - current_unique_id_.size()); UAVCAN_ASSERT(max_expected_bytes > 0); if (msg.unique_id.size() > max_expected_bytes) { - return; // Malformed request + return; // Malformed request } /* @@ -189,11 +187,16 @@ class AllocationRequestManager { broadcastIntermediateAllocationResponse(); } + + /* + * It is super important to update timestamp only if the request has been processed successfully. + */ + last_message_timestamp_ = msg.getMonotonicTimestamp(); } public: AllocationRequestManager(INode& node, IAllocationRequestHandler& handler) - : stage_timeout_(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS)) + : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)) , active_(false) , handler_(handler) , allocation_sub_(node) From aae4317beb9ba2660b69987199b95d89f5c6260c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 14:12:19 +0300 Subject: [PATCH 064/268] Improved allocation logic --- .../allocation_request_manager.hpp | 46 +++---- .../distributed/raft_core.hpp | 16 +-- .../distributed/server.hpp | 122 ++++++++++++------ .../allocation_request_manager.cpp | 14 +- 4 files changed, 113 insertions(+), 85 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 65cd62a0fb..30fc0915e4 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -24,6 +24,14 @@ namespace dynamic_node_id_server class IAllocationRequestHandler { public: + /** + * Allocation request manager uses this method to detect if it is allowed to publish follow-up responses. + */ + virtual bool canPublishFollowupAllocationResponse() const = 0; + + /** + * This method will be invoked when a new allocation request is received. + */ virtual void handleAllocationRequest(const UniqueID& unique_id, NodeID preferred_node_id) = 0; virtual ~IAllocationRequestHandler() { } @@ -42,7 +50,6 @@ class AllocationRequestManager const MonotonicDuration stage_timeout_; - bool active_; MonotonicTime last_message_timestamp_; protocol::dynamic_node_id::Allocation::FieldTypes::unique_id current_unique_id_; @@ -95,10 +102,8 @@ class AllocationRequestManager return InvalidStage; } - void broadcastIntermediateAllocationResponse() + void publishFollowupAllocationResponse() { - UAVCAN_ASSERT(active_); - protocol::dynamic_node_id::Allocation msg; msg.unique_id = current_unique_id_; UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); @@ -120,11 +125,6 @@ class AllocationRequestManager return; // This is a response from another allocator, ignore } - if (!active_) - { - return; // The local node is not the leader, ignore - } - /* * Reset the expected stage on timeout */ @@ -171,6 +171,7 @@ class AllocationRequestManager /* * Proceeding with allocation if possible + * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader. */ if (current_unique_id_.size() == current_unique_id_.capacity()) { @@ -185,7 +186,14 @@ class AllocationRequestManager } else { - broadcastIntermediateAllocationResponse(); + if (handler_.canPublishFollowupAllocationResponse()) + { + publishFollowupAllocationResponse(); + } + else + { + current_unique_id_.clear(); + } } /* @@ -197,7 +205,6 @@ class AllocationRequestManager public: AllocationRequestManager(INode& node, IAllocationRequestHandler& handler) : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)) - , active_(false) , handler_(handler) , allocation_sub_(node) , allocation_pub_(node) @@ -222,25 +229,8 @@ public: return 0; } - void setActive(bool x) - { - active_ = x; - if (!active_) - { - current_unique_id_.clear(); - } - } - - bool isActive() const { return active_; } - int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id) { - if (!active_) - { - UAVCAN_ASSERT(0); - return -ErrLogic; - } - protocol::dynamic_node_id::Allocation msg; msg.unique_id.resize(msg.unique_id.capacity()); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index cd79f352c4..b83f9621ee 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -36,11 +36,6 @@ public: */ virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0; - /** - * Assume false by default. - */ - virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0; - virtual ~IRaftLeaderMonitor() { } }; @@ -281,12 +276,6 @@ class RaftCore : private TimerBase int(server_state_), int(new_state)); trace(TraceRaftStateSwitch, new_state); - if ((ServerStateLeader == server_state_) || - (ServerStateLeader == new_state)) - { - leader_monitor_.handleLocalLeadershipChange(ServerStateLeader == new_state); - } - server_state_ = new_state; cluster_.resetAllServerIndices(); @@ -782,6 +771,11 @@ public: */ Log::Index getCommitIndex() const { return commit_index_; } + /** + * This essentially indicates whether the server could replicate log since last allocation. + */ + bool areAllLogEntriesCommitted() const { return commit_index_ == persistent_state_.getLog().getLastIndex(); } + /** * Only the leader can call @ref appendLog(). */ diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index a3f1e225e9..cf28299cec 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -62,7 +62,85 @@ class Server : private IAllocationRequestHandler AllocationRequestManager allocation_request_manager_; /* - * Methods + * Methods of IAllocationRequestHandler + */ + virtual bool canPublishFollowupAllocationResponse() const + { + /* + * The server is allowed to publish follow-up allocation responses only if both conditions are met: + * - The server is leader. + * - The last allocation request has been completed successfully. + * + * Why second condition? Imagine a case when there's two Raft nodes that don't hear each other - A and B, + * both of them are leaders (but only A can commit to the log, B is in a minor partition); then there's a + * client X that can exchange with both leaders, and a client Y that can exchange only with A. Such a + * situation can occur in case of a very unlikely failure of redundant interfaces. + * + * Both clients X and Y initially send a first-stage Allocation request; A responds to Y with a first-stage + * response, whereas B responds to X. Both X and Y will issue a follow-up second-stage requests, which may + * cause A to mix second-stage Allocation requests from different nodes, leading to reception of an invalid + * unique ID. When both leaders receive full unique IDs (A will receive an invalid one, B will receive a valid + * unique ID of X), only A will be able to make a commit, because B is in a minority. Since both clients were + * unable to receive node ID values in this round, they will try again later. + * + * Now, in order to prevent B from disrupting client-server communication second time around, we introduce this + * second restriction: the server cannot exchange with clients as long as its log contains uncommitted entries. + * + * Note that this restriction does not apply to allocation requests sent via CAN FD frames, as in this case + * no follow-up responses are necessary. So only CAN FD can offer reliable Allocation exchange. + */ + return raft_core_.isLeader() && raft_core_.areAllLogEntriesCommitted(); + } + + virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) + { + /* + * Note that it is possible that the local node is not leader. We will still perform the log search + * and try to find the node that requested allocation. If the node is found, response will be sent; + * otherwise the request will be ignored because only leader can add new allocations. + */ + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id)); + + if (result.isConstructed()) + { + if (result->committed) + { + tryPublishAllocationResult(result->entry); + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Allocation request served with existing allocation; node ID %d", + int(result->entry.node_id)); + } + else + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", + "Allocation request ignored - allocation exists but not committed yet; node ID %d", + int(result->entry.node_id)); + } + } + else + { + // TODO: new allocations can be added only if the list of unidentified nodes is not empty + if (raft_core_.isLeader()) + { + allocateNewNode(unique_id, preferred_node_id); + } + } + } + + /* + * Methods of IRaftLeaderMonitor + */ + virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry) + { + /* + * Maybe this node did not request allocation at all, we don't care, we publish anyway. + */ + tryPublishAllocationResult(entry); + } + + /* + * Private methods */ bool isNodeIDTaken(const NodeID node_id) const { @@ -86,50 +164,10 @@ class Server : private IAllocationRequestHandler const int res = raft_core_.appendLog(unique_id, allocated_node_id); if (res < 0) { - node_.registerInternalFailure("Raft log append"); + node_.registerInternalFailure("Raft log append new allocation"); } } - virtual void handleAllocationRequest(const UniqueID& unique_id, const NodeID preferred_node_id) - { - // TODO: allocation requests must not be served if the list of unidentified nodes is not empty - - const LazyConstructor result = - raft_core_.traverseLogFromEndUntil(UniqueIDLogPredicate(unique_id)); - - if (result.isConstructed()) - { - if (result->committed) - { - tryPublishAllocationResult(result->entry); - UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", - "Allocation request served with existing allocation; node ID %d", - int(result->entry.node_id)); - } - else - { - UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", - "Allocation request ignored - allocation exists but not committed yet; node ID %d", - int(result->entry.node_id)); - } - } - else - { - allocateNewNode(unique_id, preferred_node_id); - } - } - - virtual void handleLogCommitOnLeader(const protocol::dynamic_node_id::server::Entry& entry) - { - tryPublishAllocationResult(entry); - } - - virtual void handleLocalLeadershipChange(bool local_node_is_leader) - { - UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "I am leader: %d", int(local_node_is_leader)); - allocation_request_manager_.setActive(local_node_is_leader); - } - void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) { const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp index a11969b088..edf82906e4 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -15,11 +15,20 @@ class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocat std::vector > requests_; public: + bool can_followup; + + AllocationRequestHandler() : can_followup(false) { } + virtual void handleAllocationRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) { requests_.push_back(std::pair(unique_id, preferred_node_id)); } + virtual bool canPublishFollowupAllocationResponse() const + { + return can_followup; + } + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) { if (requests_.empty()) @@ -80,15 +89,12 @@ TEST(dynamic_node_id_server_AllocationRequestManager, Basic) * Request manager initialization */ AllocationRequestHandler handler; + handler.can_followup = true; AllocationRequestManager manager(nodes.a, handler); ASSERT_LE(0, manager.init()); - ASSERT_FALSE(manager.isActive()); - manager.setActive(true); - ASSERT_TRUE(manager.isActive()); - /* * Allocation */ From 7477de3bd9c7e9ceda8a4672ff90c9380b22eae3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 17:44:34 +0300 Subject: [PATCH 065/268] NodeDiscoverer - implemented, integrated, not tested --- .../distributed/server.hpp | 59 +++- .../node_discoverer.hpp | 320 ++++++++++++++++++ .../distributed/server.cpp | 2 + 3 files changed, 376 insertions(+), 5 deletions(-) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index cf28299cec..d61b1f761e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -13,6 +13,7 @@ #include #include #include +#include namespace uavcan { @@ -23,8 +24,9 @@ namespace distributed /** * This class implements the top-level allocation logic and server API. */ -class Server : private IAllocationRequestHandler - , private IRaftLeaderMonitor +class Server : IAllocationRequestHandler + , INodeDiscoveryHandler + , IRaftLeaderMonitor { struct UniqueIDLogPredicate { @@ -60,6 +62,7 @@ class Server : private IAllocationRequestHandler INode& node_; RaftCore raft_core_; AllocationRequestManager allocation_request_manager_; + NodeDiscoverer node_discoverer_; /* * Methods of IAllocationRequestHandler @@ -120,14 +123,55 @@ class Server : private IAllocationRequestHandler } else { - // TODO: new allocations can be added only if the list of unidentified nodes is not empty - if (raft_core_.isLeader()) + if (raft_core_.isLeader() && !node_discoverer_.hasUnknownNodes()) { allocateNewNode(unique_id, preferred_node_id); } } } + /* + * Methods of INodeDiscoveryHandler + */ + virtual bool canDiscoverNewNodes() const + { + return raft_core_.isLeader(); + } + + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const + { + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)); + if (result.isConstructed()) + { + return result->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted; + } + else + { + return NodeAwarenessUnknown; + } + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) + { + if (raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_id)).isConstructed()) + { + UAVCAN_ASSERT(0); // Such node is already known, the class that called this method should have known that + return; + } + + const UniqueID uid = (unique_id_or_null == NULL) ? UniqueID() : *unique_id_or_null; + + if (raft_core_.isLeader()) + { + const int res = raft_core_.appendLog(uid, node_id); + if (res < 0) + { + node_.registerInternalFailure("Raft log append discovered node"); + } + } + } + /* * Methods of IRaftLeaderMonitor */ @@ -184,6 +228,7 @@ public: : node_(node) , raft_core_(node, storage, tracer, *this) , allocation_request_manager_(node, *this) + , node_discoverer_(node, *this) { } int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) @@ -200,7 +245,11 @@ public: return res; } - // TODO Initialize the node info transport + res = node_discoverer_.init(); + if (res < 0) + { + return res; + } return 0; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp new file mode 100644 index 0000000000..5483795308 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -0,0 +1,320 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_NODE_DISCOVERER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * The allocator must implement this interface. + */ +class INodeDiscoveryHandler +{ +public: + /** + * In order to avoid bus congestion, normally only leader can discover new nodes. + */ + virtual bool canDiscoverNewNodes() const = 0; + + /** + * These values represent the level of awareness of a certain node by the server. + */ + enum NodeAwareness + { + NodeAwarenessUnknown, + NodeAwarenessKnownButNotCommitted, + NodeAwarenessKnownAndCommitted + }; + + /** + * It is OK to do full log traversal in this method, because the unique ID collector will cache the + * result when possible. + */ + virtual NodeAwareness checkNodeAwareness(NodeID node_id) const = 0; + + /** + * This method will be called when a new node responds to GetNodeInfo request. + * If this method fails to register the node, the node will be queried again later and this method will be + * invoked again. + * Unique ID will be NULL if the node is assumed to not implement the GetNodeInfo service. + */ + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) = 0; + + virtual ~INodeDiscoveryHandler() { } +}; + +/** + * This class listens to NodeStatus messages from other nodes and retrieves their unique ID if they are not + * known to the allocator. + */ +class NodeDiscoverer : TimerBase +{ + typedef MethodBinder&)> + GetNodeInfoResponseCallback; + + typedef MethodBinder&)> + NodeStatusCallback; + + struct NodeData + { + uint32_t last_seen_uptime; + uint8_t num_get_node_info_attempts; + + NodeData() + : last_seen_uptime(0) + , num_get_node_info_attempts(0) + { } + }; + + typedef Map NodeMap; + + /** + * When this number of attempts has been made, the discoverer will give up and assume that the node + * does not implement this service. + */ + enum { MaxAttemptsToGetNodeInfo = 3 }; + + /* + * States + */ + INodeDiscoveryHandler& handler_; + + BitSet committed_node_mask_; ///< Nodes that are marked will not be queried + NodeMap node_map_; ///< Will not work in UAVCAN_TINY + + ServiceClient get_node_info_client_; + Subscriber node_status_sub_; + + /* + * Methods + */ + INode& getNode() { return node_status_sub_.getNode(); } + + 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(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; + } + + bool needToQuery(NodeID node_id) + { + UAVCAN_ASSERT(node_id.isUnicast()); + + /* + * Fast check + */ + if (committed_node_mask_[node_id.get()]) + { + return false; + } + + /* + * Slow check - may involve full log search + */ + const INodeDiscoveryHandler::NodeAwareness awareness = handler_.checkNodeAwareness(node_id); + + if (awareness == INodeDiscoveryHandler::NodeAwarenessUnknown) + { + return true; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted) + { + node_map_.remove(node_id); + return false; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted) + { + committed_node_mask_[node_id.get()] = true; + node_map_.remove(node_id); + return false; + } + else + { + UAVCAN_ASSERT(0); + return false; + } + } + + void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) + { + node_map_.remove(node_id); + if (needToQuery(node_id)) // Making sure the server is still interested + { + handler_.handleNewNodeDiscovery(unique_id_or_null, node_id); + } + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + 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); + } + else + { + NodeData* const data = node_map_.access(result.server_node_id); + if (data == NULL) + { + return; // Probably it is a known node now + } + + 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)); + data->num_get_node_info_attempts++; + if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo) + { + finalizeNodeDiscovery(NULL, result.server_node_id); + } + } + } + + void handleTimerEvent(const TimerEvent&) + { + if (get_node_info_client_.isPending()) + { + return; + } + + if (!handler_.canDiscoverNewNodes()) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer stopped - server disallows discovery"); + stop(); + return; + } + + const NodeID node_id = pickNextNodeToQuery(); + if (!node_id.isUnicast()) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer stopped - no unknown nodes left"); + stop(); + return; + } + + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d", + int(node_id.get())); + const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request()); + if (res < 0) + { + getNode().registerInternalFailure("NodeDiscoverer GetNodeInfo call"); + } + } + + void handleNodeStatus(const ReceivedDataStructure& msg) + { + if (!needToQuery(msg.getSrcNodeID())) + { + return; + } + + NodeData* data = node_map_.access(msg.getSrcNodeID()); + if (data == NULL) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Found new node %d", int(msg.getSrcNodeID().get())); + data = node_map_.insert(msg.getSrcNodeID(), NodeData()); + if (data == NULL) + { + getNode().registerInternalFailure("NodeDiscoverer OOM"); + return; + } + } + UAVCAN_ASSERT(data != NULL); + + if (msg.uptime_sec < data->last_seen_uptime) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Node %d restarted", int(msg.getSrcNodeID().get())); + data->num_get_node_info_attempts = 0; + } + data->last_seen_uptime = msg.uptime_sec; + + if (!isRunning() && handler_.canDiscoverNewNodes()) + { + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer started"); + startPeriodic(get_node_info_client_.getRequestTimeout()); + } + } + +public: + NodeDiscoverer(INode& node, INodeDiscoveryHandler& handler) + : TimerBase(node) + , handler_(handler) + , node_map_(node.getAllocator()) + , get_node_info_client_(node) + , node_status_sub_(node) + { } + + int init() + { + int res = get_node_info_client_.init(); + if (res < 0) + { + return res; + } + get_node_info_client_.setCallback( + GetNodeInfoResponseCallback(this, &NodeDiscoverer::handleGetNodeInfoResponse)); + + res = node_status_sub_.start(NodeStatusCallback(this, &NodeDiscoverer::handleNodeStatus)); + if (res < 0) + { + return res; + } + + // Note: the timer starts ad-hoc from the node status callback, not here. + + return 0; + } + + /** + * Returns true if there's at least one node with pending GetNodeInfo. + */ + bool hasUnknownNodes() const { return !node_map_.isEmpty(); } +}; + +} +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index eb93787b51..1d08cdc00c 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -127,6 +127,8 @@ TEST(DynamicNodeIDServer, Main) uavcan::DefaultDataTypeRegistrator _reg2; uavcan::DefaultDataTypeRegistrator _reg3; uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; EventTracer tracer; MemoryStorageBackend storage; From 12a81b5bef2538fb9212a65f8e4452762c1dc51a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 17:51:25 +0300 Subject: [PATCH 066/268] Server event tracer extracted to the common level --- .../distributed/cluster_manager.hpp | 2 +- .../protocol/dynamic_node_id_server/distributed/log.hpp | 2 +- .../distributed/persistent_state.hpp | 2 +- .../dynamic_node_id_server/distributed/raft_core.hpp | 2 +- .../dynamic_node_id_server/distributed/server.hpp | 2 +- .../dynamic_node_id_server/{distributed => }/event.hpp | 8 ++------ .../distributed/cluster_manager.cpp | 2 +- .../protocol/dynamic_node_id_server/distributed/log.cpp | 2 +- .../distributed/persistent_state.cpp | 2 +- .../dynamic_node_id_server/distributed/server.cpp | 2 +- .../dynamic_node_id_server/{distributed => }/event.cpp | 2 +- .../{distributed => }/event_tracer.hpp | 6 +++--- 12 files changed, 15 insertions(+), 19 deletions(-) rename libuavcan/include/uavcan/protocol/dynamic_node_id_server/{distributed => }/event.hpp (94%) rename libuavcan/test/protocol/dynamic_node_id_server/{distributed => }/event.cpp (90%) rename libuavcan/test/protocol/dynamic_node_id_server/{distributed => }/event_tracer.hpp (58%) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 52328fd23d..8452ab985a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -13,8 +13,8 @@ #include #include #include -#include #include +#include // UAVCAN types #include diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp index 85be7d6e4b..6a023d919c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -8,8 +8,8 @@ #include #include #include -#include #include +#include namespace uavcan { diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp index 580bab219c..371abcd704 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -8,9 +8,9 @@ #include #include #include -#include #include #include +#include namespace uavcan { diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index b83f9621ee..aeb6bd060a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -12,9 +12,9 @@ #include #include #include -#include #include #include +#include // UAVCAN types #include #include diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index d61b1f761e..e98838bf2e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -9,11 +9,11 @@ #include #include #include -#include #include #include #include #include +#include namespace uavcan { diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp similarity index 94% rename from libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp rename to libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index a82c100ab3..566c9a7ed7 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_EVENT_HPP_INCLUDED -#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_EVENT_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED #include #include @@ -12,8 +12,6 @@ namespace uavcan { namespace dynamic_node_id_server { -namespace distributed -{ /** * @ref IEventTracer. * Event codes cannot be changed, only new ones can be added. @@ -109,7 +107,6 @@ public: /** * The server invokes this method every time it believes that a noteworthy event has happened. - * The table of event codes can be found in the server sources. * It is guaranteed that event code values will never change, but new ones can be added in future. This ensures * full backward compatibility. * @param event_code Event code, see the sources for the enum with values. @@ -120,7 +117,6 @@ public: virtual ~IEventTracer() { } }; -} } } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp index 1cc9c1e885..679386087a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/cluster_manager.cpp @@ -4,7 +4,7 @@ #include #include -#include "event_tracer.hpp" +#include "../event_tracer.hpp" #include "../../helpers.hpp" #include "../memory_storage_backend.hpp" diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp index a836c90302..e5d7339410 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp @@ -4,7 +4,7 @@ #include #include -#include "event_tracer.hpp" +#include "../event_tracer.hpp" #include "../../helpers.hpp" #include "../memory_storage_backend.hpp" diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp index 94cad3a11a..a47fa81585 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/persistent_state.cpp @@ -4,7 +4,7 @@ #include #include -#include "event_tracer.hpp" +#include "../event_tracer.hpp" #include "../memory_storage_backend.hpp" diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 1d08cdc00c..d2780d3c53 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -11,7 +11,7 @@ #include #include #include -#include "event_tracer.hpp" +#include "../event_tracer.hpp" #include "../../helpers.hpp" #include "../memory_storage_backend.hpp" diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp similarity index 90% rename from libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp rename to libuavcan/test/protocol/dynamic_node_id_server/event.cpp index 40033726e5..8aea0e56c0 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/event.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -3,7 +3,7 @@ */ #include -#include +#include TEST(DynamicNodeIDServer, EventCodeToString) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp b/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp similarity index 58% rename from libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp rename to libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp index 20f8f024c6..0b8f89c533 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/event_tracer.hpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp @@ -6,14 +6,14 @@ #include #include -#include +#include -class EventTracer : public uavcan::dynamic_node_id_server::distributed::IEventTracer +class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer { const std::string id_; - virtual void onEvent(uavcan::dynamic_node_id_server::distributed::TraceCode code, uavcan::int64_t argument) + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) { std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; } From c49ee1c4d3926b873cb60493353525333961e474 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 17:56:48 +0300 Subject: [PATCH 067/268] Top-level typedef for distributed::Server --- .../protocol/dynamic_node_id_server/distributed.hpp | 10 ++++++++++ .../dynamic_node_id_server/distributed/server.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp index eef49ccba1..822f757522 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp @@ -7,4 +7,14 @@ #include +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef distributed::Server DistributedServer; + +} +} + #endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_DISTRIBUTED_HPP_INCLUDED diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index d2780d3c53..2dc96aa94f 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -139,7 +139,7 @@ TEST(DynamicNodeIDServer, Main) /* * Server */ - distributed::Server server(nodes.a, storage, tracer); + DistributedServer server(nodes.a, storage, tracer); ASSERT_LE(0, server.init(1)); From 10b5661da41aeb30fa040757c3386f9dbbda4222 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 19:22:03 +0300 Subject: [PATCH 068/268] Advanced tracing --- .../allocation_request_manager.hpp | 57 ++++++++++++----- .../distributed/server.hpp | 4 +- .../protocol/dynamic_node_id_server/event.hpp | 62 +++++++++++++++++-- .../node_discoverer.hpp | 22 +++++-- .../allocation_request_manager.cpp | 4 +- .../protocol/dynamic_node_id_server/event.cpp | 1 - 6 files changed, 120 insertions(+), 30 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 30fc0915e4..bb5a6fb3bd 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -11,6 +11,7 @@ #include #include #include +#include // UAVCAN types #include @@ -44,25 +45,27 @@ public: class AllocationRequestManager { typedef MethodBinder&)> + void (AllocationRequestManager::*)(const ReceivedDataStructure&)> AllocationCallback; const MonotonicDuration stage_timeout_; MonotonicTime last_message_timestamp_; - protocol::dynamic_node_id::Allocation::FieldTypes::unique_id current_unique_id_; + Allocation::FieldTypes::unique_id current_unique_id_; IAllocationRequestHandler& handler_; + IEventTracer& tracer_; - Subscriber allocation_sub_; - Publisher allocation_pub_; + Subscriber allocation_sub_; + Publisher allocation_pub_; enum { InvalidStage = 0 }; - static uint8_t detectRequestStage(const protocol::dynamic_node_id::Allocation& msg) + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + + static uint8_t detectRequestStage(const Allocation& msg) { - const uint8_t max_bytes_per_request = protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; + const uint8_t max_bytes_per_request = Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST; if ((msg.unique_id.size() != max_bytes_per_request) && (msg.unique_id.size() != (msg.unique_id.capacity() - max_bytes_per_request * 2U)) && @@ -74,11 +77,11 @@ class AllocationRequestManager { return 1; // Note that CAN FD frames can deliver the unique ID in one stage! } - if (msg.unique_id.size() == protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + if (msg.unique_id.size() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) { return 2; } - if (msg.unique_id.size() < protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + if (msg.unique_id.size() < Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) { return 3; } @@ -91,11 +94,11 @@ class AllocationRequestManager { return 1; } - if (current_unique_id_.size() >= (protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) + if (current_unique_id_.size() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) { return 3; } - if (current_unique_id_.size() >= protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) { return 2; } @@ -104,21 +107,24 @@ class AllocationRequestManager void publishFollowupAllocationResponse() { - protocol::dynamic_node_id::Allocation msg; + Allocation msg; msg.unique_id = current_unique_id_; UAVCAN_ASSERT(msg.unique_id.size() < msg.unique_id.capacity()); UAVCAN_TRACE("AllocationRequestManager", "Intermediate response with %u bytes of unique ID", unsigned(msg.unique_id.size())); + trace(TraceAllocationFollowupResponse, msg.unique_id.size()); + const int res = allocation_pub_.broadcast(msg); if (res < 0) { + trace(TraceError, res); allocation_pub_.getNode().registerInternalFailure("Dynamic allocation broadcast"); } } - void handleAllocation(const ReceivedDataStructure& msg) + void handleAllocation(const ReceivedDataStructure& msg) { if (!msg.isAnonymousTransfer()) { @@ -132,6 +138,7 @@ class AllocationRequestManager { UAVCAN_TRACE("AllocationRequestManager", "Stage timeout, reset"); current_unique_id_.clear(); + trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec()); } /* @@ -140,24 +147,29 @@ class AllocationRequestManager const uint8_t request_stage = detectRequestStage(msg); if (request_stage == InvalidStage) { + trace(TraceAllocationBadRequest, msg.unique_id.size()); return; // Malformed request - ignore without resetting } const uint8_t expected_stage = getExpectedStage(); if (request_stage == InvalidStage) { + UAVCAN_ASSERT(0); return; } if (request_stage != expected_stage) { + trace(TraceAllocationUnexpectedStage, request_stage); return; // Ignore - stage mismatch } - const uint8_t max_expected_bytes = static_cast(current_unique_id_.capacity() - current_unique_id_.size()); + const uint8_t max_expected_bytes = + static_cast(current_unique_id_.capacity() - current_unique_id_.size()); UAVCAN_ASSERT(max_expected_bytes > 0); if (msg.unique_id.size() > max_expected_bytes) { + trace(TraceAllocationBadRequest, msg.unique_id.size()); return; // Malformed request } @@ -169,6 +181,8 @@ class AllocationRequestManager current_unique_id_.push_back(msg.unique_id[i]); } + trace(TraceAllocationRequestAccepted, current_unique_id_.size()); + /* * Proceeding with allocation if possible * Note that single-frame CAN FD allocation requests will be delivered to the server even if it's not leader. @@ -183,6 +197,13 @@ class AllocationRequestManager current_unique_id_.clear(); handler_.handleAllocationRequest(unique_id, msg.node_id); + + uint64_t event_agrument = 0; + for (uint8_t i = 0; i < 8; i++) + { + event_agrument |= static_cast(unique_id[i]) << (i * 8U); + } + trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); } else { @@ -192,6 +213,7 @@ class AllocationRequestManager } else { + trace(TraceAllocationFollowupDenied, 0); current_unique_id_.clear(); } } @@ -203,9 +225,10 @@ class AllocationRequestManager } public: - AllocationRequestManager(INode& node, IAllocationRequestHandler& handler) + AllocationRequestManager(INode& node, IEventTracer& tracer, IAllocationRequestHandler& handler) : stage_timeout_(MonotonicDuration::fromMSec(Allocation::FOLLOWUP_TIMEOUT_MS)) , handler_(handler) + , tracer_(tracer) , allocation_sub_(node) , allocation_pub_(node) { } @@ -231,13 +254,15 @@ public: int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_node_id) { - protocol::dynamic_node_id::Allocation msg; + Allocation msg; msg.unique_id.resize(msg.unique_id.capacity()); copy(unique_id.begin(), unique_id.end(), msg.unique_id.begin()); msg.node_id = allocated_node_id.get(); + trace(TraceAllocationResponse, msg.node_id); + return allocation_pub_.broadcast(msg); } }; diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index e98838bf2e..274045a6f3 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -227,8 +227,8 @@ public: IEventTracer& tracer) : node_(node) , raft_core_(node, storage, tracer, *this) - , allocation_request_manager_(node, *this) - , node_discoverer_(node, *this) + , allocation_request_manager_(node, tracer, *this) + , node_discoverer_(node, tracer, *this) { } int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index 566c9a7ed7..d3ab3949c8 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -6,7 +6,7 @@ #define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED #include -#include +#include namespace uavcan { @@ -51,6 +51,35 @@ enum TraceCode TraceRaftNewEntryCommitted, // new commit index value // 25 TraceRaftAppendEntriesCallFailure, // error code (may be negated) + Trace0, + Trace1, + Trace2, + Trace3, + // 30 + TraceAllocationFollowupResponse, // number of unique ID bytes in this response + TraceAllocationFollowupDenied, // reason code (see sources for details) + TraceAllocationFollowupTimeout, // timeout value in microseconds + TraceAllocationBadRequest, // number of unique ID bytes in this request + TraceAllocationUnexpectedStage, // stage number in the request - 1, 2, or 3 + // 35 + TraceAllocationRequestAccepted, // number of bytes of unique ID after request + TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit little endian + TraceAllocationResponse, // allocated node ID + Trace11, + Trace12, + // 40 + TraceDiscoveryNewNodeFound, // node ID + TraceDiscoveryCommitCacheUpdated, // node ID marked as committed + TraceDiscoveryNodeFinalized, // node ID in lower 7 bits, bit 8 (256, 0x100) is set if unique ID is known + TraceDiscoveryGetNodeInfoFailure, // node ID + TraceDiscoveryTimerStart, // interval in microseconds + // 45 + TraceDiscoveryTimerStop, // reason code (see sources for details) + TraceDiscoveryGetNodeInfoRequest, // target node ID + Trace20, + Trace21, + Trace22, + // 50 NumTraceCodes }; @@ -68,9 +97,8 @@ public: */ static const char* getEventName(TraceCode code) { - // import re - // make_strings = lambda s: ',\n'.join('"%s"' % x for x in re.findall(r'\ \ \ \ Trace([A-Za-z0-9]+),', s)) - static const char* const Strings[NumTraceCodes] = + // import re;m = lambda s:',\n'.join('"%s"' % x for x in re.findall(r'\ {4}Trace[0-9]*([A-Za-z0-9]*),',s)) + static const char* const Strings[] = { "Error", "LogLastIndexRestored", @@ -97,7 +125,31 @@ public: "RaftCommitIndexUpdate", "RaftNewerTermInResponse", "RaftNewEntryCommitted", - "RaftAppendEntriesCallFailure" + "RaftAppendEntriesCallFailure", + "", + "", + "", + "", + "AllocationFollowupResponse", + "AllocationFollowupDenied", + "AllocationFollowupTimeout", + "AllocationBadRequest", + "AllocationUnexpectedStage", + "AllocationRequestAccepted", + "AllocationExchangeComplete", + "AllocationResponse", + "", + "", + "DiscoveryNewNodeFound", + "DiscoveryCommitCacheUpdated", + "DiscoveryNodeFinalized", + "DiscoveryGetNodeInfoFailure", + "DiscoveryTimerStart", + "DiscoveryTimerStop", + "DiscoveryGetNodeInfoRequest", + "", + "", + "" }; uavcan::StaticAssert::check(); UAVCAN_ASSERT(code < NumTraceCodes); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 5483795308..c1074c3a8e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include // UAVCAN types #include @@ -94,6 +95,7 @@ class NodeDiscoverer : TimerBase * States */ INodeDiscoveryHandler& handler_; + IEventTracer& tracer_; BitSet committed_node_mask_; ///< Nodes that are marked will not be queried NodeMap node_map_; ///< Will not work in UAVCAN_TINY @@ -104,6 +106,8 @@ class NodeDiscoverer : TimerBase /* * Methods */ + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + INode& getNode() { return node_status_sub_.getNode(); } NodeID pickNextNodeToQuery() const @@ -166,6 +170,7 @@ class NodeDiscoverer : TimerBase } else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted) { + trace(TraceDiscoveryCommitCacheUpdated, node_id.get()); committed_node_mask_[node_id.get()] = true; node_map_.remove(node_id); return false; @@ -179,6 +184,7 @@ class NodeDiscoverer : TimerBase void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) { + trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == NULL) ? 0U : 0x100U)); node_map_.remove(node_id); if (needToQuery(node_id)) // Making sure the server is still interested { @@ -196,6 +202,8 @@ class NodeDiscoverer : TimerBase } else { + trace(TraceDiscoveryGetNodeInfoFailure, result.server_node_id.get()); + NodeData* const data = node_map_.access(result.server_node_id); if (data == NULL) { @@ -222,7 +230,7 @@ class NodeDiscoverer : TimerBase if (!handler_.canDiscoverNewNodes()) { - UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer stopped - server disallows discovery"); + trace(TraceDiscoveryTimerStop, 0); stop(); return; } @@ -230,11 +238,13 @@ class NodeDiscoverer : TimerBase const NodeID node_id = pickNextNodeToQuery(); if (!node_id.isUnicast()) { - UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer stopped - no unknown nodes left"); + trace(TraceDiscoveryTimerStop, 1); stop(); return; } + trace(TraceDiscoveryGetNodeInfoRequest, node_id.get()); + UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d", int(node_id.get())); const int res = get_node_info_client_.call(node_id, protocol::GetNodeInfo::Request()); @@ -254,7 +264,8 @@ class NodeDiscoverer : TimerBase NodeData* data = node_map_.access(msg.getSrcNodeID()); if (data == NULL) { - UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Found new node %d", int(msg.getSrcNodeID().get())); + trace(TraceDiscoveryNewNodeFound, msg.getSrcNodeID().get()); + data = node_map_.insert(msg.getSrcNodeID(), NodeData()); if (data == NULL) { @@ -273,15 +284,16 @@ class NodeDiscoverer : TimerBase if (!isRunning() && handler_.canDiscoverNewNodes()) { - UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Query timer started"); + trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec()); startPeriodic(get_node_info_client_.getRequestTimeout()); } } public: - NodeDiscoverer(INode& node, INodeDiscoveryHandler& handler) + NodeDiscoverer(INode& node, IEventTracer& tracer, INodeDiscoveryHandler& handler) : TimerBase(node) , handler_(handler) + , tracer_(tracer) , node_map_(node.getAllocator()) , get_node_info_client_(node) , node_status_sub_(node) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp index edf82906e4..dfecf86b6a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -5,6 +5,7 @@ #include #include #include +#include "event_tracer.hpp" #include "../helpers.hpp" @@ -88,10 +89,11 @@ TEST(dynamic_node_id_server_AllocationRequestManager, Basic) /* * Request manager initialization */ + EventTracer tracer; AllocationRequestHandler handler; handler.can_followup = true; - AllocationRequestManager manager(nodes.a, handler); + AllocationRequestManager manager(nodes.a, tracer, handler); ASSERT_LE(0, manager.init()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index 8aea0e56c0..b62abff9de 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -8,7 +8,6 @@ TEST(DynamicNodeIDServer, EventCodeToString) { - using namespace uavcan::dynamic_node_id_server::distributed; using namespace uavcan::dynamic_node_id_server; // Simply checking some error codes From 6e287dc1b27c7603cdfaed7a7fa04c8a870adf99 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 19:31:52 +0300 Subject: [PATCH 069/268] Fixed naming of trace events --- .../distributed/cluster_manager.hpp | 10 ++-- .../distributed/log.hpp | 6 +-- .../distributed/persistent_state.hpp | 8 ++-- .../protocol/dynamic_node_id_server/event.hpp | 48 +++++++++---------- .../protocol/dynamic_node_id_server/event.cpp | 2 +- 5 files changed, 37 insertions(+), 37 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 8452ab985a..af9ccb6929 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -94,7 +94,7 @@ private: UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); if (!isKnownServer(node_id) && node_id.isUnicast()) { - tracer_.onEvent(TraceNewServerDiscovered, node_id.get()); + 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(num_known_servers_ + 1U); @@ -109,7 +109,7 @@ private: { UAVCAN_ASSERT(num_known_servers_ < cluster_size_); - tracer_.onEvent(TraceDiscoveryBroadcast, num_known_servers_); + tracer_.onEvent(TraceRaftDiscoveryBroadcast, num_known_servers_); /* * Filling the message @@ -154,7 +154,7 @@ private: void handleDiscovery(const ReceivedDataStructure& msg) { - tracer_.onEvent(TraceDiscoveryReceived, msg.getSrcNodeID().get()); + tracer_.onEvent(TraceRaftDiscoveryReceived, msg.getSrcNodeID().get()); /* * Validating cluster configuration @@ -162,7 +162,7 @@ private: */ if (msg.configured_cluster_size != cluster_size_) { - tracer_.onEvent(TraceInvalidClusterSizeReceived, msg.configured_cluster_size); + tracer_.onEvent(TraceRaftBadClusterSizeReceived, msg.configured_cluster_size); getNode().registerInternalFailure("Bad Raft cluster size"); return; } @@ -271,7 +271,7 @@ public: } } - tracer_.onEvent(TraceClusterSizeInited, cluster_size_); + tracer_.onEvent(TraceRaftClusterSizeInited, cluster_size_); UAVCAN_ASSERT(cluster_size_ > 0); UAVCAN_ASSERT(cluster_size_ <= MaxClusterSize); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp index 6a023d919c..d829d293e0 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -185,7 +185,7 @@ public: last_index_ = Index(value); } - tracer_.onEvent(TraceLogLastIndexRestored, last_index_); + tracer_.onEvent(TraceRaftLogLastIndexRestored, last_index_); // Restoring log entries - note that index 0 always exists for (Index index = 0; index <= last_index_; index++) @@ -214,7 +214,7 @@ public: return -ErrLogic; } - tracer_.onEvent(TraceLogAppend, last_index_ + 1U); + tracer_.onEvent(TraceRaftLogAppend, last_index_ + 1U); // If next operations fail, we'll get a dangling entry, but it's absolutely OK. int res = writeEntryToStorage(Index(last_index_ + 1), entry); @@ -258,7 +258,7 @@ public: uint32_t new_last_index = index - 1U; - tracer_.onEvent(TraceLogRemove, new_last_index); + tracer_.onEvent(TraceRaftLogRemove, new_last_index); if (new_last_index != last_index_) { diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp index 371abcd704..b5d8b0891d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -96,7 +96,7 @@ public: } } - tracer_.onEvent(TraceCurrentTermRestored, current_term_); + tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_); if (current_term_ < last_entry->term) { @@ -144,7 +144,7 @@ public: voted_for_ = NodeID(uint8_t(stored_voted_for)); } - tracer_.onEvent(TraceVotedForRestored, voted_for_.get()); + tracer_.onEvent(TraceRaftVotedForRestored, voted_for_.get()); return 0; } @@ -168,7 +168,7 @@ public: return -ErrInvalidParam; } - tracer_.onEvent(TraceCurrentTermUpdate, term); + tracer_.onEvent(TraceRaftCurrentTermUpdate, term); StorageMarshaller io(storage_); @@ -199,7 +199,7 @@ public: return -ErrInvalidParam; } - tracer_.onEvent(TraceVotedForUpdate, node_id.get()); + tracer_.onEvent(TraceRaftVotedForUpdate, node_id.get()); StorageMarshaller io(storage_); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index d3ab3949c8..b83eff1f87 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -21,20 +21,20 @@ enum TraceCode // Event name Argument // 0 TraceError, // error code (may be negated) - TraceLogLastIndexRestored, // recovered last index value - TraceLogAppend, // index of new entry - TraceLogRemove, // new last index value - TraceCurrentTermRestored, // current term + TraceRaftLogLastIndexRestored, // recovered last index value + TraceRaftLogAppend, // index of new entry + TraceRaftLogRemove, // new last index value + TraceRaftCurrentTermRestored, // current term // 5 - TraceCurrentTermUpdate, // current term - TraceVotedForRestored, // value of votedFor - TraceVotedForUpdate, // value of votedFor - TraceDiscoveryBroadcast, // number of known servers - TraceNewServerDiscovered, // node ID of the new server + TraceRaftCurrentTermUpdate, // current term + TraceRaftVotedForRestored, // value of votedFor + TraceRaftVotedForUpdate, // value of votedFor + TraceRaftDiscoveryBroadcast, // number of known servers + TraceRaftNewServerDiscovered, // node ID of the new server // 10 - TraceDiscoveryReceived, // node ID of the sender - TraceClusterSizeInited, // cluster size - TraceInvalidClusterSizeReceived, // received cluster size + TraceRaftDiscoveryReceived, // node ID of the sender + TraceRaftClusterSizeInited, // cluster size + TraceRaftBadClusterSizeReceived, // received cluster size TraceRaftCoreInited, // update interval in usec TraceRaftStateSwitch, // 0 - Follower, 1 - Candidate, 2 - Leader // 15 @@ -101,18 +101,18 @@ public: static const char* const Strings[] = { "Error", - "LogLastIndexRestored", - "LogAppend", - "LogRemove", - "CurrentTermRestored", - "CurrentTermUpdate", - "VotedForRestored", - "VotedForUpdate", - "DiscoveryBroadcast", - "NewServerDiscovered", - "DiscoveryReceived", - "ClusterSizeInited", - "InvalidClusterSizeReceived", + "RaftLogLastIndexRestored", + "RaftLogAppend", + "RaftLogRemove", + "RaftCurrentTermRestored", + "RaftCurrentTermUpdate", + "RaftVotedForRestored", + "RaftVotedForUpdate", + "RaftDiscoveryBroadcast", + "RaftNewServerDiscovered", + "RaftDiscoveryReceived", + "RaftClusterSizeInited", + "RaftBadClusterSizeReceived", "RaftCoreInited", "RaftStateSwitch", "RaftActiveSwitch", diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index b62abff9de..b8efb3de25 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -14,5 +14,5 @@ TEST(DynamicNodeIDServer, EventCodeToString) ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError)); ASSERT_STREQ("RaftActiveSwitch", IEventTracer::getEventName(TraceRaftActiveSwitch)); ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); - ASSERT_STREQ("DiscoveryReceived", IEventTracer::getEventName(TraceDiscoveryReceived)); + ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived)); } From a309c6d8dae76a3356aa59ff6f6445236c2d34dd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 20:03:17 +0300 Subject: [PATCH 070/268] Raft allocator adds its own allocation entry to the log --- libuavcan/include/uavcan/error.hpp | 1 + .../distributed/raft_core.hpp | 55 ++++++++++++++----- .../distributed/server.hpp | 49 ++++++++++++++++- .../distributed/server.cpp | 8 ++- 4 files changed, 98 insertions(+), 15 deletions(-) diff --git a/libuavcan/include/uavcan/error.hpp b/libuavcan/include/uavcan/error.hpp index 3776f27b8e..4f67e9a6ec 100644 --- a/libuavcan/include/uavcan/error.hpp +++ b/libuavcan/include/uavcan/error.hpp @@ -34,6 +34,7 @@ const int16_t ErrRecursiveCall = 9; const int16_t ErrLogic = 10; const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type +const int16_t ErrInvalidConfiguration = 13; /** * @} */ diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index aeb6bd060a..337ba532cb 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -36,6 +36,13 @@ public: */ virtual void handleLogCommitOnLeader(const Entry& committed_entry) = 0; + /** + * Invoked by the Raft core when the local node becomes a leader or ceases to be one. + * By default the local node is not leader. + * It is possible to commit to the log right from this method. + */ + virtual void handleLocalLeadershipChange(bool local_node_is_leader) = 0; + virtual ~IRaftLeaderMonitor() { } }; @@ -270,24 +277,46 @@ class RaftCore : private TimerBase void switchState(ServerState new_state) { - if (server_state_ != new_state) + if (server_state_ == new_state) { - UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d", - int(server_state_), int(new_state)); - trace(TraceRaftStateSwitch, new_state); + return; + } - server_state_ = new_state; + /* + * Logging + */ + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d", + int(server_state_), int(new_state)); + trace(TraceRaftStateSwitch, new_state); - cluster_.resetAllServerIndices(); + /* + * Updating the current state + */ + const ServerState old_state = server_state_; + server_state_ = new_state; - next_server_index_ = 0; - num_votes_received_in_this_campaign_ = 0; + /* + * Resetting specific states + */ + cluster_.resetAllServerIndices(); - for (uint8_t i = 0; i < NumRequestVoteClients; i++) - { - request_vote_clients_[i]->cancel(); - } - append_entries_client_.cancel(); + 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(); + + /* + * Calling the switch handler + * Note that the handler may commit to the log directly + */ + if ((old_state == ServerStateLeader) || + (new_state == ServerStateLeader)) + { + leader_monitor_.handleLocalLeadershipChange(new_state == ServerStateLeader); } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 274045a6f3..bab791c7d7 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -56,6 +56,11 @@ class Server : IAllocationRequestHandler } }; + /* + * Constants + */ + UniqueID own_unique_id_; + /* * States */ @@ -183,6 +188,26 @@ class Server : IAllocationRequestHandler tryPublishAllocationResult(entry); } + virtual void handleLocalLeadershipChange(bool local_node_is_leader) + { + if (!local_node_is_leader) + { + return; + } + + const LazyConstructor result = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID())); + + if (!result.isConstructed()) + { + const int res = raft_core_.appendLog(own_unique_id_, node_.getNodeID()); + if (res < 0) + { + node_.registerInternalFailure("Raft log append with self ID"); + } + } + } + /* * Private methods */ @@ -231,14 +256,36 @@ public: , node_discoverer_(node, tracer, *this) { } - int init(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) + int init(const UniqueID& own_unique_id, const uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) { + /* + * Initializing Raft core first, because the next step requires Log to be loaded + */ int res = raft_core_.init(cluster_size); if (res < 0) { return res; } + /* + * Making sure that the server is started with the same node ID + */ + own_unique_id_ = own_unique_id; + + const LazyConstructor own_log_entry = + raft_core_.traverseLogFromEndUntil(NodeIDLogPredicate(node_.getNodeID())); + + if (own_log_entry.isConstructed()) + { + if (own_log_entry->entry.unique_id != own_unique_id_) + { + return -ErrInvalidConfiguration; + } + } + + /* + * Misc + */ res = allocation_request_manager_.init(); if (res < 0) { diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 2dc96aa94f..2ec67a5f61 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -136,12 +136,18 @@ TEST(DynamicNodeIDServer, Main) // 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 */ DistributedServer server(nodes.a, storage, tracer); - ASSERT_LE(0, server.init(1)); + ASSERT_LE(0, server.init(own_unique_id, 1)); /* * Client From b7a4c9524e946462f18eb13769e846babe20bc63 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 20:05:40 +0300 Subject: [PATCH 071/268] Fixed error codes --- .../dynamic_node_id_server/distributed/cluster_manager.hpp | 2 +- .../dynamic_node_id_server/distributed/persistent_state.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index af9ccb6929..50a5c5cefb 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -248,7 +248,7 @@ public: if ((value == 0) || (value > MaxClusterSize)) { UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", "Cluster size is invalid"); - return -ErrFailure; + return -ErrInvalidConfiguration; } cluster_size_ = static_cast(value); } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp index b5d8b0891d..91869679c1 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -139,7 +139,7 @@ public: } if (stored_voted_for > NodeID::Max) { - return -ErrFailure; + return -ErrInvalidConfiguration; } voted_for_ = NodeID(uint8_t(stored_voted_for)); } From f2c393e90dc8356ce6fc2b729e9ed5d2672caae9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 20:10:13 +0300 Subject: [PATCH 072/268] Server test extension --- .../dynamic_node_id_server/distributed/raft_core.hpp | 6 ++++++ .../protocol/dynamic_node_id_server/distributed/server.hpp | 2 ++ .../protocol/dynamic_node_id_server/distributed/server.cpp | 4 ++++ 3 files changed, 12 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 337ba532cb..d4e334d759 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -875,6 +875,12 @@ public: } return LazyConstructor(); } + + Log::Index getNumAllocations() const + { + // Remember that index zero contains a special-purpose entry that doesn't count as allocation + return persistent_state_.getLog().getLastIndex(); + } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index bab791c7d7..0fc0f7404a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -300,6 +300,8 @@ public: return 0; } + + Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); } }; } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 2ec67a5f61..4e566bcf21 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -149,6 +149,8 @@ TEST(DynamicNodeIDServer, Main) ASSERT_LE(0, server.init(own_unique_id, 1)); + ASSERT_EQ(0, server.getNumAllocations()); + /* * Client */ @@ -168,6 +170,8 @@ TEST(DynamicNodeIDServer, Main) ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); + + ASSERT_EQ(2, server.getNumAllocations()); // Server's own node ID + client } From 526b9371e4f540ca7d33fd2afd37128109e2e7a2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 20:24:32 +0300 Subject: [PATCH 073/268] Minor logging fix --- .../allocation_request_manager.hpp | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index bb5a6fb3bd..7bd852dd96 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -196,14 +196,16 @@ class AllocationRequestManager copy(current_unique_id_.begin(), current_unique_id_.end(), unique_id.begin()); current_unique_id_.clear(); - handler_.handleAllocationRequest(unique_id, msg.node_id); - - uint64_t event_agrument = 0; - for (uint8_t i = 0; i < 8; i++) { - event_agrument |= static_cast(unique_id[i]) << (i * 8U); + uint64_t event_agrument = 0; + for (uint8_t i = 0; i < 8; i++) + { + event_agrument |= static_cast(unique_id[i]) << (i * 8U); + } + trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); } - trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); + + handler_.handleAllocationRequest(unique_id, msg.node_id); } else { From a6b0a256fba90034f4fbb30af6ec166ba483563e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 20:28:28 +0300 Subject: [PATCH 074/268] Byte order fix in logging --- .../dynamic_node_id_server/allocation_request_manager.hpp | 2 +- .../include/uavcan/protocol/dynamic_node_id_server/event.hpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 7bd852dd96..a1c2f05b2f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -200,7 +200,7 @@ class AllocationRequestManager uint64_t event_agrument = 0; for (uint8_t i = 0; i < 8; i++) { - event_agrument |= static_cast(unique_id[i]) << (i * 8U); + event_agrument |= static_cast(unique_id[i]) << (56U - i * 8U); } trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index b83eff1f87..6a61b8dfdb 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -63,7 +63,7 @@ enum TraceCode TraceAllocationUnexpectedStage, // stage number in the request - 1, 2, or 3 // 35 TraceAllocationRequestAccepted, // number of bytes of unique ID after request - TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit little endian + TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit big endian TraceAllocationResponse, // allocated node ID Trace11, Trace12, From 731d9f45749265342817472dbe00a5089fd8b7ac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 10 May 2015 23:43:08 +0300 Subject: [PATCH 075/268] Changed RaftCore API; giving up Leader status if the leader cannot write its log --- .../distributed/raft_core.hpp | 15 +++++++++------ .../distributed/server.hpp | 18 +++--------------- .../distributed/server.cpp | 2 +- 3 files changed, 13 insertions(+), 22 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index d4e334d759..fb39a57892 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -812,10 +812,10 @@ public: /** * Inserts one entry into log. - * Failures are tolerble because all operations are idempotent. * This method will trigger an assertion failure and return error if the current node is not the leader. + * If operation fails, the node may give up its Leader status. */ - int appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) + void appendLog(const Entry::FieldTypes::unique_id& unique_id, NodeID node_id) { if (isLeader()) { @@ -825,12 +825,15 @@ public: entry.term = persistent_state_.getCurrentTerm(); trace(TraceRaftNewLogEntry, entry.node_id); - return persistent_state_.getLog().append(entry); + const int res = persistent_state_.getLog().append(entry); + if (res < 0) + { + handlePersistentStateUpdateError(res); + } } else { UAVCAN_ASSERT(0); - return -ErrLogic; } } @@ -853,8 +856,8 @@ public: * Predicate is a callable of the following prototype: * bool (const LogEntryInfo& entry) * Once the predicate returns true, the loop will be terminated and the method will return an initialized lazy - * contructor to the last visited entry; otherwise the constructor will not be initialized. In this case, lazy - * constructor is used as boost::optional. + * contructor with the last visited entry; otherwise the constructor will not be initialized. + * In this case, lazy constructor is used as boost::optional. * The log is always traversed from HIGH to LOW index values, i.e. entry 0 will be traversed last. */ template diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 0fc0f7404a..eedb2eb46f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -169,11 +169,7 @@ class Server : IAllocationRequestHandler if (raft_core_.isLeader()) { - const int res = raft_core_.appendLog(uid, node_id); - if (res < 0) - { - node_.registerInternalFailure("Raft log append discovered node"); - } + raft_core_.appendLog(uid, node_id); } } @@ -200,11 +196,7 @@ class Server : IAllocationRequestHandler if (!result.isConstructed()) { - const int res = raft_core_.appendLog(own_unique_id_, node_.getNodeID()); - if (res < 0) - { - node_.registerInternalFailure("Raft log append with self ID"); - } + raft_core_.appendLog(own_unique_id_, node_.getNodeID()); } } @@ -230,11 +222,7 @@ class Server : IAllocationRequestHandler UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d", int(allocated_node_id.get())); - const int res = raft_core_.appendLog(unique_id, allocated_node_id); - if (res < 0) - { - node_.registerInternalFailure("Raft log append new allocation"); - } + raft_core_.appendLog(unique_id, allocated_node_id); } void tryPublishAllocationResult(const protocol::dynamic_node_id::server::Entry& entry) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 4e566bcf21..425fa64573 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -83,7 +83,7 @@ TEST(DynamicNodeIDServer, RaftCoreBasic) Entry::FieldTypes::unique_id unique_id; uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); - ASSERT_LE(0, raft_a->appendLog(unique_id, uavcan::NodeID(1))); + raft_a->appendLog(unique_id, uavcan::NodeID(1)); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); From 5af19f82c3a0de26e253ee1ebcf7054955449403 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 13:06:30 +0300 Subject: [PATCH 076/268] Raft logic fixes --- .../distributed/raft_core.hpp | 28 ++++++------------- 1 file changed, 9 insertions(+), 19 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index fb39a57892..5d0641fbc0 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -402,11 +402,10 @@ class RaftCore : private TimerBase if (!cluster_.isKnownServer(request.getSrcNodeID())) { trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); return; } - registerActivity(); - UAVCAN_ASSERT(response.isResponseEnabled()); // This is default /* @@ -418,22 +417,16 @@ class RaftCore : private TimerBase int res = persistent_state_.setCurrentTerm(request.term); if (res < 0) { + handlePersistentStateUpdateError(res); response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); + return; } res = persistent_state_.resetVotedFor(); if (res < 0) { + handlePersistentStateUpdateError(res); response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); - } - - switchState(ServerStateFollower); - setActiveMode(false); - - if (!response.isResponseEnabled()) - { return; } } @@ -454,6 +447,7 @@ class RaftCore : private TimerBase return; } + registerActivity(); switchState(ServerStateFollower); setActiveMode(false); @@ -565,6 +559,7 @@ class RaftCore : private TimerBase if (!cluster_.isKnownServer(request.getSrcNodeID())) { trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); return; } @@ -581,21 +576,16 @@ class RaftCore : private TimerBase int res = persistent_state_.setCurrentTerm(request.term); if (res < 0) { + handlePersistentStateUpdateError(res); response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); + return; } res = persistent_state_.resetVotedFor(); if (res < 0) { + handlePersistentStateUpdateError(res); response.setResponseEnabled(false); - trace(TraceRaftPersistStateUpdateError, res); - } - - switchState(ServerStateFollower); - - if (!response.isResponseEnabled()) - { return; } } From d623eee54af33a8ba5fca68aac04a8f4d1d4e1f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 13:26:53 +0300 Subject: [PATCH 077/268] Raft logic fix: forcing active mode when allocation activity is detected --- .../allocation_request_manager.hpp | 8 ++++++++ .../distributed/raft_core.hpp | 14 ++++++++++++-- .../dynamic_node_id_server/distributed/server.hpp | 5 +++++ .../protocol/dynamic_node_id_server/event.hpp | 2 +- .../allocation_request_manager.cpp | 6 ++++++ 5 files changed, 32 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index a1c2f05b2f..8c63132adc 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -35,6 +35,11 @@ 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& msg) = 0; + virtual ~IAllocationRequestHandler() { } }; @@ -126,6 +131,9 @@ class AllocationRequestManager void handleAllocation(const ReceivedDataStructure& msg) { + trace(TraceAllocationActivity, msg.getSrcNodeID().get()); + handler_.handleAllocationActivityDetection(msg); + if (!msg.isAnonymousTransfer()) { return; // This is a response from another allocator, ignore diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 5d0641fbc0..5323024c29 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -763,7 +763,8 @@ public: { return res; } - append_entries_client_.setCallback(AppendEntriesResponseCallback(this, &RaftCore::handleAppendEntriesResponse)); + append_entries_client_.setCallback(AppendEntriesResponseCallback(this, + &RaftCore::handleAppendEntriesResponse)); append_entries_client_.setRequestTimeout(update_interval_); for (uint8_t i = 0; i < NumRequestVoteClients; i++) @@ -773,7 +774,8 @@ public: { return res; } - request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); + request_vote_clients_[i]->setCallback(RequestVoteResponseCallback(this, + &RaftCore::handleRequestVoteResponse)); request_vote_clients_[i]->setRequestTimeout(update_interval_); } @@ -785,6 +787,14 @@ public: 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. */ diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index eedb2eb46f..2c20fcb11d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -135,6 +135,11 @@ class Server : IAllocationRequestHandler } } + virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) + { + raft_core_.forceActiveMode(); + } + /* * Methods of INodeDiscoveryHandler */ diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index 6a61b8dfdb..97f58e4493 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -65,7 +65,7 @@ enum TraceCode TraceAllocationRequestAccepted, // number of bytes of unique ID after request TraceAllocationExchangeComplete, // first 8 bytes of unique ID interpreted as signed 64 bit big endian TraceAllocationResponse, // allocated node ID - Trace11, + TraceAllocationActivity, // source node ID of the message Trace12, // 40 TraceDiscoveryNewNodeFound, // node ID diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp index dfecf86b6a..037d73bc3a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -30,6 +30,12 @@ public: return can_followup; } + virtual void handleAllocationActivityDetection( + const uavcan::ReceivedDataStructure& msg) + { + std::cout << "ALLOCATION ACTIVITY\n" << msg << std::endl; + } + bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) { if (requests_.empty()) From 706e72cc7440bf0d017e3131cd0c413144ded142 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 13:28:07 +0300 Subject: [PATCH 078/268] Raft tracepoint at election completion --- .../dynamic_node_id_server/distributed/raft_core.hpp | 1 + .../uavcan/protocol/dynamic_node_id_server/event.hpp | 6 +++--- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 5323024c29..e2c29b3a65 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -165,6 +165,7 @@ class RaftCore : private TimerBase if (num_votes_received_in_this_campaign_ > 0) { + trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_); const bool won = num_votes_received_in_this_campaign_ >= cluster_.getQuorumSize(); UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Election complete, won: %d", int(won)); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index 97f58e4493..26413b1e7c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -51,7 +51,7 @@ enum TraceCode TraceRaftNewEntryCommitted, // new commit index value // 25 TraceRaftAppendEntriesCallFailure, // error code (may be negated) - Trace0, + TraceRaftElectionComplete, // number of votes collected Trace1, Trace2, Trace3, @@ -126,7 +126,7 @@ public: "RaftNewerTermInResponse", "RaftNewEntryCommitted", "RaftAppendEntriesCallFailure", - "", + "RaftElectionComplete", "", "", "", @@ -138,7 +138,7 @@ public: "AllocationRequestAccepted", "AllocationExchangeComplete", "AllocationResponse", - "", + "AllocationActivity", "", "DiscoveryNewNodeFound", "DiscoveryCommitCacheUpdated", From 1a6226ddc25acb4aeb6e3c0cbf28681a083b7c52 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 13:36:34 +0300 Subject: [PATCH 079/268] Added a comment explaining extensions to Raft --- .../distributed/raft_core.hpp | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index e2c29b3a65..17887557a3 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -50,6 +50,27 @@ public: * This class implements log replication and voting. * It does not implement client-server interaction at all; instead it just exposes a public method for adding * allocation entries. + * + * 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 { From 3221179eef22b1ebc112c5fe3a5ff136beb1c07a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 13:53:59 +0300 Subject: [PATCH 080/268] Fixed test names --- .../protocol/dynamic_node_id_server/distributed/server.cpp | 6 +++--- libuavcan/test/protocol/dynamic_node_id_server/event.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 425fa64573..e46cf27a4e 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -37,7 +37,7 @@ public: }; -TEST(DynamicNodeIDServer, RaftCoreBasic) +TEST(dynamic_node_id_server_RaftCore, Basic) { using namespace uavcan::dynamic_node_id_server::distributed; using namespace uavcan::protocol::dynamic_node_id::server; @@ -116,7 +116,7 @@ TEST(DynamicNodeIDServer, RaftCoreBasic) } -TEST(DynamicNodeIDServer, Main) +TEST(dynamic_node_id_server_Server, Basic) { using namespace uavcan::dynamic_node_id_server; using namespace uavcan::protocol::dynamic_node_id; @@ -175,7 +175,7 @@ TEST(DynamicNodeIDServer, Main) } -TEST(DynamicNodeIDServer, ObjectSizes) +TEST(dynamic_node_id_server, ObjectSizes) { using namespace uavcan::dynamic_node_id_server; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index b8efb3de25..b6dd2697ec 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -6,7 +6,7 @@ #include -TEST(DynamicNodeIDServer, EventCodeToString) +TEST(dynamic_node_id_server_EventTracer, EventCodeToString) { using namespace uavcan::dynamic_node_id_server; From 63ce2b793dd9d00adf46b12bef76607ebe18bef3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 14:27:55 +0300 Subject: [PATCH 081/268] New trace code on remote node restart --- .../include/uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- .../protocol/dynamic_node_id_server/node_discoverer.hpp | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index 26413b1e7c..d3d052aee1 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -76,7 +76,7 @@ enum TraceCode // 45 TraceDiscoveryTimerStop, // reason code (see sources for details) TraceDiscoveryGetNodeInfoRequest, // target node ID - Trace20, + TraceDiscoveryNodeRestartDetected, // node ID Trace21, Trace22, // 50 @@ -147,7 +147,7 @@ public: "DiscoveryTimerStart", "DiscoveryTimerStop", "DiscoveryGetNodeInfoRequest", - "", + "DiscoveryNodeRestartDetected", "", "" }; diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index c1074c3a8e..7fd1474691 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -277,7 +277,7 @@ class NodeDiscoverer : TimerBase if (msg.uptime_sec < data->last_seen_uptime) { - UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Node %d restarted", int(msg.getSrcNodeID().get())); + trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get()); data->num_get_node_info_attempts = 0; } data->last_seen_uptime = msg.uptime_sec; From e10d262631a1435995538bbabb55465175886e41 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 14:34:07 +0300 Subject: [PATCH 082/268] Improved event tracer stub --- .../protocol/dynamic_node_id_server/event.cpp | 28 ++++++++++ .../dynamic_node_id_server/event_tracer.hpp | 52 +++++++++++++++++-- 2 files changed, 75 insertions(+), 5 deletions(-) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index b6dd2697ec..e8f2c457e6 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -4,6 +4,7 @@ #include #include +#include "event_tracer.hpp" TEST(dynamic_node_id_server_EventTracer, EventCodeToString) @@ -15,4 +16,31 @@ TEST(dynamic_node_id_server_EventTracer, EventCodeToString) ASSERT_STREQ("RaftActiveSwitch", IEventTracer::getEventName(TraceRaftActiveSwitch)); ASSERT_STREQ("RaftAppendEntriesCallFailure", IEventTracer::getEventName(TraceRaftAppendEntriesCallFailure)); ASSERT_STREQ("RaftDiscoveryReceived", IEventTracer::getEventName(TraceRaftDiscoveryReceived)); + ASSERT_STREQ("DiscoveryNodeRestartDetected", IEventTracer::getEventName(TraceDiscoveryNodeRestartDetected)); + ASSERT_STREQ("AllocationUnexpectedStage", IEventTracer::getEventName(TraceAllocationUnexpectedStage)); +} + + +TEST(dynamic_node_id_server_EventTracer, EnvironmentSelfTest) +{ + using namespace uavcan::dynamic_node_id_server; + + EventTracer tracer; + + ASSERT_EQ(0, tracer.getNumEvents()); + + tracer.onEvent(TraceRaftAppendEntriesCallFailure, 123); + ASSERT_EQ(1, tracer.getNumEvents()); + tracer.onEvent(TraceRaftAppendEntriesCallFailure, -456); + ASSERT_EQ(2, tracer.getNumEvents()); + tracer.onEvent(TraceError, -0xFFFFFFFFFFFFFFFLL); + ASSERT_EQ(3, tracer.getNumEvents()); + + ASSERT_EQ(0, tracer.countEvents(TraceAllocationActivity)); + ASSERT_EQ(2, tracer.countEvents(TraceRaftAppendEntriesCallFailure)); + ASSERT_EQ(1, tracer.countEvents(TraceError)); + + ASSERT_EQ(-456, tracer.getLastEventArgumentOrFail(TraceRaftAppendEntriesCallFailure)); + + ASSERT_EQ(-0xFFFFFFFFFFFFFFFLL, tracer.getLastEventArgumentOrFail(TraceError)); } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp b/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp index 0b8f89c533..dc7ba09077 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp @@ -6,20 +6,62 @@ #include #include +#include #include class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer { - const std::string id_; - - virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + struct EventLogEntry { - std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; - } + const uavcan::dynamic_node_id_server::TraceCode code; + const uavcan::int64_t argument; + + EventLogEntry(uavcan::dynamic_node_id_server::TraceCode arg_code, uavcan::int64_t arg_argument) + : code(arg_code) + , argument(arg_argument) + { } + }; + + const std::string id_; + std::list event_log_; public: EventTracer() { } EventTracer(const std::string& id) : id_(id) { } + + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + std::cout << "EVENT [" << id_ << "]\t" << code << "\t" << getEventName(code) << "\t" << argument << std::endl; + event_log_.push_back(EventLogEntry(code, argument)); + } + + unsigned countEvents(const uavcan::dynamic_node_id_server::TraceCode code) const + { + unsigned count = 0; + for (std::list::const_iterator it = event_log_.begin(); it != event_log_.end(); ++it) + { + count += (it->code == code) ? 1U : 0U; + } + return count; + } + + uavcan::int64_t getLastEventArgumentOrFail(const uavcan::dynamic_node_id_server::TraceCode code) const + { + for (std::list::const_reverse_iterator it = event_log_.rbegin(); it != event_log_.rend(); ++it) + { + if (it->code == code) + { + return it->argument; + } + } + + std::cout << "No such event in the event log, code " << code << ", log length " << event_log_.size() + << std::endl; + + throw std::runtime_error("EventTracer::getLastEventArgumentOrFail()"); + } + + unsigned getNumEvents() const { return static_cast(event_log_.size()); } }; From f0627423c629b8e21592e9709f0adbe1c8c67e37 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 17:15:18 +0300 Subject: [PATCH 083/268] NodeDiscoverer: Pausing querying GetNodeInfo if the node does not send NodeStatus --- .../protocol/dynamic_node_id_server/event.hpp | 2 +- .../node_discoverer.hpp | 24 ++++++++++++++----- 2 files changed, 19 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index d3d052aee1..e28800a6dd 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -74,7 +74,7 @@ enum TraceCode TraceDiscoveryGetNodeInfoFailure, // node ID TraceDiscoveryTimerStart, // interval in microseconds // 45 - TraceDiscoveryTimerStop, // reason code (see sources for details) + TraceDiscoveryTimerStop, // number of unknown nodes TraceDiscoveryGetNodeInfoRequest, // target node ID TraceDiscoveryNodeRestartDetected, // node ID Trace21, diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 7fd1474691..e1a9527101 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -74,12 +74,14 @@ class NodeDiscoverer : TimerBase struct NodeData { - uint32_t last_seen_uptime; - uint8_t num_get_node_info_attempts; + uint32_t last_seen_uptime; ///< Used for restart detection + uint8_t num_get_node_info_attempts; ///< Used to detect if the node doesn't support GetNodeInfo + bool updated_since_last_query; ///< Allows to pause trying to GetNodeInfo if the node is offline NodeData() : last_seen_uptime(0) , num_get_node_info_attempts(0) + , updated_since_last_query(false) { } }; @@ -120,7 +122,10 @@ class NodeDiscoverer : TimerBase { 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); + if (kv->value.updated_since_last_query) + { + 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. @@ -131,7 +136,8 @@ class NodeDiscoverer : TimerBase 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)) + (kv->value.last_seen_uptime >= largest_uptime) && + (kv->value.updated_since_last_query)) { largest_uptime = kv->value.last_seen_uptime; output = kv->key; @@ -230,7 +236,7 @@ class NodeDiscoverer : TimerBase if (!handler_.canDiscoverNewNodes()) { - trace(TraceDiscoveryTimerStop, 0); + trace(TraceDiscoveryTimerStop, node_map_.getSize()); stop(); return; } @@ -238,11 +244,16 @@ class NodeDiscoverer : TimerBase const NodeID node_id = pickNextNodeToQuery(); if (!node_id.isUnicast()) { - trace(TraceDiscoveryTimerStop, 1); + trace(TraceDiscoveryTimerStop, node_map_.getSize()); stop(); return; } + NodeData* const data = node_map_.access(node_id); + UAVCAN_ASSERT(data != NULL); + UAVCAN_ASSERT(data->updated_since_last_query); + data->updated_since_last_query = false; + trace(TraceDiscoveryGetNodeInfoRequest, node_id.get()); UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d", @@ -281,6 +292,7 @@ class NodeDiscoverer : TimerBase data->num_get_node_info_attempts = 0; } data->last_seen_uptime = msg.uptime_sec; + data->updated_since_last_query = true; if (!isRunning() && handler_.canDiscoverNewNodes()) { From c62b871f2025cbb5f98a92159f0f9b10ade8428e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 17:25:38 +0300 Subject: [PATCH 084/268] Revert "NodeDiscoverer: Pausing querying GetNodeInfo if the node does not send NodeStatus" This reverts commit f0627423c629b8e21592e9709f0adbe1c8c67e37. --- .../protocol/dynamic_node_id_server/event.hpp | 2 +- .../node_discoverer.hpp | 24 +++++-------------- 2 files changed, 7 insertions(+), 19 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index e28800a6dd..d3d052aee1 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -74,7 +74,7 @@ enum TraceCode TraceDiscoveryGetNodeInfoFailure, // node ID TraceDiscoveryTimerStart, // interval in microseconds // 45 - TraceDiscoveryTimerStop, // number of unknown nodes + TraceDiscoveryTimerStop, // reason code (see sources for details) TraceDiscoveryGetNodeInfoRequest, // target node ID TraceDiscoveryNodeRestartDetected, // node ID Trace21, diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index e1a9527101..7fd1474691 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -74,14 +74,12 @@ class NodeDiscoverer : TimerBase struct NodeData { - uint32_t last_seen_uptime; ///< Used for restart detection - uint8_t num_get_node_info_attempts; ///< Used to detect if the node doesn't support GetNodeInfo - bool updated_since_last_query; ///< Allows to pause trying to GetNodeInfo if the node is offline + uint32_t last_seen_uptime; + uint8_t num_get_node_info_attempts; NodeData() : last_seen_uptime(0) , num_get_node_info_attempts(0) - , updated_since_last_query(false) { } }; @@ -122,10 +120,7 @@ class NodeDiscoverer : TimerBase { const NodeMap::KVPair* const kv = node_map_.getByIndex(i); UAVCAN_ASSERT(kv != NULL); - if (kv->value.updated_since_last_query) - { - lowest_number_of_attempts = min(lowest_number_of_attempts, kv->value.num_get_node_info_attempts); - } + 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. @@ -136,8 +131,7 @@ class NodeDiscoverer : TimerBase 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) && - (kv->value.updated_since_last_query)) + (kv->value.last_seen_uptime >= largest_uptime)) { largest_uptime = kv->value.last_seen_uptime; output = kv->key; @@ -236,7 +230,7 @@ class NodeDiscoverer : TimerBase if (!handler_.canDiscoverNewNodes()) { - trace(TraceDiscoveryTimerStop, node_map_.getSize()); + trace(TraceDiscoveryTimerStop, 0); stop(); return; } @@ -244,16 +238,11 @@ class NodeDiscoverer : TimerBase const NodeID node_id = pickNextNodeToQuery(); if (!node_id.isUnicast()) { - trace(TraceDiscoveryTimerStop, node_map_.getSize()); + trace(TraceDiscoveryTimerStop, 1); stop(); return; } - NodeData* const data = node_map_.access(node_id); - UAVCAN_ASSERT(data != NULL); - UAVCAN_ASSERT(data->updated_since_last_query); - data->updated_since_last_query = false; - trace(TraceDiscoveryGetNodeInfoRequest, node_id.get()); UAVCAN_TRACE("dynamic_node_id_server::NodeDiscoverer", "Requesting GetNodeInfo from node %d", @@ -292,7 +281,6 @@ class NodeDiscoverer : TimerBase data->num_get_node_info_attempts = 0; } data->last_seen_uptime = msg.uptime_sec; - data->updated_since_last_query = true; if (!isRunning() && handler_.canDiscoverNewNodes()) { From 558171bf71793d02960f913a3cd1982cb588f768 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 17:50:36 +0300 Subject: [PATCH 085/268] NodeDiscoverer: fixes and test --- .../node_discoverer.hpp | 12 +- .../get_node_info_mock_server.hpp | 35 ++++ .../node_discoverer.cpp | 185 ++++++++++++++++++ 3 files changed, 226 insertions(+), 6 deletions(-) create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 7fd1474691..def1b9e9fe 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include #include @@ -89,7 +90,7 @@ class NodeDiscoverer : TimerBase * When this number of attempts has been made, the discoverer will give up and assume that the node * does not implement this service. */ - enum { MaxAttemptsToGetNodeInfo = 3 }; + enum { MaxAttemptsToGetNodeInfo = 5 }; /* * States @@ -217,6 +218,7 @@ class NodeDiscoverer : TimerBase if (data->num_get_node_info_attempts >= MaxAttemptsToGetNodeInfo) { finalizeNodeDiscovery(NULL, result.server_node_id); + // Warning: data pointer is invalidated now } } } @@ -230,15 +232,13 @@ class NodeDiscoverer : TimerBase if (!handler_.canDiscoverNewNodes()) { - trace(TraceDiscoveryTimerStop, 0); - stop(); - return; + return; // Timer must continue to run in order to not stuck when it unlocks } const NodeID node_id = pickNextNodeToQuery(); if (!node_id.isUnicast()) { - trace(TraceDiscoveryTimerStop, 1); + trace(TraceDiscoveryTimerStop, 0); stop(); return; } @@ -282,7 +282,7 @@ class NodeDiscoverer : TimerBase } data->last_seen_uptime = msg.uptime_sec; - if (!isRunning() && handler_.canDiscoverNewNodes()) + if (!isRunning()) { trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec()); startPeriodic(get_node_info_client_.getRequestTimeout()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp b/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp new file mode 100644 index 0000000000..e1da5b02ba --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/get_node_info_mock_server.hpp @@ -0,0 +1,35 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +class GetNodeInfoMockServer +{ + typedef uavcan::MethodBinder&, + uavcan::protocol::GetNodeInfo::Response&) const> + GetNodeInfoCallback; + + uavcan::ServiceServer server_; + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::protocol::GetNodeInfo::Response& res) const + { + res = response; + std::cout << "GET NODE INFO:\nREQUEST\n" << req << "RESPONSE\n" << res << std::endl; + } + +public: + uavcan::protocol::GetNodeInfo::Response response; + + GetNodeInfoMockServer(uavcan::INode& node) : server_(node) { } + + int start() { return server_.start(GetNodeInfoCallback(this, &GetNodeInfoMockServer::handleRequest)); } +}; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp new file mode 100644 index 0000000000..233c44a549 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -0,0 +1,185 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + +#include +#include +#include +#include +#include "event_tracer.hpp" +#include "get_node_info_mock_server.hpp" +#include "../helpers.hpp" + +using namespace uavcan::dynamic_node_id_server; + + +class NodeDiscoveryHandler : public uavcan::dynamic_node_id_server::INodeDiscoveryHandler +{ +public: + struct NodeInfo + { + UniqueID unique_id; + uavcan::NodeID node_id; + bool committed; + + NodeInfo() : committed(false) { } + }; + + bool can_discover; + std::vector nodes; + + NodeDiscoveryHandler() : can_discover(false) { } + + virtual bool canDiscoverNewNodes() const + { + return can_discover; + } + + virtual NodeAwareness checkNodeAwareness(uavcan::NodeID node_id) const + { + const NodeInfo* const ni = const_cast(this)->findNode(node_id); + if (ni == NULL) + { + return NodeAwarenessUnknown; + } + return ni->committed ? NodeAwarenessKnownAndCommitted : NodeAwarenessKnownButNotCommitted; + } + + virtual void handleNewNodeDiscovery(const UniqueID* unique_id_or_null, uavcan::NodeID node_id) + { + NodeInfo info; + if (unique_id_or_null != NULL) + { + info.unique_id = *unique_id_or_null; + } + info.node_id = node_id; + nodes.push_back(info); + } + + NodeInfo* findNode(const UniqueID& unique_id) + { + for (unsigned i = 0; i < nodes.size(); i++) + { + if (nodes.at(i).unique_id == unique_id) + { + return &nodes.at(i); + } + } + return NULL; + } + + NodeInfo* findNode(const uavcan::NodeID& node_id) + { + for (unsigned i = 0; i < nodes.size(); i++) + { + if (nodes.at(i).node_id == node_id) + { + return &nodes.at(i); + } + } + return NULL; + } +}; + + +TEST(dynamic_node_id_server_NodeDiscoverer, Basic) +{ + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + EventTracer tracer; + InterlinkedTestNodesWithSysClock nodes; + NodeDiscoveryHandler handler; + + NodeDiscoverer disc(nodes.a, tracer, handler); + + /* + * Initialization + */ + ASSERT_LE(0, disc.init()); + + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus, discovery is disabled + */ + handler.can_discover = false; + + uavcan::Publisher node_status_pub(nodes.b); + ASSERT_LE(0, node_status_pub.init()); + + uavcan::protocol::NodeStatus node_status; + node_status.status_code = node_status.STATUS_OK; // Status will be ignored anyway + node_status.uptime_sec = 0; + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); // The timer runs as long as there are unknown nodes + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); // Querying is disabled! + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryGetNodeInfoFailure)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Enabling discovery - the querying will continue despite the fact that NodeStatus messages are not arriving + */ + handler.can_discover = true; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + 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_TRUE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus + */ + node_status.uptime_sec += 5U; + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + 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_TRUE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus, discovery is enabled, GetNodeInfo mock server is initialized + */ + 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)); + + 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(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Checking the results + */ + ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id)); + ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get()); +} From 8df1cfe1d9e9f073c19ad54722cb4b2e176f8be0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 18:26:42 +0300 Subject: [PATCH 086/268] NodeDiscoverer: another test --- .../node_discoverer.hpp | 33 ++++++- .../node_discoverer.cpp | 86 +++++++++++++++++++ 2 files changed, 116 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index def1b9e9fe..ba3d363921 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -183,11 +183,38 @@ class NodeDiscoverer : TimerBase } } + NodeID pickNextNodeToQueryAndCleanupMap() + { + NodeID node_id; + do + { + node_id = pickNextNodeToQuery(); + if (node_id.isUnicast()) + { + if (needToQuery(node_id)) + { + return node_id; + } + else + { + node_map_.remove(node_id); + } + } + } + while (node_id.isUnicast()); + return NodeID(); + } + void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) { trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == NULL) ? 0U : 0x100U)); node_map_.remove(node_id); - if (needToQuery(node_id)) // Making sure the server is still interested + /* + * It is paramount to check if the server is still interested to receive this data. + * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with + * duplicate node ID in the log. + */ + if (needToQuery(node_id)) { handler_.handleNewNodeDiscovery(unique_id_or_null, node_id); } @@ -235,7 +262,7 @@ class NodeDiscoverer : TimerBase return; // Timer must continue to run in order to not stuck when it unlocks } - const NodeID node_id = pickNextNodeToQuery(); + const NodeID node_id = pickNextNodeToQueryAndCleanupMap(); if (!node_id.isUnicast()) { trace(TraceDiscoveryTimerStop, 0); @@ -285,7 +312,7 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec()); - startPeriodic(get_node_info_client_.getRequestTimeout()); + startPeriodic(get_node_info_client_.getRequestTimeout() * 2); } } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 233c44a549..bdea141a06 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -183,3 +183,89 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) ASSERT_TRUE(handler.findNode(get_node_info_server.response.hardware_version.unique_id)); ASSERT_EQ(2, handler.findNode(get_node_info_server.response.hardware_version.unique_id)->node_id.get()); } + + +TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) +{ + using namespace uavcan::protocol::dynamic_node_id::server; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + + EventTracer tracer; + InterlinkedTestNodesWithSysClock nodes; + NodeDiscoveryHandler handler; + + NodeDiscoverer disc(nodes.a, tracer, handler); + + /* + * Initialization + */ + ASSERT_LE(0, disc.init()); + + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Publishing NodeStatus once to trigger querying + * Querying for 2 seconds, no responses will be sent (there's no server) + */ + handler.can_discover = true; + + uavcan::Publisher node_status_pub(nodes.b); + ASSERT_LE(0, node_status_pub.init()); + + uavcan::protocol::NodeStatus node_status; + node_status.status_code = node_status.STATUS_OK; // Status will be ignored anyway + node_status.uptime_sec = 10; // Nonzero + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400)); + + 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(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Emulating node restart + */ + node_status.status_code = node_status.STATUS_OK; // Status will be ignored anyway + node_status.uptime_sec = 9; // Less than previous + ASSERT_LE(0, node_status_pub.broadcast(node_status)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400)); + + 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(0, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_TRUE(disc.hasUnknownNodes()); + + /* + * Waiting for timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(3400)); + + 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(1, tracer.countEvents(TraceDiscoveryNodeFinalized)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNodeRestartDetected)); + ASSERT_FALSE(disc.hasUnknownNodes()); + + /* + * Checking the results + */ + ASSERT_TRUE(handler.findNode(UniqueID())); + ASSERT_EQ(2, handler.findNode(UniqueID())->node_id.get()); +} From 2868fd5712e6e06f0267db0e85d3c34398101b00 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 18:36:50 +0300 Subject: [PATCH 087/268] NodeDiscoverer test fix --- .../protocol/dynamic_node_id_server/node_discoverer.hpp | 2 +- .../test/protocol/dynamic_node_id_server/node_discoverer.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index ba3d363921..65a8b50822 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -311,8 +311,8 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { - trace(TraceDiscoveryTimerStart, get_node_info_client_.getRequestTimeout().toUSec()); startPeriodic(get_node_info_client_.getRequestTimeout() * 2); + trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index bdea141a06..427213bad1 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -134,7 +134,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) */ handler.can_discover = true; - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -149,7 +149,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) node_status.uptime_sec += 5U; ASSERT_LE(0, node_status_pub.broadcast(node_status)); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 92c8944e497e47a530facce759e53c051419d36d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 18:52:03 +0300 Subject: [PATCH 088/268] Too many tracepoints? No such thing. --- .../protocol/dynamic_node_id_server/event.hpp | 4 ++-- .../dynamic_node_id_server/node_discoverer.hpp | 14 ++++++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index d3d052aee1..eb775ecd7b 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -77,7 +77,7 @@ enum TraceCode TraceDiscoveryTimerStop, // reason code (see sources for details) TraceDiscoveryGetNodeInfoRequest, // target node ID TraceDiscoveryNodeRestartDetected, // node ID - Trace21, + TraceDiscoveryNodeRemoved, // node ID Trace22, // 50 @@ -148,7 +148,7 @@ public: "DiscoveryTimerStop", "DiscoveryGetNodeInfoRequest", "DiscoveryNodeRestartDetected", - "", + "DiscoveryNodeRemoved", "" }; uavcan::StaticAssert::check(); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 65a8b50822..d98d55f09f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -111,6 +111,12 @@ class NodeDiscoverer : TimerBase INode& getNode() { return node_status_sub_.getNode(); } + void removeNode(const NodeID node_id) + { + node_map_.remove(node_id); + trace(TraceDiscoveryNodeRemoved, node_id.get()); + } + NodeID pickNextNodeToQuery() const { const unsigned node_map_size = node_map_.getSize(); @@ -166,14 +172,14 @@ class NodeDiscoverer : TimerBase } else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownButNotCommitted) { - node_map_.remove(node_id); + removeNode(node_id); return false; } else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted) { trace(TraceDiscoveryCommitCacheUpdated, node_id.get()); committed_node_mask_[node_id.get()] = true; - node_map_.remove(node_id); + removeNode(node_id); return false; } else @@ -197,7 +203,7 @@ class NodeDiscoverer : TimerBase } else { - node_map_.remove(node_id); + removeNode(node_id); } } } @@ -208,7 +214,7 @@ class NodeDiscoverer : TimerBase void finalizeNodeDiscovery(const UniqueID* unique_id_or_null, NodeID node_id) { trace(TraceDiscoveryNodeFinalized, node_id.get() | ((unique_id_or_null == NULL) ? 0U : 0x100U)); - node_map_.remove(node_id); + removeNode(node_id); /* * It is paramount to check if the server is still interested to receive this data. * Otherwise, if the node appeared in the log while we were waiting for response, we'd end up with From 0253933f75edf0059a7bee2afa7a290df9562534 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 11 May 2015 21:54:16 +0300 Subject: [PATCH 089/268] Linux driver: added system_utils.hpp with a helper class that reads Linux machine ID --- libuavcan_drivers/linux/CMakeLists.txt | 4 +- .../linux/apps/test_system_utils.cpp | 39 +++++ .../include/uavcan_linux/system_utils.hpp | 136 ++++++++++++++++++ .../include/uavcan_linux/uavcan_linux.hpp | 1 + 4 files changed, 179 insertions(+), 1 deletion(-) create mode 100644 libuavcan_drivers/linux/apps/test_system_utils.cpp create mode 100644 libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 51cbcf1c97..a88c7da13a 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -52,9 +52,11 @@ target_link_libraries(test_node ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) add_executable(test_time_sync apps/test_time_sync.cpp) 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}) + # # Tools -# Someday they will be replaced with Python scripts (pyuavcan is not finished at the moment) # add_executable(uavcan_status_monitor apps/uavcan_status_monitor.cpp) target_link_libraries(uavcan_status_monitor ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) diff --git a/libuavcan_drivers/linux/apps/test_system_utils.cpp b/libuavcan_drivers/linux/apps/test_system_utils.cpp new file mode 100644 index 0000000000..719768fda5 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_system_utils.cpp @@ -0,0 +1,39 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "debug.hpp" + +int main(int argc, const char** argv) +{ + try + { + const std::vector iface_names(argv + 1, argv + argc); + + const auto res = uavcan_linux::MachineIDReader(iface_names).readAndGetLocation(); + + const auto original_flags = std::cout.flags(); + + for (auto x : res.first) + { + std::cout << std::hex << std::setw(2) << std::setfill('0') << int(x); + } + + std::cout.width(0); + std::cout.flags(original_flags); + + std::cout << std::endl; + + std::cout << res.second << std::endl; + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Exception: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp new file mode 100644 index 0000000000..14aa91ad63 --- /dev/null +++ b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan_linux +{ +/** + * This class can find and read machine ID from a text file, represented as 32-char (16-byte) long hexadecimal string, + * possibly with separators (like dashes or colons). If the available ID is more than 16 bytes, extra bytes will be + * ignored. A shorter ID will not be accepted as valid. + * In order to be read, the ID must be located on the first line of the file and must not contain any whitespace + * characters. + * + * Examples of valid ID: + * 0123456789abcdef0123456789abcdef + * 20CE0b1E-8C03-07C8-13EC-00242C491652 + */ +class MachineIDReader +{ +public: + static constexpr int MachineIDSize = 16; + + typedef std::array MachineID; + + static std::vector getDefaultSearchLocations() + { + return + { + "/etc/machine-id", + "/var/lib/dbus/machine-id", + "/sys/class/dmi/id/product_uuid" + }; + } + +private: + const std::vector search_locations_; + + static std::vector mergeLists(const std::vector& a, const std::vector& b) + { + std::vector ab; + ab.reserve(a.size() + b.size()); + ab.insert(ab.end(), a.begin(), a.end()); + ab.insert(ab.end(), b.begin(), b.end()); + return ab; + } + + bool tryRead(const std::string& location, MachineID& out_id) const + { + /* + * Reading the file + */ + std::string token; + try + { + std::ifstream infile(location); + infile >> token; + } + catch (std::exception&) + { + return false; + } + + /* + * Preprocessing the input string - convert to lowercase, remove all non-hex characters, limit to 32 chars + */ + std::transform(token.begin(), token.end(), token.begin(), [](char x) { return std::tolower(x); }); + token.erase(std::remove_if(token.begin(), token.end(), + [](char x){ return (x < 'a' || x > 'f') && !std::isdigit(x); }), + token.end()); + + if (token.length() < (MachineIDSize * 2)) + { + return false; + } + token.resize(MachineIDSize * 2); // Truncating + + /* + * Parsing the string as hex bytes + */ + auto sym = std::begin(token); + for (auto& byte : out_id) + { + assert(sym != std::end(token)); + byte = std::stoi(std::string{*sym++, *sym++}, nullptr, 16); + } + + return true; + } + +public: + /** + * This class can use extra seach locations. If provided, they will be checked first, before default ones. + */ + MachineIDReader(const std::vector& extra_search_locations = {}) + : search_locations_(mergeLists(extra_search_locations, getDefaultSearchLocations())) + { } + + /** + * Just like @ref readAndGetLocation(), but this one doesn't return location where this ID was obtained from. + */ + MachineID read() const { return readAndGetLocation().first; } + + /** + * This function checks available search locations and reads the ID from the first valid location. + * It returns std::pair<> with ID and the file path where it was read from. + * In case if none of the search locations turned out to be valid, @ref uavcan_linux::Exception will be thrown. + */ + std::pair readAndGetLocation() const + { + for (auto x : search_locations_) + { + auto out = MachineID(); + if (tryRead(x, out)) + { + return {out, x}; + } + } + throw Exception("Failed to read machine ID"); + } +}; + + +} diff --git a/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp b/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp index 3d68a42e99..4886fa66bd 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/uavcan_linux.hpp @@ -9,3 +9,4 @@ #include #include #include +#include From 9efd1ac7aecd34a2f938b2eac1938d1eab4b3825 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 12 May 2015 12:59:22 +0300 Subject: [PATCH 090/268] NodeIDSelector: allocating only in the range [1, 125], unless the node explicitly requested higher node ID --- .../protocol/dynamic_node_id_server/node_id_selector.hpp | 8 ++++---- libuavcan/include/uavcan/transport/transfer.hpp | 1 + libuavcan/src/transport/uc_transfer.cpp | 1 + 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp index 2af730a67e..97c86ab21b 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp @@ -35,12 +35,12 @@ public: /** * Reutrns a default-constructed (invalid) node ID if a free one could not be found. */ - NodeID findFreeNodeID(const NodeID preferred_node_id) const + NodeID findFreeNodeID(const NodeID preferred) const { - uint8_t candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; // Up - while (candidate <= NodeID::Max) + while (candidate <= NodeID::MaxRecommendedForRegularNodes) { if (!(owner_->*is_node_id_taken_)(candidate)) { @@ -49,7 +49,7 @@ public: candidate++; } - candidate = preferred_node_id.isUnicast() ? preferred_node_id.get() : NodeID::Max; + candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; candidate--; // This has been tested already // Down diff --git a/libuavcan/include/uavcan/transport/transfer.hpp b/libuavcan/include/uavcan/transport/transfer.hpp index c1cdc38abb..f452c344bc 100644 --- a/libuavcan/include/uavcan/transport/transfer.hpp +++ b/libuavcan/include/uavcan/transport/transfer.hpp @@ -129,6 +129,7 @@ class UAVCAN_EXPORT NodeID public: static const uint8_t BitLen = 7U; static const uint8_t Max = (1U << BitLen) - 1U; + static const uint8_t MaxRecommendedForRegularNodes = Max - 2; static const NodeID Broadcast; NodeID() : value_(ValueInvalid) { } diff --git a/libuavcan/src/transport/uc_transfer.cpp b/libuavcan/src/transport/uc_transfer.cpp index 51c850ea02..da1897e842 100644 --- a/libuavcan/src/transport/uc_transfer.cpp +++ b/libuavcan/src/transport/uc_transfer.cpp @@ -21,6 +21,7 @@ const uint8_t NodeID::ValueBroadcast; const uint8_t NodeID::ValueInvalid; const uint8_t NodeID::BitLen; const uint8_t NodeID::Max; +const uint8_t NodeID::MaxRecommendedForRegularNodes; const NodeID NodeID::Broadcast(ValueBroadcast); /** From 93376316b8573e4de8708bee6099411f6c845f4a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 12 May 2015 21:48:30 +0300 Subject: [PATCH 091/268] Using static receivers in NodeDiscoverer --- .../protocol/dynamic_node_id_server/node_discoverer.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index d98d55f09f..0f96708cd5 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -92,6 +92,8 @@ class NodeDiscoverer : TimerBase */ enum { MaxAttemptsToGetNodeInfo = 5 }; + enum { NumNodeStatusStaticReceivers = 64 }; + /* * States */ @@ -102,7 +104,7 @@ class NodeDiscoverer : TimerBase NodeMap node_map_; ///< Will not work in UAVCAN_TINY ServiceClient get_node_info_client_; - Subscriber node_status_sub_; + Subscriber node_status_sub_; /* * Methods From adeb1ef58f863d56f59277133536e5d1494d7294 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 12 May 2015 10:52:32 -1000 Subject: [PATCH 092/268] Added POSIX file storage backend --- .../posix_tools/file_storage_backend.hpp | 137 ++++++++++++++++++ 1 file changed, 137 insertions(+) create mode 100644 libuavcan_drivers/include/posix_tools/file_storage_backend.hpp diff --git a/libuavcan_drivers/include/posix_tools/file_storage_backend.hpp b/libuavcan_drivers/include/posix_tools/file_storage_backend.hpp new file mode 100644 index 0000000000..67762606ac --- /dev/null +++ b/libuavcan_drivers/include/posix_tools/file_storage_backend.hpp @@ -0,0 +1,137 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_posix +{ +namespace dynamic_node_id_server +{ +/** + * This interface implements a POSIX compliant IStorageBackend interface + */ +class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBackend +{ + /** + * Maximum length of full path including / and key max + */ + + enum { MaxPathLength = 128 }; + + + /** + * This type is used for the path + */ + typedef uavcan::Array, + uavcan::ArrayModeDynamic, MaxPathLength> PathString; + + + PathString base_path; + +public: + + FileStorageBackend() { } + + + virtual String get(const String& key) const + { + using namespace std; + PathString path = base_path.c_str(); + path += key; + String value; + int fd = open(path.c_str(), O_RDONLY); + if (fd >= 0) + { + char buffer[MaxStringLength + 1]; + memset(buffer, 0, sizeof(buffer)); + int len = read(fd, buffer, MaxStringLength); + close(fd); + if (len > 0) + { + for (int i = 0; i < len; i++) + { + if (buffer[i] == ' ' || buffer[i] == '\n' || buffer[i] == '\r' ) + { + buffer[i] = '\0'; + break; + } + } + } + } + return value; + + } + + virtual void set(const String& key, const String& value) + { + using namespace std; + PathString path = base_path.c_str(); + path += key; + int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC); + if (fd >= 0) + { + write(fd, value.c_str(), value.size()); + close(fd); + } + } + + /** + * Initializes the File based back end storage by passing to a path to + * the directory where the key named files will be stored. + * This the return result should be 0 on success. + * If it is -EFBIG then the the path name is too long to + * Accommodate the trailing slash and max key length; + * + */ + + int init(const String & path) + { + using namespace std; + + int rv = -EINVAL; + + if (path.size() > 0) + { + + base_path = path.c_str(); + + if (base_path.back() == '/') + { + base_path.pop_back(); + } + + struct stat sb; + + if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) + { + rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + base_path.push_back('/'); + if ((base_path.size() + MaxStringLength) > MaxPathLength) + { + rv = -EFBIG; + } + } + return rv; + + } + +}; + +} +} From 08d96ef329fe2e12ee6149787b4aca69b1f48df1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 12 May 2015 23:56:13 +0300 Subject: [PATCH 093/268] NodeStatusMonitor API update --- libuavcan/include/uavcan/protocol/node_status_monitor.hpp | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index cf0dd7fd9b..60350ef6ff 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -110,6 +110,10 @@ 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&) { const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; @@ -134,7 +138,6 @@ private: } } -protected: /** * Called when a node becomes online, changes status or goes offline. * Refer to uavcan.protocol.NodeStatus for the offline timeout value. From e43cf6b553f748d8ad118d408c740a4f9cba767f Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 12 May 2015 13:18:59 -1000 Subject: [PATCH 094/268] Added POSIX File event tracer --- libuavcan_drivers/posix_tools/include.mk | 7 ++ .../include/posix_tools/file_event_tracer.hpp | 76 +++++++++++++++++++ .../posix_tools/file_storage_backend.hpp | 10 +-- 3 files changed, 88 insertions(+), 5 deletions(-) create mode 100644 libuavcan_drivers/posix_tools/include.mk create mode 100644 libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp rename libuavcan_drivers/{ => posix_tools}/include/posix_tools/file_storage_backend.hpp (93%) diff --git a/libuavcan_drivers/posix_tools/include.mk b/libuavcan_drivers/posix_tools/include.mk new file mode 100644 index 0000000000..84ad8e1744 --- /dev/null +++ b/libuavcan_drivers/posix_tools/include.mk @@ -0,0 +1,7 @@ +# +# Copyright (C) 2015 David Sidrane +# + +LIBUAVCAN_POSIX_DIR := $(dir $(lastword $(MAKEFILE_LIST))) + +LIBUAVCAN_POSIX_INC := $(LIBUAVCAN_POSIX_DIR)include/ diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp new file mode 100644 index 0000000000..1d69bd932e --- /dev/null +++ b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp @@ -0,0 +1,76 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include + +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::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) + { + path_ = path.c_str(); + rv = open(path_.c_str(), O_WRONLY | O_CREAT | O_TRUNC); + close(rv); + } + return rv; + } + +}; + +} +} diff --git a/libuavcan_drivers/include/posix_tools/file_storage_backend.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp similarity index 93% rename from libuavcan_drivers/include/posix_tools/file_storage_backend.hpp rename to libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp index 67762606ac..5595f3b478 100644 --- a/libuavcan_drivers/include/posix_tools/file_storage_backend.hpp +++ b/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp @@ -94,16 +94,16 @@ public: * Initializes the File based back end storage by passing to a path to * the directory where the key named files will be stored. * This the return result should be 0 on success. - * If it is -EFBIG then the the path name is too long to + * If it is -ErrInvalidConfiguration then the the path name is too long to * Accommodate the trailing slash and max key length; * */ - int init(const String & path) + int init(const PathString & path) { using namespace std; - int rv = -EINVAL; + int rv = -uavcan::ErrInvalidParam; if (path.size() > 0) { @@ -115,8 +115,8 @@ public: base_path.pop_back(); } + rv = 0; struct stat sb; - if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) { rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); @@ -124,7 +124,7 @@ public: base_path.push_back('/'); if ((base_path.size() + MaxStringLength) > MaxPathLength) { - rv = -EFBIG; + rv = -uavcan::ErrInvalidConfiguration; } } return rv; From 416a5e69a484ddcd450f30719a7274df0f088895 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 12 May 2015 14:46:49 -1000 Subject: [PATCH 095/268] Debuged - missing return value --- .../include/posix_tools/file_event_tracer.hpp | 7 +++++-- .../include/posix_tools/file_storage_backend.hpp | 11 +++++++---- 2 files changed, 12 insertions(+), 6 deletions(-) diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp index 1d69bd932e..e88e622c85 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp +++ b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp @@ -63,9 +63,12 @@ public: if (path.size() > 0) { + rv = 0; path_ = path.c_str(); - rv = open(path_.c_str(), O_WRONLY | O_CREAT | O_TRUNC); - close(rv); + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC); + if ( fd >= 0) { + close(fd); + } } return rv; } diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp index 5595f3b478..55292e7ef6 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp +++ b/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp @@ -71,6 +71,7 @@ public: break; } } + value = buffer; } } return value; @@ -121,10 +122,12 @@ public: { rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); } - base_path.push_back('/'); - if ((base_path.size() + MaxStringLength) > MaxPathLength) - { - rv = -uavcan::ErrInvalidConfiguration; + if (rv >= 0 ) { + base_path.push_back('/'); + if ((base_path.size() + MaxStringLength) > MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } } } return rv; From 37ace75abc0e9b90b54faa75e43284f219e1dd59 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 12 May 2015 15:00:32 -1000 Subject: [PATCH 096/268] Formated, tested --- .../include/posix_tools/file_event_tracer.hpp | 3 ++- .../include/posix_tools/file_storage_backend.hpp | 13 +++++++------ 2 files changed, 9 insertions(+), 7 deletions(-) diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp index e88e622c85..719e34694d 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp +++ b/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp @@ -66,7 +66,8 @@ public: rv = 0; path_ = path.c_str(); int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC); - if ( fd >= 0) { + if ( fd >= 0) + { close(fd); } } diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp b/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp index 55292e7ef6..e5233420cf 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp +++ b/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp @@ -122,12 +122,13 @@ public: { rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); } - if (rv >= 0 ) { - base_path.push_back('/'); - if ((base_path.size() + MaxStringLength) > MaxPathLength) - { - rv = -uavcan::ErrInvalidConfiguration; - } + if (rv >= 0 ) + { + base_path.push_back('/'); + if ((base_path.size() + MaxStringLength) > MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } } } return rv; From a548d8311ce9c72562327bc92cd02b7ee2f38f2c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 16:39:22 +0300 Subject: [PATCH 097/268] Testing framework: added emulateSingleFrameBroadcastTransfer() --- libuavcan/test/protocol/helpers.hpp | 28 +++++++++++++++++++ .../test/protocol/node_status_monitor.cpp | 24 +--------------- 2 files changed, 29 insertions(+), 23 deletions(-) diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index df8085566e..171fb66871 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -103,3 +103,31 @@ struct BackgroundSpinner : uavcan::TimerBase spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1)); } }; + +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriverMock& 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)); + + for (uint8_t i = 0; i < can.getNumIfaces(); i++) + { + can.ifaces.at(i).pushRx(can_frame); + } +} diff --git a/libuavcan/test/protocol/node_status_monitor.cpp b/libuavcan/test/protocol/node_status_monitor.cpp index bfb97b0053..4f47b33252 100644 --- a/libuavcan/test/protocol/node_status_monitor.cpp +++ b/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); } From bc00899e7080d5cc7f5e25c4d4c6d41482b33005 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 17:50:45 +0300 Subject: [PATCH 098/268] Testing tooling fix --- libuavcan/test/node/test_node.hpp | 5 +++++ libuavcan/test/protocol/helpers.hpp | 9 +++------ libuavcan/test/transport/can/can.hpp | 8 ++++++++ 3 files changed, 16 insertions(+), 6 deletions(-) diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index c8b2055834..5aaa60288d 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -122,6 +122,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; } diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index 171fb66871..e0fb164f2f 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -104,8 +104,8 @@ struct BackgroundSpinner : uavcan::TimerBase } }; -template -static inline void emulateSingleFrameBroadcastTransfer(CanDriverMock& can, uavcan::NodeID node_id, +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id, const MessageType& message, uavcan::TransferID tid) { uavcan::StaticTransferBuffer<100> buffer; @@ -126,8 +126,5 @@ static inline void emulateSingleFrameBroadcastTransfer(CanDriverMock& can, uavca 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); - } + can.pushRxToAllIfaces(can_frame); } diff --git a/libuavcan/test/transport/can/can.hpp b/libuavcan/test/transport/can/can.hpp index 6557d16533..00a038df9e 100644 --- a/libuavcan/test/transport/can/can.hpp +++ b/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); From 842319a2905cb8cb2778b92be6a44feeca6ae084 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 21:59:43 +0300 Subject: [PATCH 099/268] Initial implementation of NodeInfoRetriever; fixes pending --- .../uavcan/protocol/node_info_retriever.hpp | 340 ++++++++++++++++++ .../test/protocol/node_info_retriever.cpp | 139 +++++++ 2 files changed, 479 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/node_info_retriever.hpp create mode 100644 libuavcan/test/protocol/node_info_retriever.cpp diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp new file mode 100644 index 0000000000..542210e113 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -0,0 +1,340 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_NODE_INFO_RETRIEVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Classes that need to receive GetNodeInfo responses should implement this interface. + */ +class 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& 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. + * Events from this class can be routed to many subscribers. + */ +class NodeInfoRetriever : protected NodeStatusMonitor +{ +public: + enum { MaxNumRequestAttempts = 254 }; + enum { DefaultNumRequestAttempts = 30 }; + enum { UnlimitedRequestAttempts = 0 }; + +private: + typedef MethodBinder&)> + 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::check(); +#endif + } + }; + + /* + * Callers are used with removeWhere() predicate. They don't actually remove anything. + */ + 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, bool) + { + UAVCAN_ASSERT(key != NULL); + key->handleNodeInfoRetrieved(node_id, node_info); + return false; + } + }; + + template + 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, bool) + { + UAVCAN_ASSERT(key != NULL); + (key->*method)(event); + return false; + } + }; + + /* + * State + */ + Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Map listeners_; // Only keys are used + + ServiceClient get_node_info_client_; + + mutable uint8_t last_picked_node_; + + uint8_t num_attempts_; + + /* + * Methods + */ + const Entry& getEntry(NodeID node_id) const { return const_cast(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]; + } + + NodeID pickNextNodeToQuery() const + { + 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 && entry.updated_since_last_attempt) + { + 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& event) // 2 Hz + { + NodeStatusMonitor::handleTimerEvent(event); + + if (!get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time + { + const NodeID next = pickNextNodeToQuery(); // Typ. 1 Hz + if (next.isUnicast()) + { + 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"); + } + } + } + } + + 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)); + } + + listeners_.removeWhere( + GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); + } + + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + Entry& entry = getEntry(msg.getSrcNodeID()); + + if (msg.uptime_sec < entry.uptime_sec) + { + entry.request_needed = true; + entry.num_attempts_made = 0; + } + entry.uptime_sec = msg.uptime_sec; + entry.updated_since_last_attempt = true; + + listeners_.removeWhere(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + Entry& entry = getEntry(result.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.response.status.uptime_sec; + entry.request_needed = false; + listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.server_node_id, result.response)); + } + else + { + if (num_attempts_ != UnlimitedRequestAttempts) + { + entry.num_attempts_made++; + if (entry.num_attempts_made >= num_attempts_) + { + entry.request_needed = false; + listeners_.removeWhere(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.server_node_id)); + } + } + } + } + +public: + NodeInfoRetriever(INode& node) + : NodeStatusMonitor(node) + , listeners_(node.getAllocator()) + , get_node_info_client_(node) + , 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)); + + return 0; + } + + /** + * Adds one listener to the set. + * May return -ErrMemory if there's no space to add the listener. + */ + int addListener(INodeInfoListener* listener) + { + UAVCAN_ASSERT(listener != NULL); + bool* value = listeners_.insert(listener, true); + return (value == NULL) ? -ErrMemory : 0; + } + + /** + * Removes the listener. + * If the listener was not registered, nothing will be done. + */ + void removeListener(INodeInfoListener* listener) + { + UAVCAN_ASSERT(listener != NULL); + listeners_.remove(listener); + } + + /** + * 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(MaxNumRequestAttempts), num); + } +}; + +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp new file mode 100644 index 0000000000..04f2d346ac --- /dev/null +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -0,0 +1,139 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + +#include +#include +#include +#include +#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 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) + { + (void)event; + status_change_cnt++; + } + + virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& msg) + { + std::cout << msg << std::endl; + status_message_cnt++; + } +}; + + +TEST(NodeInfoRetriever, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << std::endl; + + std::auto_ptr provider(new uavcan::NodeStatusProvider(nodes.b)); + + NodeInfoListener listener; + + /* + * Initialization + */ + ASSERT_LE(0, retr.start()); + + retr.removeListener(&listener); // Does nothing + retr.addListener(&listener); + + 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()); + + /* + * Waiting for discovery + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); + + 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 + */ + 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(2100)); + + 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); + + // TODO finish the test when the logic is fixed +} From 2b0d669d7fe6476d381326e47ef4210c0a3ad370 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 22:02:02 +0300 Subject: [PATCH 100/268] Revert "NodeStatusMonitor API update" This reverts commit 08d96ef329fe2e12ee6149787b4aca69b1f48df1. --- libuavcan/include/uavcan/protocol/node_status_monitor.hpp | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index 60350ef6ff..cf0dd7fd9b 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp @@ -110,10 +110,6 @@ 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&) { const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; @@ -138,6 +134,7 @@ protected: } } +protected: /** * Called when a node becomes online, changes status or goes offline. * Refer to uavcan.protocol.NodeStatus for the offline timeout value. From e2ef4a4518a0064a9e86b3427b7f83c4cdb322f1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 22:32:23 +0300 Subject: [PATCH 101/268] Restructured NodeStatusMonitor and NodeInfoRetriever - TimerBase is not inherited by the monitor now because that was a suboptimal solution for a class designed for inheritance --- .../uavcan/protocol/node_info_retriever.hpp | 62 +++++++++++++++---- .../uavcan/protocol/node_status_monitor.hpp | 20 +++--- 2 files changed, 62 insertions(+), 20 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 542210e113..4c4245b6c8 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -9,6 +9,7 @@ #include #include #include +#include #include #include @@ -17,7 +18,7 @@ namespace uavcan /** * Classes that need to receive GetNodeInfo responses should implement this interface. */ -class INodeInfoListener +class UAVCAN_EXPORT INodeInfoListener { public: /** @@ -63,7 +64,8 @@ public: * implement the GetNodeInfo service. * Events from this class can be routed to many subscribers. */ -class NodeInfoRetriever : protected NodeStatusMonitor +class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor + , TimerBase { public: enum { MaxNumRequestAttempts = 254 }; @@ -150,6 +152,8 @@ private: /* * Methods */ + static MonotonicDuration getTimerPollInterval() { return MonotonicDuration::fromMSec(100); } + const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } Entry& getEntry(NodeID node_id) { @@ -160,6 +164,14 @@ private: return entries_[node_id.get() - 1]; } + void startTimerIfNotRunning() + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(getTimerPollInterval()); + } + } + NodeID pickNextNodeToQuery() const { for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin @@ -182,22 +194,38 @@ private: return NodeID(); // No node could be found } - virtual void handleTimerEvent(const TimerEvent& event) // 2 Hz + virtual void handleTimerEvent(const TimerEvent&) { - NodeStatusMonitor::handleTimerEvent(event); - - if (!get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time + if (get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time { - const NodeID next = pickNextNodeToQuery(); // Typ. 1 Hz - if (next.isUnicast()) + return; + } + + const NodeID next = pickNextNodeToQuery(); + if (next.isUnicast()) + { + getEntry(next).updated_since_last_attempt = false; + const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request()); + if (res < 0) { - 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 + { + bool requests_needed = false; + for (uint8_t i = 1; i <= NodeID::Max; i++) + { + if (getEntry(i).request_needed) { - get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call"); + requests_needed = true; + break; } } + if (!requests_needed) + { + TimerBase::stop(); + } } } @@ -218,6 +246,11 @@ private: 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_.removeWhere( @@ -232,6 +265,8 @@ private: { entry.request_needed = true; entry.num_attempts_made = 0; + + startTimerIfNotRunning(); } entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; @@ -272,6 +307,7 @@ private: public: NodeInfoRetriever(INode& node) : NodeStatusMonitor(node) + , TimerBase(node) , listeners_(node.getAllocator()) , get_node_info_client_(node) , last_picked_node_(1) @@ -298,7 +334,7 @@ public: } get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this, &NodeInfoRetriever::handleGetNodeInfoResponse)); - + // Note: the timer will be started ad-hoc return 0; } diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index cf0dd7fd9b..06f7bb7605 100644 --- a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp +++ b/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::Type NodeStatusCode; @@ -43,12 +43,14 @@ public: }; private: - enum { TimerPeriodMs100 = 5 }; + enum { TimerPeriodMs100 = 4 }; typedef MethodBinder&)> NodeStatusCallback; + typedef MethodBinder TimerCallback; + /* * We'll be able to handle this many nodes in the network without any dynamic memory. */ @@ -56,6 +58,8 @@ private: Subscriber sub_; + TimerEventForwarder timer_; + struct Entry { NodeStatusCode status_code; @@ -110,7 +114,7 @@ private: handleNodeStatusMessage(msg); } - virtual void handleTimerEvent(const TimerEvent&) + void handleTimerEvent(const TimerEvent&) { const int OfflineTimeoutMs100 = protocol::NodeStatus::OFFLINE_TIMEOUT_MS / 100; @@ -122,7 +126,8 @@ private: 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; @@ -157,8 +162,8 @@ protected: public: explicit NodeStatusMonitor(INode& node) - : TimerBase(node) - , sub_(node) + : sub_(node) + , timer_(node) { } virtual ~NodeStatusMonitor() { } @@ -173,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; } From 3db54cd6af14037d5116dd07159664b5a3e8221b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 13 May 2015 23:23:03 +0300 Subject: [PATCH 102/268] Improved NodeDiscoverer logic --- .../node_discoverer.hpp | 54 +++++++++---------- .../node_discoverer.cpp | 6 +-- 2 files changed, 30 insertions(+), 30 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 0f96708cd5..93e78d8a9e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -86,6 +86,27 @@ class NodeDiscoverer : TimerBase typedef Map NodeMap; + class HighestUptimeSearcher : ::uavcan::Noncopyable + { + uint32_t highest_uptime_sec_; + NodeID node_id_; + + public: + HighestUptimeSearcher() : highest_uptime_sec_(0) { } + + bool operator()(const NodeID& key, const NodeData& value) + { + if (value.last_seen_uptime >= highest_uptime_sec_) + { + highest_uptime_sec_ = value.last_seen_uptime; + node_id_ = key; + } + return false; + } + + NodeID getNodeWithHighestUptime() const { return node_id_; } + }; + /** * When this number of attempts has been made, the discoverer will give up and assume that the node * does not implement this service. @@ -121,34 +142,13 @@ class NodeDiscoverer : TimerBase NodeID pickNextNodeToQuery() const { - const unsigned node_map_size = node_map_.getSize(); + HighestUptimeSearcher searcher; - // Searching the lowest number of attempts made - uint8_t lowest_number_of_attempts = static_cast(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); - } + const NodeID* const out = node_map_.findFirstKey(searcher); + (void)out; + UAVCAN_ASSERT(out == NULL); - // 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; + return searcher.getNodeWithHighestUptime(); } bool needToQuery(NodeID node_id) @@ -319,7 +319,7 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { - startPeriodic(get_node_info_client_.getRequestTimeout() * 2); + startPeriodic(get_node_info_client_.getRequestTimeout()); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 427213bad1..4c28113ead 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -134,7 +134,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) */ handler.can_discover = true; - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1900)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1400)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -149,7 +149,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) 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(1400)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -167,7 +167,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) 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(1400)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 509ef85048c889afe8ee2882ccbab965b6a16dba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 14 May 2015 00:02:43 +0300 Subject: [PATCH 103/268] Properly defining poll interval of NodeDiscoverer --- .../node_discoverer.hpp | 4 +- .../node_discoverer.cpp | 42 +++++++++++-------- 2 files changed, 27 insertions(+), 19 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 93e78d8a9e..6d351d6176 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -113,6 +113,8 @@ class NodeDiscoverer : TimerBase */ enum { MaxAttemptsToGetNodeInfo = 5 }; + enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) + enum { NumNodeStatusStaticReceivers = 64 }; /* @@ -319,7 +321,7 @@ class NodeDiscoverer : TimerBase if (!isRunning()) { - startPeriodic(get_node_info_client_.getRequestTimeout()); + startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); } } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 4c28113ead..9a469c4461 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/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 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(1400)); + 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(1400)); + 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(1400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(400)); 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(1650)); 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(1650)); 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(1650)); 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()); From fb155d8fc9b56466c146d26ddbf936c827ddb7a0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 14 May 2015 00:08:22 +0300 Subject: [PATCH 104/268] Fixed missing UAVCAN_EXPORT declaration --- .../include/uavcan/protocol/dynamic_node_id_client.hpp | 2 +- .../protocol/dynamic_node_id_server/distributed/server.hpp | 6 +++--- .../uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- .../protocol/dynamic_node_id_server/storage_backend.hpp | 2 +- 4 files changed, 7 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp index 00d654d558..ad5238ffc8 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_client.hpp +++ b/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 Date: Fri, 15 May 2015 15:29:31 +0300 Subject: [PATCH 105/268] Multiset<> --- libuavcan/include/uavcan/util/map.hpp | 7 +- libuavcan/include/uavcan/util/multiset.hpp | 570 +++++++++++++++++++++ libuavcan/test/util/multiset.cpp | 225 ++++++++ 3 files changed, 800 insertions(+), 2 deletions(-) create mode 100644 libuavcan/include/uavcan/util/multiset.hpp create mode 100644 libuavcan/test/util/multiset.cpp diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index 814dd517a0..dcbdf638a0 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -137,7 +137,10 @@ protected: #endif /// Derived class destructor must call removeAll(); - ~MapBase() { } + ~MapBase() + { + UAVCAN_ASSERT(getSize() == 0); + } public: /** @@ -158,7 +161,7 @@ public: /** * Removes entries where the predicate returns true. * Predicate prototype: - * bool (const Key& key, const Value& value) + * bool (Key& key, Value& value) */ template void removeWhere(Predicate predicate); diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp new file mode 100644 index 0000000000..311f4775f0 --- /dev/null +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -0,0 +1,570 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED +#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Slow but memory efficient unordered set. + * + * Items can be allocated in a static buffer or in the node's memory pool if the static buffer is exhausted. + * When an item is deleted from the static buffer, one pair from the memory pool will be moved in the free + * slot of the static buffer, so the use of the memory pool is minimized. + * + * Please be aware that this container does not perform any speed optimizations to minimize memory footprint, + * so the complexity of most operations is O(N). + * + * Type requirements: + * T must be copyable, assignable and default constructible. + * T must implement a comparison operator. + * T's default constructor must initialize the object into invalid state. + * Size of T must not exceed MemPoolBlockSize. + */ +template +class UAVCAN_EXPORT MultisetBase : Noncopyable +{ + template friend class Multiset; + +protected: + /* + * Purpose of this type is to enforce default initialization of T + */ + struct Item + { + T value; + Item() : value() { } + Item(const T& v) : value(v) { } + bool operator==(const Item& rhs) const { return rhs.value == value; } + bool operator!=(const Item& rhs) const { return !operator==(rhs); } + operator T() const { return value; } + }; + + struct Chunk : LinkedListNode + { + enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; + Item items[NumItems]; + + Chunk() + { + StaticAssert<(static_cast(NumItems) > 0)>::check(); + IsDynamicallyAllocatable::check(); + UAVCAN_ASSERT(items[0].value == T()); + } + + 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* find(const Item& item) + { + for (unsigned i = 0; i < static_cast(NumItems); i++) + { + if (items[i] == item) + { + return items + i; + } + } + return NULL; + } + }; + +private: + LinkedListRoot list_; + IPoolAllocator& allocator_; +#if !UAVCAN_TINY + Item* const static_; + const unsigned num_static_entries_; +#endif + + Item* find(const Item& item); + +#if !UAVCAN_TINY + void optimizeStorage(); +#endif + void compact(); + + struct YesPredicate + { + bool operator()(const T&) const { return true; } + }; + +protected: +#if UAVCAN_TINY + MultisetBase(IPoolAllocator& allocator) + : allocator_(allocator) + { + UAVCAN_ASSERT(Item() == Item()); + } +#else + MultisetBase(Item* static_buf, unsigned num_static_entries, IPoolAllocator& allocator) + : allocator_(allocator) + , static_(static_buf) + , num_static_entries_(num_static_entries) + { + UAVCAN_ASSERT(Item() == Item()); + } +#endif + + /// Derived class destructor must call removeAll(); + ~MultisetBase() + { + UAVCAN_ASSERT(getSize() == 0); + } + +public: + /** + * Adds one item and returns a pointer to it. + * If add fails due to lack of memory, NULL will be returned. + */ + T* add(const T& item); + + /** + * Does nothing if there's no such item. + */ + void remove(const T& item); + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (T& item) + */ + template + void removeWhere(Predicate predicate); + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const T& item) + */ + template + const T* findFirst(Predicate predicate) const; + + /** + * Removes all items; all pool memory will be released. + */ + void removeAll(); + + /** + * Returns an item located at the specified position from the beginning. + * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. + * If index is greater than or equal the number of items, null pointer will be returned. + */ + T* getByIndex(unsigned index); + const T* getByIndex(unsigned index) const; + + bool isEmpty() const; + + /** + * Counts number of items stored. + * Best case complexity is O(N). + */ + unsigned getSize() const; + + /** + * For testing, do not use directly. + */ + unsigned getNumStaticItems() const; + unsigned getNumDynamicItems() const; +}; + + +template +class UAVCAN_EXPORT Multiset : public MultisetBase +{ + typename MultisetBase::Item static_[NumStaticEntries]; + +public: + +#if !UAVCAN_TINY + + // This instantiation will not be valid in UAVCAN_TINY mode + explicit Multiset(IPoolAllocator& allocator) + : MultisetBase(static_, NumStaticEntries, allocator) + { + UAVCAN_ASSERT(static_[0].value == T()); + } + + ~Multiset() { this->removeAll(); } + +#endif // !UAVCAN_TINY +}; + + +template +class UAVCAN_EXPORT Multiset : public MultisetBase +{ +public: + explicit Multiset(IPoolAllocator& allocator) +#if UAVCAN_TINY + : MultisetBase(allocator) +#else + : MultisetBase(NULL, 0, allocator) +#endif + { } + + ~Multiset() { this->removeAll(); } +}; + +// ---------------------------------------------------------------------------- + +/* + * MultisetBase<> + */ +template +typename MultisetBase::Item* MultisetBase::find(const Item& item) +{ +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] == item) + { + return static_ + i; + } + } +#endif + + Chunk* p = list_.get(); + while (p) + { + Item* const dyn = p->find(item); + if (dyn != NULL) + { + return dyn; + } + p = p->getNextListNode(); + } + return NULL; +} + +#if !UAVCAN_TINY + +template +void MultisetBase::optimizeStorage() +{ + while (true) + { + // Looking for first EMPTY static entry + Item* stat = NULL; + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] == Item()) + { + stat = static_ + i; + break; + } + } + if (stat == NULL) + { + break; + } + + // Looking for the first NON-EMPTY dynamic entry, erasing immediately + Chunk* p = list_.get(); + Item dyn; + UAVCAN_ASSERT(dyn == Item()); + while (p) + { + bool stop = false; + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i] != Item()) // Non empty + { + dyn = p->items[i]; // Copy by value + p->items[i] = Item(); // Erase immediately + stop = true; + break; + } + } + if (stop) + { + break; + } + p = p->getNextListNode(); + } + if (dyn == Item()) + { + break; + } + + // Migrating + *stat = dyn; + } +} + +#endif // !UAVCAN_TINY + +template +void MultisetBase::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] != Item()) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + Chunk::destroy(p, allocator_); + } + p = next; + } +} + +template +T* MultisetBase::add(const T& value) +{ + UAVCAN_ASSERT(!(value == T())); + remove(value); + + Item* const item = find(Item()); + if (item) + { + *item = Item(value); + return &item->value; + } + + Chunk* const itemg = Chunk::instantiate(allocator_); + if (itemg == NULL) + { + return NULL; + } + list_.insert(itemg); + itemg->items[0] = Item(value); + return &itemg->items[0].value; +} + +template +void MultisetBase::remove(const T& value) +{ + UAVCAN_ASSERT(!(value == T())); + Item* const item = find(Item(value)); + if (item != NULL) + { + *item = Item(); +#if !UAVCAN_TINY + optimizeStorage(); +#endif + compact(); + } +} + +template +template +void MultisetBase::removeWhere(Predicate predicate) +{ + unsigned num_removed = 0; + +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + if (predicate(static_[i].value)) + { + num_removed++; + static_[i] = Item(); + } + } + } +#endif + + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + const Item* const item = p->items + i; + if ((*item) != Item()) + { + if (predicate(item->value)) + { + num_removed++; + p->items[i] = Item(); + } + } + } + p = p->getNextListNode(); + } + + if (num_removed > 0) + { +#if !UAVCAN_TINY + optimizeStorage(); +#endif + compact(); + } +} + +template +template +const T* MultisetBase::findFirst(Predicate predicate) const +{ +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + if (predicate(static_[i].value)) + { + return &static_[i].value; + } + } + } +#endif + + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + const Item* const item = p->items + i; + if ((*item) != Item()) + { + if (predicate(item->value)) + { + return &p->items[i].value; + } + } + } + p = p->getNextListNode(); + } + return NULL; +} + +template +void MultisetBase::removeAll() +{ + removeWhere(YesPredicate()); +} + +template +T* MultisetBase::getByIndex(unsigned index) +{ +#if !UAVCAN_TINY + // Checking the static storage + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + if (index == 0) + { + return &static_[i].value; + } + index--; + } + } +#endif + + // Slowly crawling through the dynamic storage + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + Item* const item = p->items + i; + if ((*item) != Item()) + { + if (index == 0) + { + return &item->value; + } + index--; + } + } + p = p->getNextListNode(); + } + + return NULL; +} + +template +const T* MultisetBase::getByIndex(unsigned index) const +{ + return const_cast*>(this)->getByIndex(index); +} + +template +bool MultisetBase::isEmpty() const +{ + return getSize() == 0; +} + +template +unsigned MultisetBase::getSize() const +{ + return getNumStaticItems() + getNumDynamicItems(); +} + +template +unsigned MultisetBase::getNumStaticItems() const +{ + unsigned num = 0; +#if !UAVCAN_TINY + for (unsigned i = 0; i < num_static_entries_; i++) + { + if (static_[i] != Item()) + { + num++; + } + } +#endif + return num; +} + +template +unsigned MultisetBase::getNumDynamicItems() const +{ + unsigned num = 0; + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + const Item* const item = p->items + i; + if ((*item) != Item()) + { + num++; + } + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // Include guard diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp new file mode 100644 index 0000000000..a6cb3c71ea --- /dev/null +++ b/libuavcan/test/util/multiset.cpp @@ -0,0 +1,225 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include +#include + + +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; } +}; + + +TEST(Multiset, Basic) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr mset(new MultisetType(poolmgr)); + + // Empty + mset->remove("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->add("1")); + ASSERT_EQ("2", *mset->add("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"); + + // Dynamic addion + ASSERT_EQ("3", *mset->add("3")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + + ASSERT_EQ("4", *mset->add("4")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block + 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)); + ASSERT_EQ("3", *mset->getByIndex(2)); + ASSERT_EQ("4", *mset->getByIndex(3)); + ASSERT_FALSE(mset->getByIndex(100)); + ASSERT_FALSE(mset->getByIndex(4)); + + // Finding some items + ASSERT_EQ("1", *mset->findFirst(FindPredicate("1"))); + ASSERT_EQ("2", *mset->findFirst(FindPredicate("2"))); + ASSERT_EQ("3", *mset->findFirst(FindPredicate("3"))); + ASSERT_EQ("4", *mset->findFirst(FindPredicate("4"))); + ASSERT_FALSE(mset->findFirst(FindPredicate("nonexistent"))); + + // Removing one static + mset->remove("1"); // One of dynamics now migrates to the static storage + mset->remove("foo"); // There's no such thing anyway + ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(1, mset->getNumDynamicItems()); + + // Ordering has not changed - first dynamic entry has moved to the first static slot + ASSERT_EQ("3", *mset->getByIndex(0)); + ASSERT_EQ("2", *mset->getByIndex(1)); + ASSERT_EQ("4", *mset->getByIndex(2)); + + // Removing another static + mset->remove("2"); + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(0, mset->getNumDynamicItems()); + ASSERT_EQ(0, pool.getNumUsedBlocks()); // No dynamic entries left + + // Adding some new dynamics + unsigned max_value_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string value = toString(i); + std::string* res = mset->add(value); // Will override some from the 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; + ASSERT_LT(4, max_value_integer); + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(mset->add("nonexistent")); + + // Removing odd values - nearly half of them + ASSERT_EQ(2, mset->getNumStaticItems()); + const unsigned num_dynamics_old = mset->getNumDynamicItems(); + mset->removeWhere(oddValuePredicate); + ASSERT_EQ(2, mset->getNumStaticItems()); + const unsigned num_dynamics_new = mset->getNumDynamicItems(); + std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl; + ASSERT_LT(num_dynamics_new, num_dynamics_old); + + // 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->findFirst(FindPredicate(toString(kv_int))); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Making sure the memory will be released + mset.reset(); + ASSERT_EQ(0, pool.getNumUsedBlocks()); +} + + +TEST(Multiset, NoStatic) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr mset(new MultisetType(poolmgr)); + + // Empty + mset->remove("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ("a", *mset->add("a")); + ASSERT_EQ("b", *mset->add("b")); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + + // Ordering + ASSERT_EQ("a", *mset->getByIndex(0)); + ASSERT_EQ("b", *mset->getByIndex(1)); + ASSERT_FALSE(mset->getByIndex(3)); + ASSERT_FALSE(mset->getByIndex(1000)); +} + + +TEST(Multiset, PrimitiveKey) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr mset(new MultisetType(poolmgr)); + + // Empty + mset->remove(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getSize()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *mset->add(1)); + ASSERT_EQ(1, mset->getSize()); + ASSERT_EQ(2, *mset->add(2)); + ASSERT_EQ(2, mset->getSize()); + ASSERT_EQ(3, *mset->add(3)); + ASSERT_EQ(4, *mset->add(4)); + ASSERT_EQ(4, mset->getSize()); + + // 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)); +} From 282b995c1e94d4080cdba2eaeab270900daae596 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 18:41:38 +0300 Subject: [PATCH 106/268] Partially refactored ServiceClient, tests are failing, the code is totally broken --- .../include/uavcan/node/service_client.hpp | 354 ++++++++++++------ .../distributed/raft_core.hpp | 25 +- .../node_discoverer.hpp | 14 +- .../protocol/network_compat_checker.hpp | 16 +- .../uavcan/protocol/node_info_retriever.hpp | 13 +- .../uavcan/transport/transfer_listener.hpp | 97 ++--- libuavcan/include/uavcan/util/multiset.hpp | 1 + libuavcan/src/node/uc_service_client.cpp | 29 +- libuavcan/test/node/service_client.cpp | 30 +- .../test/protocol/data_type_info_provider.cpp | 66 ++-- .../test/protocol/node_status_provider.cpp | 11 +- libuavcan/test/protocol/param_server.cpp | 32 +- .../test/protocol/restart_request_server.cpp | 8 +- .../protocol/transport_stats_provider.cpp | 42 +-- .../linux/include/uavcan_linux/helpers.hpp | 4 +- 15 files changed, 435 insertions(+), 307 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 067d3b890c..2aefd9c1f3 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -6,6 +6,7 @@ #define UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED #include +#include #include #include @@ -20,12 +21,39 @@ namespace uavcan { -template +template class UAVCAN_EXPORT ServiceResponseTransferListenerInstantiationHelper { public: // so much templating it hurts typedef typename TransferListenerInstantiationHelper::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 -struct UAVCAN_EXPORT ServiceCallResult +class UAVCAN_EXPORT ServiceCallResult { +public: typedef ReceivedDataStructure 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& 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,42 +121,64 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& scr) /** * Do not use directly. */ -class ServiceClientBase : protected DeadlineHandler +class ServiceClientBase : public ITransferAcceptanceFilter, Noncopyable { 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_; - explicit ServiceClientBase(INode& node) - : DeadlineHandler(node.getScheduler()) - , data_type_descriptor_(NULL) + virtual void handleDeadline(MonotonicTime); + + public: + CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id) + : DeadlineHandler(node.getScheduler()) + , owner_(owner) + , id_(call_id) + { + UAVCAN_ASSERT(id_.isValid()); + DeadlineHandler::startWithDelay(owner_.request_timeout_); + } + + bool doesMatch(ServiceCallID call_id) const { return call_id == id_; } + + 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.doesMatch(id); } + }; + + MonotonicDuration request_timeout_; + + ServiceClientBase() + : 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); + + virtual void handleTimeout(ServiceCallID call_id) = 0; 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; }; /** @@ -132,14 +193,17 @@ public: */ template = UAVCAN_CPP11 - typename Callback_ = std::function&)> + typename Callback_ = std::function&)>, #else - typename Callback_ = void (*)(const ServiceCallResult&) + typename Callback_ = void (*)(const ServiceCallResult&), #endif + unsigned NumStaticCalls_ = 1 > class UAVCAN_EXPORT ServiceClient - : public GenericSubscriber::Type > + : public GenericSubscriber::Type> , public ServiceClientBase { public: @@ -149,22 +213,32 @@ public: typedef ServiceCallResult ServiceCallResultType; typedef Callback_ Callback; + enum { NumStaticCalls = NumStaticCalls_ }; + private: typedef ServiceClient SelfType; typedef GenericPublisher PublisherType; - typedef typename ServiceResponseTransferListenerInstantiationHelper::Type TransferListenerType; + typedef typename ServiceResponseTransferListenerInstantiationHelper::Type + TransferListenerType; typedef GenericSubscriber SubscriberType; +#if 0 + typedef Multiset CallRegistry; + CallRegistry call_registry_; +#endif + PublisherType publisher_; Callback callback_; - bool isCallbackValid() const { return try_implicit_cast(callback_, true); } + virtual bool shouldAcceptFrame(const RxFrame& frame) const; // Called from the transfer listener void invokeCallback(ServiceCallResultType& result); virtual void handleReceivedDataStruct(ReceivedDataStructure& response); - virtual void handleDeadline(MonotonicTime); + virtual void handleTimeout(ServiceCallID call_id); + + int addCallState(ServiceCallID call_id); public: /** @@ -173,7 +247,6 @@ public: */ explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) - , ServiceClientBase(node) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) { @@ -183,7 +256,7 @@ public: #endif } - virtual ~ServiceClient() { cancel(); } + virtual ~ServiceClient() { cancelAll(); } /** * Shall be called before first use. @@ -202,18 +275,24 @@ public: * Note that the callback will ALWAYS be called even if the service call has timed 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. */ - void cancel(); + int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); + + /** + * Cancels certain call referred via call ID structure. + */ + void cancel(ServiceCallID call_id); + + /** + * Cancels all pending calls. + */ + void cancelAll(); /** * Service response callback must be set prior service call. @@ -221,6 +300,16 @@ public: const Callback& getCallback() const { return callback_; } void setCallback(const Callback& cb) { callback_ = cb; } +#if 0 + unsigned getNumPendingCalls() const { return call_registry_.getSize(); } +#endif + +#if 0 + bool hasPendingCalls() const { return !call_registry_.isEmpty(); } +#else + bool hasPendingCalls() const { return false; } +#endif + /** * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: * - Transient failure in the transport layer. @@ -246,10 +335,10 @@ public: // ---------------------------------------------------------------------------- -template -void ServiceClient::invokeCallback(ServiceCallResultType& result) +template +void ServiceClient::invokeCallback(ServiceCallResultType& result) { - if (isCallbackValid()) + if (try_implicit_cast(callback_, true)) { callback_(result); } @@ -259,55 +348,86 @@ void ServiceClient::invokeCallback(ServiceCallResultType& } } -template -void ServiceClient::handleReceivedDataStruct(ReceivedDataStructure& response) +template +bool ServiceClient::shouldAcceptFrame(const RxFrame& frame) const +{ + UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher + +#if 0 + return call_registry_.findFirst(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))) != NULL; +#else + (void)frame; + return false; +#endif +} + +template +void ServiceClient:: +handleReceivedDataStruct(ReceivedDataStructure& response) { UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) - { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, response); - cancel(); - invokeCallback(result); - } - else - { - UAVCAN_ASSERT(0); - cancel(); - } + + ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); + cancel(call_id); + ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! + invokeCallback(result); } -template -void ServiceClient::handleDeadline(MonotonicTime) -{ - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) - { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ReceivedDataStructure& ref = SubscriberType::getReceivedStructStorage(); - ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref); - UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s", - erp.src_node_id.get(), DataType::getDataTypeFullName()); - cancel(); - invokeCallback(result); - } - else - { - UAVCAN_ASSERT(0); - cancel(); - } +template +void ServiceClient::handleTimeout(ServiceCallID call_id) +{ + cancel(call_id); + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, call_id, + SubscriberType::getReceivedStructStorage()); // Mutable! + invokeCallback(result); } -template -int ServiceClient::call(NodeID server_node_id, const RequestType& request) +template +int ServiceClient::addCallState(ServiceCallID call_id) { - cancel(); - if (!isCallbackValid()) +#if 0 + if (call_registry_.isEmpty()) + { + 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; + } + } + + if (call_registry_.add(CallState(SubscriberType::getNode(), *this, call_id)) == NULL) + { + SubscriberType::stop(); + return -ErrMemory; + } + + return 0; +#else + (void)call_id; + return 0; +#endif +} + +template +int ServiceClient::call(NodeID server_node_id, + const RequestType& request) +{ + ServiceCallID dummy; + return call(server_node_id, request, dummy); +} + +template +int ServiceClient::call(NodeID server_node_id, + const RequestType& request, + ServiceCallID& out_call_id) +{ + if (!try_implicit_cast(callback_, true)) { UAVCAN_TRACE("ServiceClient", "Invalid callback"); - return -ErrInvalidParam; + return -ErrInvalidConfiguration; } /* @@ -315,37 +435,35 @@ int ServiceClient::call(NodeID server_node_id, const Reque */ 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(); + cancel(out_call_id); return -ErrLogic; } - const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id); - tl->setExpectedResponseParams(erp); + tl->installAcceptanceFilter(this); /* * Publishing the request @@ -353,22 +471,34 @@ int ServiceClient::call(NodeID server_node_id, const Reque const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id); if (publisher_res < 0) { - cancel(); + cancel(out_call_id); + return publisher_res; } - return publisher_res; + + return 0; } -template -void ServiceClient::cancel() +template +void ServiceClient::cancel(ServiceCallID call_id) { - pending_ = false; - SubscriberType::stop(); - DeadlineHandler::stop(); - TransferListenerType* const tl = SubscriberType::getTransferListener(); - if (tl) +#if 0 + call_registry_.remove(call_id); + if (call_registry_.isEmpty()) { - tl->stopAcceptingAnything(); + SubscriberType::stop(); } +#else + (void)call_id; +#endif +} + +template +void ServiceClient::cancelAll() +{ +#if 0 + call_registry_.removeAll(); +#endif + SubscriberType::stop(); } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 17887557a3..634bd002bd 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -327,9 +327,9 @@ class RaftCore : private TimerBase for (uint8_t i = 0; i < NumRequestVoteClients; i++) { - request_vote_clients_[i]->cancel(); + request_vote_clients_[i]->cancelAll(); // TODO FIXME Concurrent calls!! } - append_entries_client_.cancel(); + append_entries_client_.cancelAll(); /* * Calling the switch handler @@ -550,22 +550,23 @@ class RaftCore : private TimerBase 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); } } @@ -654,15 +655,15 @@ class RaftCore : private TimerBase 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_++; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 6d351d6176..74280ac9f4 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -235,14 +235,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 @@ -250,11 +250,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 } } @@ -262,7 +262,7 @@ class NodeDiscoverer : TimerBase void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.isPending()) + if (get_node_info_client_.hasPendingCalls()) { return; } diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index ca6635451a..3ac0ae0264 100644 --- a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp +++ b/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(sign.get()), - static_cast(resp.response.aggregate_signature)); + int(resp.getCallID().server_node_id.get()), static_cast(sign.get()), + static_cast(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::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_.cancelAll(); return res; } diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 4c4245b6c8..c2656b645c 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -196,9 +196,9 @@ private: virtual void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.isPending()) // If request is pending, this condition will fail every second time + if (get_node_info_client_.hasPendingCalls()) // If request is pending, this condition will fail every second time { - return; + return; // TODO FIXME Concurrent calls!! } const NodeID next = pickNextNodeToQuery(); @@ -277,7 +277,7 @@ private: void handleGetNodeInfoResponse(const ServiceCallResult& result) { - Entry& entry = getEntry(result.server_node_id); + Entry& entry = getEntry(result.getCallID().server_node_id); if (result.isSuccessful()) { @@ -285,9 +285,10 @@ private: * 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.response.status.uptime_sec; + entry.uptime_sec = result.getResponse().status.uptime_sec; entry.request_needed = false; - listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.server_node_id, result.response)); + listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); } else { @@ -298,7 +299,7 @@ private: { entry.request_needed = false; listeners_.removeWhere(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, - result.server_node_id)); + result.getCallID().server_node_id)); } } } diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index dc0bc66ad2..62be67d64b 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -178,88 +178,71 @@ public: virtual ~TransferListener() { - // Map must be cleared before bufmgr is destructed + // Map must be cleared before bufmgr is destroyed receivers_.removeAll(); } }; +/** + * 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 -class UAVCAN_EXPORT ServiceResponseTransferListener - : public TransferListener +class UAVCAN_EXPORT TransferListenerWithFilter : public TransferListener { + const ITransferAcceptanceFilter* filter_; + + virtual void handleFrame(const RxFrame& frame); + public: typedef TransferListener 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) + { + UAVCAN_ASSERT(filter_ == NULL); + filter_ = acceptance_filter; + UAVCAN_ASSERT(filter_ != NULL); + } }; // ---------------------------------------------------------------------------- /* - * ServiceResponseTransferListener<> + * TransferListenerWithFilter<> */ template -void ServiceResponseTransferListener::handleFrame(const RxFrame& frame) +void TransferListenerWithFilter::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 -void ServiceResponseTransferListener::setExpectedResponseParams( - const ExpectedResponseParams& erp) -{ - response_params_ = erp; -} - -template -void ServiceResponseTransferListener::stopAcceptingAnything() -{ - response_params_ = ExpectedResponseParams(); } } diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 311f4775f0..7cce1e2a17 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -146,6 +146,7 @@ public: /** * Does nothing if there's no such item. + * Only the first matching item will be removed. */ void remove(const T& item); diff --git a/libuavcan/src/node/uc_service_client.cpp b/libuavcan/src/node/uc_service_client.cpp index a0c6cc00e8..f686db20c5 100644 --- a/libuavcan/src/node/uc_service_client.cpp +++ b/libuavcan/src/node/uc_service_client.cpp @@ -6,12 +6,26 @@ 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_ASSERT(id_.isValid()); + UAVCAN_TRACE("ServiceClient", "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()); + owner_.handleTimeout(id_); +} +/* + * 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 +34,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 +65,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; } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index f668f791d5..fa08b7214a 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -26,9 +26,9 @@ struct ServiceCallResultHandler void handleResponse(const uavcan::ServiceCallResult& 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; } bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const @@ -111,17 +111,17 @@ TEST(ServiceClient, Basic) 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()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); 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()); // Validating root_ns_a::StringService::Response expected_response; @@ -130,9 +130,9 @@ 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 :( @@ -141,7 +141,7 @@ TEST(ServiceClient, Basic) // 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()); } @@ -178,10 +178,10 @@ TEST(ServiceClient, Rejection) ASSERT_LT(0, client1.call(1, request)); ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); - ASSERT_TRUE(client1.isPending()); + ASSERT_TRUE(client1.hasPendingCalls()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); - ASSERT_FALSE(client1.isPending()); + ASSERT_FALSE(client1.hasPendingCalls()); ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out diff --git a/libuavcan/test/protocol/data_type_info_provider.cpp b/libuavcan/test/protocol/data_type_info_provider.cpp index f987b3080b..1780d5f801 100644 --- a/libuavcan/test/protocol/data_type_info_provider.cpp +++ b/libuavcan/test/protocol/data_type_info_provider.cpp @@ -31,29 +31,29 @@ static bool validateDataTypeInfoResponse(const std::auto_ptrresponse.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(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(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()); } diff --git a/libuavcan/test/protocol/node_status_provider.cpp b/libuavcan/test/protocol/node_status_provider.cpp index 31c4e5cc20..5c9feacdf6 100644 --- a/libuavcan/test/protocol/node_status_provider.cpp +++ b/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 diff --git a/libuavcan/test/protocol/param_server.cpp b/libuavcan/test/protocol/param_server.cpp index 245864a83f..bcc38e1f1d 100644 --- a/libuavcan/test/protocol/param_server.cpp +++ b/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]); } diff --git a/libuavcan/test/protocol/restart_request_server.cpp b/libuavcan/test/protocol/restart_request_server.cpp index 7b27621bf2..8229ed8ce6 100644 --- a/libuavcan/test/protocol/restart_request_server.cpp +++ b/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); } diff --git a/libuavcan/test/protocol/transport_stats_provider.cpp b/libuavcan/test/protocol/transport_stats_provider.cpp index e309977d2f..6c8ed9da69 100644 --- a/libuavcan/test/protocol/transport_stats_provider.cpp +++ b/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); } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp index 608330fb48..d27cad4833 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/helpers.hpp @@ -42,7 +42,7 @@ class BlockingServiceClient : public uavcan::ServiceClient void callback(const uavcan::ServiceCallResult& 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) From 0d85d672c7860c3fab38780b0b29487ff1a61a9b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 18:45:37 +0300 Subject: [PATCH 107/268] Temporary fix for assertion failure in tests; 12 tests are failing --- libuavcan/include/uavcan/node/service_client.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 2aefd9c1f3..8a9dced967 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -407,7 +407,7 @@ int ServiceClient::addCallState(ServiceCa return 0; #else (void)call_id; - return 0; + return -ErrNotInited; #endif } From 048e0a33eebc0e1ebd843bbc0fbbec4f178ece19 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 15 May 2015 21:32:08 +0300 Subject: [PATCH 108/268] Non-moving multiset, tests are failing in C++03 mode --- libuavcan/include/uavcan/util/map.hpp | 1 + libuavcan/include/uavcan/util/multiset.hpp | 440 +++++++++------------ libuavcan/test/util/multiset.cpp | 82 ++-- 3 files changed, 238 insertions(+), 285 deletions(-) diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index dcbdf638a0..ed5535266d 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -11,6 +11,7 @@ #include #include #include +#include namespace uavcan { diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 7cce1e2a17..1c62dd3fdf 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -11,45 +11,62 @@ #include #include #include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif namespace uavcan { /** - * Slow but memory efficient unordered set. + * 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. - * When an item is deleted from the static buffer, one pair from the memory pool will be moved in the free - * slot of the static buffer, so the use of the memory pool is minimized. - * - * Please be aware that this container does not perform any speed optimizations to minimize memory footprint, - * so the complexity of most operations is O(N). - * - * Type requirements: - * T must be copyable, assignable and default constructible. - * T must implement a comparison operator. - * T's default constructor must initialize the object into invalid state. - * Size of T must not exceed MemPoolBlockSize. */ template class UAVCAN_EXPORT MultisetBase : Noncopyable { - template friend class Multiset; - protected: - /* - * Purpose of this type is to enforce default initialization of T - */ - struct Item + struct Item : ::uavcan::Noncopyable { - T value; - Item() : value() { } - Item(const T& v) : value(v) { } - bool operator==(const Item& rhs) const { return rhs.value == value; } - bool operator!=(const Item& rhs) const { return !operator==(rhs); } - operator T() const { return value; } + 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)]; + long double _aligner1_; + long long _aligner2_; + }; +#endif + + Item() + : ptr(NULL) + { + fill_n(pool, sizeof(pool), static_cast(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(0)); + } + } }; - struct Chunk : LinkedListNode +private: + struct Chunk : LinkedListNode, ::uavcan::Noncopyable { enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; Item items[NumItems]; @@ -58,7 +75,7 @@ protected: { StaticAssert<(static_cast(NumItems) > 0)>::check(); IsDynamicallyAllocatable::check(); - UAVCAN_ASSERT(items[0].value == T()); + UAVCAN_ASSERT(!items[0].isConstructed()); } static Chunk* instantiate(IPoolAllocator& allocator) @@ -81,11 +98,11 @@ protected: } } - Item* find(const Item& item) + Item* findFreeSlot() { for (unsigned i = 0; i < static_cast(NumItems); i++) { - if (items[i] == item) + if (!items[i].isConstructed()) { return items + i; } @@ -94,7 +111,6 @@ protected: } }; -private: LinkedListRoot list_; IPoolAllocator& allocator_; #if !UAVCAN_TINY @@ -102,11 +118,8 @@ private: const unsigned num_static_entries_; #endif - Item* find(const Item& item); + Item* findOrCreateFreeSlot(); -#if !UAVCAN_TINY - void optimizeStorage(); -#endif void compact(); struct YesPredicate @@ -114,6 +127,20 @@ private: 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; } + }; + protected: #if UAVCAN_TINY MultisetBase(IPoolAllocator& allocator) @@ -126,9 +153,7 @@ protected: : allocator_(allocator) , static_(static_buf) , num_static_entries_(num_static_entries) - { - UAVCAN_ASSERT(Item() == Item()); - } + { } #endif /// Derived class destructor must call removeAll(); @@ -138,17 +163,70 @@ protected: } public: + /** + * This is needed for testing. + */ + enum { NumItemsPerDynamicChunk = Chunk::NumItems }; + /** * Adds one item and returns a pointer to it. * If add fails due to lack of memory, NULL will be returned. */ - T* add(const T& item); + T* add() + { + Item* const item = findOrCreateFreeSlot(); + if (item == NULL) + { + return NULL; + } + UAVCAN_ASSERT(item->ptr == NULL); + item->ptr = new (item->pool) T(); + return item->ptr; + } + + template + T* add(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 + T* add(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 + T* add(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; + } /** - * Does nothing if there's no such item. - * Only the first matching item will be removed. + * @ref removeMatching() */ - void remove(const T& item); + enum RemoveStrategy { RemoveOne, RemoveAll }; /** * Removes entries where the predicate returns true. @@ -156,7 +234,15 @@ public: * bool (T& item) */ template - void removeWhere(Predicate predicate); + void removeMatching(Predicate predicate, RemoveStrategy strategy); + + template + void removeAllMatching(Predicate predicate) { removeMatching(predicate, RemoveAll); } + + template + void removeFirstMatching(Predicate predicate) { removeMatching(predicate, RemoveOne); } + + void removeFirst(const T& ref) { removeFirstMatching(ComparingPredicate(ref)); } /** * Returns first entry where the predicate returns true. @@ -164,28 +250,45 @@ public: * bool (const T& item) */ template - const T* findFirst(Predicate predicate) const; + T* find(Predicate predicate); + + template + const T* find(Predicate predicate) const + { + return const_cast*>(this)->find(predicate); + } /** * Removes all items; all pool memory will be released. */ - void removeAll(); + void removeAll() { removeAllMatching(YesPredicate()); } /** * Returns an item located at the specified position from the beginning. * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. * If index is greater than or equal the number of items, null pointer will be returned. */ - T* getByIndex(unsigned index); - const T* getByIndex(unsigned index) const; + T* getByIndex(unsigned index) + { + IndexPredicate predicate(index); + return find(predicate); + } - bool isEmpty() const; + const T* getByIndex(unsigned index) const + { + return const_cast*>(this)->getByIndex(index); + } + + /** + * This is O(1) + */ + bool isEmpty() const { return find(YesPredicate()) == NULL; } /** * Counts number of items stored. * Best case complexity is O(N). */ - unsigned getSize() const; + unsigned getSize() const { return getNumStaticItems() + getNumDynamicItems(); } /** * For testing, do not use directly. @@ -207,9 +310,7 @@ public: // This instantiation will not be valid in UAVCAN_TINY mode explicit Multiset(IPoolAllocator& allocator) : MultisetBase(static_, NumStaticEntries, allocator) - { - UAVCAN_ASSERT(static_[0].value == T()); - } + { } ~Multiset() { this->removeAll(); } @@ -238,87 +339,42 @@ public: * MultisetBase<> */ template -typename MultisetBase::Item* MultisetBase::find(const Item& item) +typename MultisetBase::Item* MultisetBase::findOrCreateFreeSlot() { #if !UAVCAN_TINY + // Search in static pool for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] == item) + if (!static_[i].isConstructed()) { - return static_ + i; + return &static_[i]; } } #endif - Chunk* p = list_.get(); - while (p) + // Search in dynamic pool { - Item* const dyn = p->find(item); - if (dyn != NULL) - { - return dyn; - } - p = p->getNextListNode(); - } - return NULL; -} - -#if !UAVCAN_TINY - -template -void MultisetBase::optimizeStorage() -{ - while (true) - { - // Looking for first EMPTY static entry - Item* stat = NULL; - for (unsigned i = 0; i < num_static_entries_; i++) - { - if (static_[i] == Item()) - { - stat = static_ + i; - break; - } - } - if (stat == NULL) - { - break; - } - - // Looking for the first NON-EMPTY dynamic entry, erasing immediately Chunk* p = list_.get(); - Item dyn; - UAVCAN_ASSERT(dyn == Item()); while (p) { - bool stop = false; - for (int i = 0; i < Chunk::NumItems; i++) + Item* const dyn = p->findFreeSlot(); + if (dyn != NULL) { - if (p->items[i] != Item()) // Non empty - { - dyn = p->items[i]; // Copy by value - p->items[i] = Item(); // Erase immediately - stop = true; - break; - } - } - if (stop) - { - break; + return dyn; } p = p->getNextListNode(); } - if (dyn == Item()) - { - break; - } - - // Migrating - *stat = dyn; } -} -#endif // !UAVCAN_TINY + // Create new dynamic chunk + Chunk* const chunk = Chunk::instantiate(allocator_); + if (chunk == NULL) + { + return NULL; + } + list_.insert(chunk); + return &chunk->items[0]; +} template void MultisetBase::compact() @@ -330,7 +386,7 @@ void MultisetBase::compact() bool remove_this = true; for (int i = 0; i < Chunk::NumItems; i++) { - if (p->items[i] != Item()) + if (p->items[i].isConstructed()) { remove_this = false; break; @@ -345,103 +401,73 @@ void MultisetBase::compact() } } -template -T* MultisetBase::add(const T& value) -{ - UAVCAN_ASSERT(!(value == T())); - remove(value); - - Item* const item = find(Item()); - if (item) - { - *item = Item(value); - return &item->value; - } - - Chunk* const itemg = Chunk::instantiate(allocator_); - if (itemg == NULL) - { - return NULL; - } - list_.insert(itemg); - itemg->items[0] = Item(value); - return &itemg->items[0].value; -} - -template -void MultisetBase::remove(const T& value) -{ - UAVCAN_ASSERT(!(value == T())); - Item* const item = find(Item(value)); - if (item != NULL) - { - *item = Item(); -#if !UAVCAN_TINY - optimizeStorage(); -#endif - compact(); - } -} - template template -void MultisetBase::removeWhere(Predicate predicate) +void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy strategy) { unsigned num_removed = 0; #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] != Item()) + if (static_[i].isConstructed()) { - if (predicate(static_[i].value)) + if (predicate(*static_[i].ptr)) { num_removed++; - static_[i] = Item(); + static_[i].destroy(); } } + + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } } #endif Chunk* p = list_.get(); while (p) { + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } + for (int i = 0; i < Chunk::NumItems; i++) { - const Item* const item = p->items + i; - if ((*item) != Item()) + Item& item = p->items[i]; + if (item.isConstructed()) { - if (predicate(item->value)) + if (predicate(*item.ptr)) { num_removed++; - p->items[i] = Item(); + item.destroy(); } } } + p = p->getNextListNode(); } if (num_removed > 0) { -#if !UAVCAN_TINY - optimizeStorage(); -#endif compact(); } } template template -const T* MultisetBase::findFirst(Predicate predicate) const +T* MultisetBase::find(Predicate predicate) { #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] != Item()) + if (static_[i].isConstructed()) { - if (predicate(static_[i].value)) + if (predicate(*static_[i].ptr)) { - return &static_[i].value; + return static_[i].ptr; } } } @@ -452,12 +478,11 @@ const T* MultisetBase::findFirst(Predicate predicate) const { for (int i = 0; i < Chunk::NumItems; i++) { - const Item* const item = p->items + i; - if ((*item) != Item()) + if (p->items[i].isConstructed()) { - if (predicate(item->value)) + if (predicate(*p->items[i].ptr)) { - return &p->items[i].value; + return p->items[i].ptr; } } } @@ -466,70 +491,6 @@ const T* MultisetBase::findFirst(Predicate predicate) const return NULL; } -template -void MultisetBase::removeAll() -{ - removeWhere(YesPredicate()); -} - -template -T* MultisetBase::getByIndex(unsigned index) -{ -#if !UAVCAN_TINY - // Checking the static storage - for (unsigned i = 0; i < num_static_entries_; i++) - { - if (static_[i] != Item()) - { - if (index == 0) - { - return &static_[i].value; - } - index--; - } - } -#endif - - // Slowly crawling through the dynamic storage - Chunk* p = list_.get(); - while (p) - { - for (int i = 0; i < Chunk::NumItems; i++) - { - Item* const item = p->items + i; - if ((*item) != Item()) - { - if (index == 0) - { - return &item->value; - } - index--; - } - } - p = p->getNextListNode(); - } - - return NULL; -} - -template -const T* MultisetBase::getByIndex(unsigned index) const -{ - return const_cast*>(this)->getByIndex(index); -} - -template -bool MultisetBase::isEmpty() const -{ - return getSize() == 0; -} - -template -unsigned MultisetBase::getSize() const -{ - return getNumStaticItems() + getNumDynamicItems(); -} - template unsigned MultisetBase::getNumStaticItems() const { @@ -537,10 +498,7 @@ unsigned MultisetBase::getNumStaticItems() const #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) { - if (static_[i] != Item()) - { - num++; - } + num += static_[i].isConstructed() ? 1U : 0U; } #endif return num; @@ -555,11 +513,7 @@ unsigned MultisetBase::getNumDynamicItems() const { for (int i = 0; i < Chunk::NumItems; i++) { - const Item* const item = p->items + i; - if ((*item) != Item()) - { - num++; - } + num += p->items[i].isConstructed() ? 1U : 0U; } p = p->getNextListNode(); } diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index a6cb3c71ea..21f74500cd 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -44,7 +44,7 @@ TEST(Multiset, Basic) std::auto_ptr mset(new MultisetType(poolmgr)); // Empty - mset->remove("foo"); + mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); ASSERT_FALSE(mset->getByIndex(0)); ASSERT_FALSE(mset->getByIndex(1)); @@ -61,54 +61,57 @@ TEST(Multiset, Basic) ASSERT_TRUE(*mset->getByIndex(0) == "1"); ASSERT_TRUE(*mset->getByIndex(1) == "2"); - // Dynamic addion + // Dynamic addition ASSERT_EQ("3", *mset->add("3")); + ASSERT_EQ("3", *mset->getByIndex(2)); ASSERT_EQ(1, pool.getNumUsedBlocks()); ASSERT_EQ("4", *mset->add("4")); - ASSERT_EQ(1, pool.getNumUsedBlocks()); // Assuming that at least 2 items fit one block + 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)); - ASSERT_EQ("3", *mset->getByIndex(2)); - ASSERT_EQ("4", *mset->getByIndex(3)); + // 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->findFirst(FindPredicate("1"))); - ASSERT_EQ("2", *mset->findFirst(FindPredicate("2"))); - ASSERT_EQ("3", *mset->findFirst(FindPredicate("3"))); - ASSERT_EQ("4", *mset->findFirst(FindPredicate("4"))); - ASSERT_FALSE(mset->findFirst(FindPredicate("nonexistent"))); + 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"))); - // Removing one static - mset->remove("1"); // One of dynamics now migrates to the static storage - mset->remove("foo"); // There's no such thing anyway - ASSERT_EQ(1, pool.getNumUsedBlocks()); - ASSERT_EQ(2, mset->getNumStaticItems()); - ASSERT_EQ(1, mset->getNumDynamicItems()); + // 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 - first dynamic entry has moved to the first static slot - ASSERT_EQ("3", *mset->getByIndex(0)); - ASSERT_EQ("2", *mset->getByIndex(1)); - ASSERT_EQ("4", *mset->getByIndex(2)); + // 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->remove("2"); - ASSERT_EQ(2, mset->getNumStaticItems()); - ASSERT_EQ(0, mset->getNumDynamicItems()); - ASSERT_EQ(0, pool.getNumUsedBlocks()); // No dynamic entries left + mset->removeFirst("2"); + ASSERT_EQ(0, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + ASSERT_LE(1, pool.getNumUsedBlocks()); - // Adding some new dynamics + // 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->add(value); // Will override some from the above + std::string* res = mset->add(value); // Will NOT override above if (res == NULL) { ASSERT_LT(2, i); @@ -121,25 +124,18 @@ TEST(Multiset, Basic) max_value_integer = unsigned(i); } std::cout << "Max value: " << max_value_integer << std::endl; - ASSERT_LT(4, max_value_integer); // Making sure there is true OOM ASSERT_EQ(0, pool.getNumFreeBlocks()); ASSERT_FALSE(mset->add("nonexistent")); // Removing odd values - nearly half of them - ASSERT_EQ(2, mset->getNumStaticItems()); - const unsigned num_dynamics_old = mset->getNumDynamicItems(); - mset->removeWhere(oddValuePredicate); - ASSERT_EQ(2, mset->getNumStaticItems()); - const unsigned num_dynamics_new = mset->getNumDynamicItems(); - std::cout << "Num of dynamic pairs reduced from " << num_dynamics_old << " to " << num_dynamics_new << std::endl; - ASSERT_LT(num_dynamics_new, num_dynamics_old); + mset->removeAllMatching(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->findFirst(FindPredicate(toString(kv_int))); + const std::string* val = mset->find(FindPredicate(toString(kv_int))); if (val) { ASSERT_FALSE(kv_int & 1); @@ -160,16 +156,17 @@ TEST(Multiset, NoStatic) { using uavcan::Multiset; - static const int POOL_BLOCKS = 3; - uavcan::PoolAllocator pool; + uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything uavcan::PoolManager<2> poolmgr; poolmgr.addPool(&pool); typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); + ASSERT_LE(2, MultisetType::NumItemsPerDynamicChunk); + // Empty - mset->remove("foo"); + mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); ASSERT_FALSE(mset->getByIndex(0)); @@ -192,16 +189,17 @@ TEST(Multiset, PrimitiveKey) { using uavcan::Multiset; - static const int POOL_BLOCKS = 3; - uavcan::PoolAllocator pool; + uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything uavcan::PoolManager<2> poolmgr; poolmgr.addPool(&pool); - typedef Multiset MultisetType; + typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); + ASSERT_LE(2, MultisetType::NumItemsPerDynamicChunk); + // Empty - mset->remove(8); + mset->removeFirst(8); ASSERT_EQ(0, pool.getNumUsedBlocks()); ASSERT_EQ(0, mset->getSize()); ASSERT_FALSE(mset->getByIndex(0)); From 5e7f81c11b57b65bfd96af244b4319d8a9826ace Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 02:36:45 +0300 Subject: [PATCH 109/268] Fixed Multiset tests --- libuavcan/test/util/multiset.cpp | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 21f74500cd..f89f61fd0a 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -156,15 +156,14 @@ TEST(Multiset, NoStatic) { using uavcan::Multiset; - uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; uavcan::PoolManager<2> poolmgr; poolmgr.addPool(&pool); typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); - ASSERT_LE(2, MultisetType::NumItemsPerDynamicChunk); - // Empty mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); @@ -173,15 +172,17 @@ TEST(Multiset, NoStatic) // Insertion ASSERT_EQ("a", *mset->add("a")); ASSERT_EQ("b", *mset->add("b")); - ASSERT_EQ(1, pool.getNumUsedBlocks()); + ASSERT_LE(1, pool.getNumUsedBlocks()); ASSERT_EQ(0, mset->getNumStaticItems()); ASSERT_EQ(2, mset->getNumDynamicItems()); - // Ordering +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + // Only C++11 because C++03 uses one entry per pool block which breaks ordering ASSERT_EQ("a", *mset->getByIndex(0)); ASSERT_EQ("b", *mset->getByIndex(1)); ASSERT_FALSE(mset->getByIndex(3)); ASSERT_FALSE(mset->getByIndex(1000)); +#endif } @@ -189,15 +190,14 @@ TEST(Multiset, PrimitiveKey) { using uavcan::Multiset; - uavcan::PoolAllocator<1024, 128> pool; // Large enough to keep everything + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; uavcan::PoolManager<2> poolmgr; poolmgr.addPool(&pool); typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); - ASSERT_LE(2, MultisetType::NumItemsPerDynamicChunk); - // Empty mset->removeFirst(8); ASSERT_EQ(0, pool.getNumUsedBlocks()); @@ -213,11 +213,13 @@ TEST(Multiset, PrimitiveKey) ASSERT_EQ(4, *mset->add(4)); ASSERT_EQ(4, mset->getSize()); - // Ordering +#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 } From 24f0ec56f4030289c4abc4aa666376e7d990d2bd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 03:14:46 +0300 Subject: [PATCH 110/268] Multiset fixes and more tests --- libuavcan/include/uavcan/util/multiset.hpp | 70 ++++++++--------- libuavcan/test/util/multiset.cpp | 87 ++++++++++++++++++---- 2 files changed, 107 insertions(+), 50 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 1c62dd3fdf..d39b31b7c5 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -122,6 +122,11 @@ private: void compact(); + enum RemoveStrategy { RemoveOne, RemoveAll }; + + template + void removeWhere(Predicate predicate, RemoveStrategy strategy); + struct YesPredicate { bool operator()(const T&) const { return true; } @@ -156,7 +161,7 @@ protected: { } #endif - /// Derived class destructor must call removeAll(); + /// Derived class destructor must call clear(); ~MultisetBase() { UAVCAN_ASSERT(getSize() == 0); @@ -164,15 +169,11 @@ protected: public: /** - * This is needed for testing. + * 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). */ - enum { NumItemsPerDynamicChunk = Chunk::NumItems }; - - /** - * Adds one item and returns a pointer to it. - * If add fails due to lack of memory, NULL will be returned. - */ - T* add() + T* emplace() { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -185,7 +186,7 @@ public: } template - T* add(P1 p1) + T* emplace(P1 p1) { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -198,7 +199,7 @@ public: } template - T* add(P1 p1, P2 p2) + T* emplace(P1 p1, P2 p2) { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -211,7 +212,7 @@ public: } template - T* add(P1 p1, P2 p2, P3 p3) + T* emplace(P1 p1, P2 p2, P3 p3) { Item* const item = findOrCreateFreeSlot(); if (item == NULL) @@ -223,26 +224,22 @@ public: return item->ptr; } - /** - * @ref removeMatching() - */ - enum RemoveStrategy { RemoveOne, RemoveAll }; - /** * Removes entries where the predicate returns true. * Predicate prototype: * bool (T& item) */ template - void removeMatching(Predicate predicate, RemoveStrategy strategy); + void removeAllWhere(Predicate predicate) { removeWhere(predicate, RemoveAll); } template - void removeAllMatching(Predicate predicate) { removeMatching(predicate, RemoveAll); } + void removeFirstWhere(Predicate predicate) { removeWhere(predicate, RemoveOne); } - template - void removeFirstMatching(Predicate predicate) { removeMatching(predicate, RemoveOne); } + void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); } - void removeFirst(const T& ref) { removeFirstMatching(ComparingPredicate(ref)); } + void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); } + + void clear() { removeAllWhere(YesPredicate()); } /** * Returns first entry where the predicate returns true. @@ -258,15 +255,11 @@ public: return const_cast*>(this)->find(predicate); } - /** - * Removes all items; all pool memory will be released. - */ - void removeAll() { removeAllMatching(YesPredicate()); } - /** * Returns an item located at the specified position from the beginning. - * Note that any insertion or deletion may greatly disturb internal ordering, so use with care. + * 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) { @@ -280,7 +273,7 @@ public: } /** - * This is O(1) + * Complexity is O(N). */ bool isEmpty() const { return find(YesPredicate()) == NULL; } @@ -312,7 +305,7 @@ public: : MultisetBase(static_, NumStaticEntries, allocator) { } - ~Multiset() { this->removeAll(); } + ~Multiset() { this->clear(); } #endif // !UAVCAN_TINY }; @@ -330,7 +323,7 @@ public: #endif { } - ~Multiset() { this->removeAll(); } + ~Multiset() { this->clear(); } }; // ---------------------------------------------------------------------------- @@ -403,7 +396,7 @@ void MultisetBase::compact() template template -void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy strategy) +void MultisetBase::removeWhere(Predicate predicate, const RemoveStrategy strategy) { unsigned num_removed = 0; @@ -416,13 +409,12 @@ void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy s { num_removed++; static_[i].destroy(); + if (strategy == RemoveOne) + { + break; + } } } - - if ((num_removed > 0) && (strategy == RemoveOne)) - { - break; - } } #endif @@ -443,6 +435,10 @@ void MultisetBase::removeMatching(Predicate predicate, const RemoveStrategy s { num_removed++; item.destroy(); + if (strategy == RemoveOne) + { + break; + } } } } diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index f89f61fd0a..2c49e77122 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -30,6 +30,25 @@ struct FindPredicate 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; + TEST(Multiset, Basic) { @@ -51,8 +70,8 @@ TEST(Multiset, Basic) ASSERT_FALSE(mset->getByIndex(10000)); // Static addion - ASSERT_EQ("1", *mset->add("1")); - ASSERT_EQ("2", *mset->add("2")); + 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()); @@ -62,11 +81,11 @@ TEST(Multiset, Basic) ASSERT_TRUE(*mset->getByIndex(1) == "2"); // Dynamic addition - ASSERT_EQ("3", *mset->add("3")); + ASSERT_EQ("3", *mset->emplace("3")); ASSERT_EQ("3", *mset->getByIndex(2)); ASSERT_EQ(1, pool.getNumUsedBlocks()); - ASSERT_EQ("4", *mset->add("4")); + ASSERT_EQ("4", *mset->emplace("4")); ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more ASSERT_EQ(2, mset->getNumStaticItems()); ASSERT_EQ(2, mset->getNumDynamicItems()); @@ -111,7 +130,7 @@ TEST(Multiset, Basic) for (int i = 0; i < 100; i++) { const std::string value = toString(i); - std::string* res = mset->add(value); // Will NOT override above + std::string* res = mset->emplace(value); // Will NOT override above if (res == NULL) { ASSERT_LT(2, i); @@ -127,10 +146,10 @@ TEST(Multiset, Basic) // Making sure there is true OOM ASSERT_EQ(0, pool.getNumFreeBlocks()); - ASSERT_FALSE(mset->add("nonexistent")); + ASSERT_FALSE(mset->emplace("nonexistent")); // Removing odd values - nearly half of them - mset->removeAllMatching(oddValuePredicate); + mset->removeAllWhere(oddValuePredicate); // Making sure there's no odd values left for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++) @@ -170,8 +189,8 @@ TEST(Multiset, NoStatic) ASSERT_FALSE(mset->getByIndex(0)); // Insertion - ASSERT_EQ("a", *mset->add("a")); - ASSERT_EQ("b", *mset->add("b")); + ASSERT_EQ("a", *mset->emplace("a")); + ASSERT_EQ("b", *mset->emplace("b")); ASSERT_LE(1, pool.getNumUsedBlocks()); ASSERT_EQ(0, mset->getNumStaticItems()); ASSERT_EQ(2, mset->getNumDynamicItems()); @@ -205,12 +224,12 @@ TEST(Multiset, PrimitiveKey) ASSERT_FALSE(mset->getByIndex(0)); // Insertion - ASSERT_EQ(1, *mset->add(1)); + ASSERT_EQ(1, *mset->emplace(1)); ASSERT_EQ(1, mset->getSize()); - ASSERT_EQ(2, *mset->add(2)); + ASSERT_EQ(2, *mset->emplace(2)); ASSERT_EQ(2, mset->getSize()); - ASSERT_EQ(3, *mset->add(3)); - ASSERT_EQ(4, *mset->add(4)); + ASSERT_EQ(3, *mset->emplace(3)); + ASSERT_EQ(4, *mset->emplace(4)); ASSERT_EQ(4, mset->getSize()); #if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 @@ -223,3 +242,45 @@ TEST(Multiset, PrimitiveKey) ASSERT_FALSE(mset->getByIndex(1000)); #endif } + + +TEST(Multiset, NoncopyableWithCounter) +{ + using uavcan::Multiset; + + static const int POOL_BLOCKS = 3; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + typedef Multiset MultisetType; + std::auto_ptr 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 +} From eb370b08dd42696fdf7d8e7b8594aef8bf772e44 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 03:31:18 +0300 Subject: [PATCH 111/268] Refactored Map<> --- .../node_discoverer.hpp | 2 +- .../uavcan/protocol/node_info_retriever.hpp | 14 +++--- .../transport/outgoing_transfer_registry.hpp | 4 +- .../uavcan/transport/transfer_listener.hpp | 2 +- libuavcan/include/uavcan/util/map.hpp | 46 +++++++++---------- .../src/transport/uc_transfer_listener.cpp | 2 +- libuavcan/test/util/map.cpp | 22 ++++----- 7 files changed, 46 insertions(+), 46 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 74280ac9f4..03b4145759 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -146,7 +146,7 @@ class NodeDiscoverer : TimerBase { HighestUptimeSearcher searcher; - const NodeID* const out = node_map_.findFirstKey(searcher); + const NodeID* const out = node_map_.find(searcher); (void)out; UAVCAN_ASSERT(out == NULL); diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index c2656b645c..29391a15a6 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -253,7 +253,7 @@ private: } } - listeners_.removeWhere( + listeners_.removeAllWhere( GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); } @@ -271,8 +271,8 @@ private: entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; - listeners_.removeWhere(GenericHandlerCaller&>( - &INodeInfoListener::handleNodeStatusMessage, msg)); + listeners_.removeAllWhere(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); } void handleGetNodeInfoResponse(const ServiceCallResult& result) @@ -287,8 +287,8 @@ private: */ entry.uptime_sec = result.getResponse().status.uptime_sec; entry.request_needed = false; - listeners_.removeWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, - result.getResponse())); + listeners_.removeAllWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); } else { @@ -298,8 +298,8 @@ private: if (entry.num_attempts_made >= num_attempts_) { entry.request_needed = false; - listeners_.removeWhere(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, - result.getCallID().server_node_id)); + listeners_.removeAllWhere(GenericHandlerCaller( + &INodeInfoListener::handleNodeInfoUnavailable, result.getCallID().server_node_id)); } } } diff --git a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp index 114cecee22..2dbc508118 100644 --- a/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp +++ b/libuavcan/include/uavcan/transport/outgoing_transfer_registry.hpp @@ -165,13 +165,13 @@ TransferID* OutgoingTransferRegistry::accessOrCreate(const Out template bool OutgoingTransferRegistry::exists(DataTypeID dtid, TransferType tt) const { - return NULL != map_.findFirstKey(ExistenceCheckingPredicate(dtid, tt)); + return NULL != map_.find(ExistenceCheckingPredicate(dtid, tt)); } template void OutgoingTransferRegistry::cleanup(MonotonicTime ts) { - map_.removeWhere(DeadlineExpiredPredicate(ts)); + map_.removeAllWhere(DeadlineExpiredPredicate(ts)); } } diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 62be67d64b..428ec1489b 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -179,7 +179,7 @@ public: virtual ~TransferListener() { // Map must be cleared before bufmgr is destroyed - receivers_.removeAll(); + receivers_.clear(); } }; diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index ed5535266d..1f805fa0a2 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -108,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(); @@ -117,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: @@ -137,7 +137,7 @@ protected: } #endif - /// Derived class destructor must call removeAll(); + /// Derived class destructor must call clear(); ~MapBase() { UAVCAN_ASSERT(getSize() == 0); @@ -165,7 +165,7 @@ public: * bool (Key& key, Value& value) */ template - void removeWhere(Predicate predicate); + void removeAllWhere(Predicate predicate); /** * Returns first entry where the predicate returns true. @@ -173,9 +173,12 @@ public: * bool (const Key& key, const Value& value) */ template - 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. @@ -185,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; @@ -211,7 +217,7 @@ public: : MapBase(static_, NumStaticEntries, allocator) { } - ~Map() { this->removeAll(); } + ~Map() { this->clear(); } #endif // !UAVCAN_TINY }; @@ -229,7 +235,7 @@ public: #endif { } - ~Map() { this->removeAll(); } + ~Map() { this->clear(); } }; // ---------------------------------------------------------------------------- @@ -238,7 +244,7 @@ public: * MapBase<> */ template -typename MapBase::KVPair* MapBase::find(const Key& key) +typename MapBase::KVPair* MapBase::findKey(const Key& key) { #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) @@ -348,7 +354,7 @@ template Value* MapBase::access(const Key& key) { UAVCAN_ASSERT(!(key == Key())); - KVPair* const kv = find(key); + KVPair* const kv = findKey(key); return kv ? &kv->value : NULL; } @@ -358,7 +364,7 @@ Value* MapBase::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); @@ -379,7 +385,7 @@ template void MapBase::remove(const Key& key) { UAVCAN_ASSERT(!(key == Key())); - KVPair* const kv = find(key); + KVPair* const kv = findKey(key); if (kv) { *kv = KVPair(); @@ -392,7 +398,7 @@ void MapBase::remove(const Key& key) template template -void MapBase::removeWhere(Predicate predicate) +void MapBase::removeAllWhere(Predicate predicate) { unsigned num_removed = 0; @@ -439,7 +445,7 @@ void MapBase::removeWhere(Predicate predicate) template template -const Key* MapBase::findFirstKey(Predicate predicate) const +const Key* MapBase::find(Predicate predicate) const { #if !UAVCAN_TINY for (unsigned i = 0; i < num_static_entries_; i++) @@ -474,9 +480,9 @@ const Key* MapBase::findFirstKey(Predicate predicate) const } template -void MapBase::removeAll() +void MapBase::clear() { - removeWhere(YesPredicate()); + removeAllWhere(YesPredicate()); } template @@ -525,12 +531,6 @@ const typename MapBase::KVPair* MapBase::getByIndex(unsi return const_cast*>(this)->getByIndex(index); } -template -bool MapBase::isEmpty() const -{ - return getSize() == 0; -} - template unsigned MapBase::getSize() const { diff --git a/libuavcan/src/transport/uc_transfer_listener.cpp b/libuavcan/src/transport/uc_transfer_listener.cpp index a48878f0c1..38cdb726eb 100644 --- a/libuavcan/src/transport/uc_transfer_listener.cpp +++ b/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); } diff --git a/libuavcan/test/util/map.cpp b/libuavcan/test/util/map.cpp index 3f8671ede9..e394415b00 100644 --- a/libuavcan/test/util/map.cpp +++ b/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; From f713ef5e00864d80915e20bcadd871c10db86890 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 03:36:01 +0300 Subject: [PATCH 112/268] LazyConstructor memory optimization --- libuavcan/include/uavcan/util/lazy_constructor.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/libuavcan/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 93152adc71..08ff9ab5f9 100644 --- a/libuavcan/include/uavcan/util/lazy_constructor.hpp +++ b/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -26,12 +26,19 @@ namespace uavcan template 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_; From be5bcf9084cbb30cda0c507fd48cd43199da8d2e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 13:21:36 +0300 Subject: [PATCH 113/268] ParameterType<> template --- .../include/uavcan/util/lazy_constructor.hpp | 39 +++++++++---------- libuavcan/include/uavcan/util/templates.hpp | 9 +++++ 2 files changed, 27 insertions(+), 21 deletions(-) diff --git a/libuavcan/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 08ff9ab5f9..c7ae975663 100644 --- a/libuavcan/include/uavcan/util/lazy_constructor.hpp +++ b/libuavcan/include/uavcan/util/lazy_constructor.hpp @@ -58,12 +58,6 @@ class UAVCAN_EXPORT LazyConstructor } } - template struct ParamType { typedef const U& Type; }; - template struct ParamType { typedef U& Type; }; -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - template struct ParamType { typedef U&& Type; }; -#endif - public: LazyConstructor() : ptr_(NULL) @@ -130,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::Type p%d' % (x, x) for x in nums] +// l2 = ['typename ParameterType::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 - void construct(typename ParamType::Type p1) + void construct(typename ParameterType::Type p1) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3, - typename ParamType::Type p4) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3, - typename ParamType::Type p4, typename ParamType::Type p5) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5); } - template - void construct(typename ParamType::Type p1, typename ParamType::Type p2, typename ParamType::Type p3, - typename ParamType::Type p4, typename ParamType::Type p5, typename ParamType::Type p6) + template + void construct(typename ParameterType::Type p1, typename ParameterType::Type p2, + typename ParameterType::Type p3, typename ParameterType::Type p4, + typename ParameterType::Type p5, typename ParameterType::Type p6) { ensureNotConstructed(); ptr_ = new (static_cast(data_.pool)) T(p1, p2, p3, p4, p5, p6); diff --git a/libuavcan/include/uavcan/util/templates.hpp b/libuavcan/include/uavcan/util/templates.hpp index e2572ff3d3..b32e559d87 100644 --- a/libuavcan/include/uavcan/util/templates.hpp +++ b/libuavcan/include/uavcan/util/templates.hpp @@ -100,6 +100,15 @@ template struct RemoveReference { typedef T Type; }; template struct RemoveReference { typedef T Type; }; #endif +/** + * Parameter types + */ +template struct ParameterType { typedef const U& Type; }; +template struct ParameterType { typedef U& Type; }; +#if UAVCAN_CPP_VERSION > UAVCAN_CPP03 +template struct ParameterType { typedef U&& Type; }; +#endif + /** * Value types */ From 713ec48ce9d41074ef556f90e83b0ad83ea0a692 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 13:38:42 +0300 Subject: [PATCH 114/268] Multiset<>::forEach() --- libuavcan/include/uavcan/util/multiset.hpp | 63 ++++++++++++++++++++-- libuavcan/test/util/multiset.cpp | 56 +++++++++++++++++++ 2 files changed, 115 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index d39b31b7c5..0f8b0799f8 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -135,15 +135,50 @@ private: struct IndexPredicate : ::uavcan::Noncopyable { unsigned index; - IndexPredicate(unsigned target_index) : index(target_index) { } - bool operator()(const T&) { return index--==0; } + 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; } + + ComparingPredicate(const T& ref) + : reference(ref) + { } + + bool operator()(const T& sample) + { + return reference == sample; + } + }; + + template + struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable + { + const typename ParameterType::Type oper; + + OperatorToFalsePredicateAdapter(typename ParameterType::Type o) + : oper(o) + { } + + bool operator()(T& item) + { + oper(item); + return false; + } + + bool operator()(const T& item) const + { + oper(item); + return false; + } }; protected: @@ -255,6 +290,26 @@ public: return const_cast*>(this)->find(predicate); } + /** + * Calls Operator for each item of the set. + * Operator prototype: + * void (T& item) + * void (const T& item) - const overload + */ + template + void forEach(Operator oper) + { + OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + + template + void forEach(Operator oper) const + { + const OperatorToFalsePredicateAdapter adapter(oper); + (void)find&>(adapter); + } + /** * Returns an item located at the specified position from the beginning. * Note that addition and removal operations invalidate indices. diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 2c49e77122..35e7376e75 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -49,6 +49,20 @@ struct NoncopyableWithCounter : uavcan::Noncopyable int NoncopyableWithCounter::num_objects = 0; +template +struct SummationOperator : uavcan::Noncopyable +{ + T accumulator; + SummationOperator() : accumulator() { } + void operator()(const T& x) { accumulator += x; } +}; + +struct ClearingOperator +{ + template + void operator()(T& x) const { x = T(); } +}; + TEST(Multiset, Basic) { @@ -62,6 +76,8 @@ TEST(Multiset, Basic) typedef Multiset MultisetType; std::auto_ptr mset(new MultisetType(poolmgr)); + typedef SummationOperator StringConcatenationOperator; + // Empty mset->removeFirst("foo"); ASSERT_EQ(0, pool.getNumUsedBlocks()); @@ -80,6 +96,12 @@ TEST(Multiset, Basic) ASSERT_TRUE(*mset->getByIndex(0) == "1"); ASSERT_TRUE(*mset->getByIndex(1) == "2"); + { + StringConcatenationOperator op; + mset->forEach(op); + ASSERT_EQ("12", op.accumulator); + } + // Dynamic addition ASSERT_EQ("3", *mset->emplace("3")); ASSERT_EQ("3", *mset->getByIndex(2)); @@ -107,6 +129,13 @@ TEST(Multiset, Basic) ASSERT_EQ("4", *mset->find(FindPredicate("4"))); ASSERT_FALSE(mset->find(FindPredicate("nonexistent"))); + { + StringConcatenationOperator op; + mset->forEach(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 @@ -165,6 +194,20 @@ TEST(Multiset, Basic) } } + // Clearing all strings + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator before clearing: " << op.accumulator << std::endl; + } + mset->forEach(ClearingOperator()); + { + StringConcatenationOperator op; + mset->forEach(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()); @@ -241,6 +284,19 @@ TEST(Multiset, PrimitiveKey) ASSERT_FALSE(mset->getByIndex(5)); ASSERT_FALSE(mset->getByIndex(1000)); #endif + + // Summation and clearing + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(1 + 2 + 3 + 4, summation_operator.accumulator); + } + mset->forEach(ClearingOperator()); + { + SummationOperator summation_operator; + mset->forEach&>(summation_operator); + ASSERT_EQ(0, summation_operator.accumulator); + } } From 861315d1c375c4e5552f6269ef6d690a5b9506b0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 13:44:03 +0300 Subject: [PATCH 115/268] Typo --- libuavcan/include/uavcan/util/multiset.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 0f8b0799f8..b9a9b4b07e 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -328,7 +328,7 @@ public: } /** - * Complexity is O(N). + * Complexity is O(1). */ bool isEmpty() const { return find(YesPredicate()) == NULL; } From 39b924cd8ad6d6e0400cb078911647a00706ba86 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 14:06:32 +0300 Subject: [PATCH 116/268] Multiset storage alignment fix --- libuavcan/include/uavcan/util/multiset.hpp | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index b9a9b4b07e..d796b1bff6 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -39,7 +39,12 @@ protected: union { unsigned char pool[sizeof(T)]; - long double _aligner1_; + /* + * 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 From 40e68d41033b29ac39f16e627a0a44f715114c39 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 14:09:15 +0300 Subject: [PATCH 117/268] TransferListenerWithFilter - NULL checks removed as they were conflicting with ServiceClient<>'s logic --- libuavcan/include/uavcan/transport/transfer_listener.hpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 428ec1489b..6b57c25e99 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -218,9 +218,7 @@ public: void installAcceptanceFilter(const ITransferAcceptanceFilter* acceptance_filter) { - UAVCAN_ASSERT(filter_ == NULL); filter_ = acceptance_filter; - UAVCAN_ASSERT(filter_ != NULL); } }; From b2b7693ee66a14fbfedbdfdcbece241972252f71 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 14:19:48 +0300 Subject: [PATCH 118/268] Partially implemented and fixed ServiceClient<>, 7 tests are failing --- .../include/uavcan/node/service_client.hpp | 51 +++++++------------ 1 file changed, 17 insertions(+), 34 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 8a9dced967..feb10702f9 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -222,10 +222,8 @@ private: TransferListenerType; typedef GenericSubscriber SubscriberType; -#if 0 typedef Multiset CallRegistry; CallRegistry call_registry_; -#endif PublisherType publisher_; Callback callback_; @@ -247,6 +245,7 @@ public: */ explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) + , call_registry_(node.getAllocator()) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) { @@ -300,15 +299,15 @@ public: const Callback& getCallback() const { return callback_; } void setCallback(const Callback& cb) { callback_ = cb; } -#if 0 + /** + * Complexity is O(N) of number of pending calls. + */ unsigned getNumPendingCalls() const { return call_registry_.getSize(); } -#endif -#if 0 + /** + * Complexity is O(1). + */ bool hasPendingCalls() const { return !call_registry_.isEmpty(); } -#else - bool hasPendingCalls() const { return false; } -#endif /** * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: @@ -353,13 +352,9 @@ bool ServiceClient::shouldAcceptFrame(con { UAVCAN_ASSERT(frame.getTransferType() == TransferTypeServiceResponse); // Other types filtered out by dispatcher -#if 0 - return call_registry_.findFirst(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), - frame.getTransferID()))) != NULL; -#else - (void)frame; - return false; -#endif + return NULL != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))); + } template @@ -387,7 +382,6 @@ void ServiceClient::handleTimeout(Service template int ServiceClient::addCallState(ServiceCallID call_id) { -#if 0 if (call_registry_.isEmpty()) { const int subscriber_res = SubscriberType::startAsServiceResponseListener(); @@ -398,30 +392,25 @@ int ServiceClient::addCallState(ServiceCa } } - if (call_registry_.add(CallState(SubscriberType::getNode(), *this, call_id)) == NULL) + if (NULL == call_registry_.template emplace(SubscriberType::getNode(), + *this, call_id)) { SubscriberType::stop(); return -ErrMemory; } return 0; -#else - (void)call_id; - return -ErrNotInited; -#endif } template -int ServiceClient::call(NodeID server_node_id, - const RequestType& request) +int ServiceClient::call(NodeID server_node_id, const RequestType& request) { ServiceCallID dummy; return call(server_node_id, request, dummy); } template -int ServiceClient::call(NodeID server_node_id, - const RequestType& request, +int ServiceClient::call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id) { if (!try_implicit_cast(callback_, true)) @@ -475,29 +464,23 @@ int ServiceClient::call(NodeID server_nod return publisher_res; } - return 0; + return publisher_res; } template void ServiceClient::cancel(ServiceCallID call_id) { -#if 0 - call_registry_.remove(call_id); + call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); if (call_registry_.isEmpty()) { SubscriberType::stop(); } -#else - (void)call_id; -#endif } template void ServiceClient::cancelAll() { -#if 0 - call_registry_.removeAll(); -#endif + call_registry_.clear(); SubscriberType::stop(); } From da98060a5876a392d6e2a0a268125477bcda4df4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 15:03:53 +0300 Subject: [PATCH 119/268] Nasty bug in ServiceClient<>::call() --- libuavcan/include/uavcan/node/service_client.hpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index feb10702f9..b0ba225baa 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -422,7 +422,6 @@ int ServiceClient::call(NodeID server_nod /* * 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, out_call_id); if (prep_res < 0) @@ -457,13 +456,15 @@ int ServiceClient::call(NodeID server_nod /* * 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(out_call_id); return publisher_res; } + UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id); return publisher_res; } From e921f4da026f6823b30d48b6acb133fed8e387ee Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 15:07:21 +0300 Subject: [PATCH 120/268] More debug outputs --- libuavcan/include/uavcan/node/generic_subscriber.hpp | 6 ++++++ libuavcan/test/node/service_client.cpp | 6 ++++++ 2 files changed, 12 insertions(+) diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index e3d51c9440..235f5518df 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/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_); } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index fa08b7214a..bef7ea69c1 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -105,10 +105,14 @@ 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, client2.call(1, request)); // OK ASSERT_LT(0, client3.call(99, request)); // Will timeout! + std::cout << "!!! Spinning!" << std::endl; + ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! ASSERT_TRUE(client1.hasPendingCalls()); @@ -117,6 +121,8 @@ TEST(ServiceClient, Basic) 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.hasPendingCalls()); From cbbb3bd9bebea5541d04db4ade2837d3a0d90ca1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 15:12:10 +0300 Subject: [PATCH 121/268] All tests are passing --- .../test/protocol/dynamic_node_id_server/node_discoverer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 9a469c4461..7d1d692976 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -173,7 +173,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, Basic) get_node_info_server.response.hardware_version.unique_id[14] = 52; ASSERT_LE(0, get_node_info_server.start()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(400)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 81533eda46b7d3bed851cac0da193bfd8cd26cf0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:04:25 +0300 Subject: [PATCH 122/268] Method to generate immediate deadlines in DeadlineHandler --- libuavcan/include/uavcan/node/scheduler.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/include/uavcan/node/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 847f76b9d9..0d1d82553c 100644 --- a/libuavcan/include/uavcan/node/scheduler.hpp +++ b/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(); From 9ba6050af15548701f5cff0c99c6a27c252046fc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:12:15 +0300 Subject: [PATCH 123/268] ServiceClient<>: proper destruction of CallState objects via execution relaying --- .../include/uavcan/node/service_client.hpp | 69 +++++++++++++++---- libuavcan/src/node/uc_service_client.cpp | 13 +++- 2 files changed, 66 insertions(+), 16 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b0ba225baa..1ebd2f5bcb 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -121,7 +121,8 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& scr) /** * Do not use directly. */ -class ServiceClientBase : public ITransferAcceptanceFilter, Noncopyable +class ServiceClientBase : protected ITransferAcceptanceFilter + , protected DeadlineHandler { const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call @@ -130,6 +131,7 @@ protected: { ServiceClientBase& owner_; const ServiceCallID id_; + bool timed_out_; virtual void handleDeadline(MonotonicTime); @@ -138,12 +140,17 @@ protected: : DeadlineHandler(node.getScheduler()) , owner_(owner) , id_(call_id) + , timed_out_(false) { UAVCAN_ASSERT(id_.isValid()); DeadlineHandler::startWithDelay(owner_.request_timeout_); } - bool doesMatch(ServiceCallID call_id) const { return call_id == id_; } + 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 { @@ -155,13 +162,14 @@ protected: { const ServiceCallID id; CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } - bool operator()(const CallState& state) const { return state.doesMatch(id); } + bool operator()(const CallState& state) const { return state.getCallID() == id; } }; MonotonicDuration request_timeout_; - ServiceClientBase() - : data_type_descriptor_(NULL) + ServiceClientBase(INode& node) + : DeadlineHandler(node.getScheduler()) + , data_type_descriptor_(NULL) , request_timeout_(getDefaultRequestTimeout()) { } @@ -169,8 +177,6 @@ protected: int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id); - virtual void handleTimeout(ServiceCallID call_id) = 0; - public: /** * It's not recommended to override default timeouts. @@ -223,6 +229,27 @@ private: typedef GenericSubscriber SubscriberType; typedef Multiset 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_; @@ -234,7 +261,7 @@ private: virtual void handleReceivedDataStruct(ReceivedDataStructure& response); - virtual void handleTimeout(ServiceCallID call_id); + virtual void handleDeadline(MonotonicTime); int addCallState(ServiceCallID call_id); @@ -245,6 +272,7 @@ public: */ explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) + , ServiceClientBase(node) , call_registry_(node.getAllocator()) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) @@ -371,12 +399,27 @@ handleReceivedDataStruct(ReceivedDataStructure& response) template -void ServiceClient::handleTimeout(ServiceCallID call_id) +void ServiceClient::handleDeadline(MonotonicTime) { - cancel(call_id); - ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, call_id, - SubscriberType::getReceivedStructStorage()); // Mutable! - invokeCallback(result); + UAVCAN_TRACE("ServiceClient", "Shared deadline event received"); + /* + * Invoking callbacks for timed out call state objects. + */ + TimeoutCallbackCaller callback_caller(*this); + call_registry_.template forEach(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()) + { + SubscriberType::stop(); + } } template diff --git a/libuavcan/src/node/uc_service_client.cpp b/libuavcan/src/node/uc_service_client.cpp index f686db20c5..49c8179a3a 100644 --- a/libuavcan/src/node/uc_service_client.cpp +++ b/libuavcan/src/node/uc_service_client.cpp @@ -11,11 +11,18 @@ namespace uavcan */ void ServiceClientBase::CallState::handleDeadline(MonotonicTime) { - UAVCAN_ASSERT(id_.isValid()); - UAVCAN_TRACE("ServiceClient", "Timeout from nid=%d, tid=%d, dtname=%s", + 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()); - owner_.handleTimeout(id_); + /* + * 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"); } /* From 90d60688b3d32242aea25e5944d561aab42bd1c7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:19:54 +0300 Subject: [PATCH 124/268] ServiceClient<>: renaming and a minor logic fix --- .../include/uavcan/node/service_client.hpp | 20 ++++++++++--------- .../distributed/raft_core.hpp | 4 ++-- .../protocol/network_compat_checker.hpp | 2 +- 3 files changed, 14 insertions(+), 12 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 1ebd2f5bcb..576ea47bcb 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -162,7 +162,7 @@ protected: { const ServiceCallID id; CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } - bool operator()(const CallState& state) const { return state.getCallID() == id; } + bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); } }; MonotonicDuration request_timeout_; @@ -283,7 +283,7 @@ public: #endif } - virtual ~ServiceClient() { cancelAll(); } + virtual ~ServiceClient() { cancelAllCalls(); } /** * Shall be called before first use. @@ -314,12 +314,12 @@ public: /** * Cancels certain call referred via call ID structure. */ - void cancel(ServiceCallID call_id); + void cancelCall(ServiceCallID call_id); /** * Cancels all pending calls. */ - void cancelAll(); + void cancelAllCalls(); /** * Service response callback must be set prior service call. @@ -329,11 +329,13 @@ public: /** * 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(); } @@ -392,7 +394,7 @@ handleReceivedDataStruct(ReceivedDataStructure& response) UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); - cancel(call_id); + cancelCall(call_id); ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! invokeCallback(result); } @@ -491,7 +493,7 @@ int ServiceClient::call(NodeID server_nod if (tl == NULL) { UAVCAN_ASSERT(0); // Must have been created - cancel(out_call_id); + cancelCall(out_call_id); return -ErrLogic; } tl->installAcceptanceFilter(this); @@ -503,7 +505,7 @@ int ServiceClient::call(NodeID server_nod out_call_id.transfer_id); if (publisher_res < 0) { - cancel(out_call_id); + cancelCall(out_call_id); return publisher_res; } @@ -512,7 +514,7 @@ int ServiceClient::call(NodeID server_nod } template -void ServiceClient::cancel(ServiceCallID call_id) +void ServiceClient::cancelCall(ServiceCallID call_id) { call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); if (call_registry_.isEmpty()) @@ -522,7 +524,7 @@ void ServiceClient::cancel(ServiceCallID } template -void ServiceClient::cancelAll() +void ServiceClient::cancelAllCalls() { call_registry_.clear(); SubscriberType::stop(); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 634bd002bd..89f84c2260 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -327,9 +327,9 @@ class RaftCore : private TimerBase for (uint8_t i = 0; i < NumRequestVoteClients; i++) { - request_vote_clients_[i]->cancelAll(); // TODO FIXME Concurrent calls!! + request_vote_clients_[i]->cancelAllCalls(); // TODO FIXME Concurrent calls!! } - append_entries_client_.cancelAll(); + append_entries_client_.cancelAllCalls(); /* * Calling the switch handler diff --git a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index 3ac0ae0264..461085eb05 100644 --- a/libuavcan/include/uavcan/protocol/network_compat_checker.hpp +++ b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp @@ -253,7 +253,7 @@ public: exit: ns_sub_.stop(); - cats_cln_.cancelAll(); + cats_cln_.cancelAllCalls(); return res; } From dbf3d1622ead3ba134892a647e24b269cbbedac5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 21:25:27 +0300 Subject: [PATCH 125/268] Improved test of ServiceClient<> --- libuavcan/test/node/service_client.cpp | 31 ++++++++++++++++++++++---- 1 file changed, 27 insertions(+), 4 deletions(-) diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index bef7ea69c1..ea1c339a62 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -33,10 +33,18 @@ struct ServiceCallResultHandler 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 Date: Sat, 16 May 2015 22:17:14 +0300 Subject: [PATCH 126/268] ServiceClient<>: test of concurrent call logic --- libuavcan/test/node/service_client.cpp | 123 +++++++++++++++++++++++++ libuavcan/test/node/test_node.hpp | 2 +- 2 files changed, 124 insertions(+), 1 deletion(-) diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index ea1c339a62..4022de062e 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -12,6 +12,7 @@ #include #include #include +#include #include "test_node.hpp" @@ -22,6 +23,7 @@ struct ServiceCallResultHandler StatusType last_status; uavcan::NodeID last_server_node_id; typename DataType::Response last_response; + std::queue responses; void handleResponse(const uavcan::ServiceCallResult& result) { @@ -29,6 +31,7 @@ struct ServiceCallResultHandler 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 @@ -218,6 +221,126 @@ TEST(ServiceClient, Rejection) } +TEST(ServiceClient, ConcurrentCalls) +{ + InterlinkedTestNodesWithSysClock nodes; + + // Type registration + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _registrator; + + // Server + uavcan::ServiceServer server(nodes.a); + ASSERT_EQ(0, server.start(stringServiceServerCallback)); + + // Caller + typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceClient::Binder > ClientType; + ServiceCallResultHandler 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; diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 5aaa60288d..418159aa33 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -11,7 +11,7 @@ struct TestNode : public uavcan::INode { - uavcan::PoolAllocator pool; + uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; uavcan::MarshalBufferProvider<> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; From 16a9d206c6ab285ca370a044e56a4e2d477a9084 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 22:26:04 +0300 Subject: [PATCH 127/268] ServiceClient documentation --- libuavcan/include/uavcan/node/service_client.hpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 576ea47bcb..47035056a1 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -190,12 +190,19 @@ public: /** * 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 = UAVCAN_CPP11 @@ -299,7 +306,7 @@ 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. * * Returns negative error code. @@ -308,6 +315,7 @@ public: /** * 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. */ int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); @@ -347,7 +355,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. */ From 02fe76cd6f09b8b872713fbbd79cdfc6c968a236 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 22:38:42 +0300 Subject: [PATCH 128/268] Simplified Multiset<> --- libuavcan/include/uavcan/util/multiset.hpp | 122 +++++++-------------- libuavcan/test/util/multiset.cpp | 34 ------ 2 files changed, 39 insertions(+), 117 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index d796b1bff6..aae7d295f4 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -24,11 +24,12 @@ namespace uavcan * 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 -class UAVCAN_EXPORT MultisetBase : Noncopyable +template +class UAVCAN_EXPORT Multiset : Noncopyable { -protected: struct Item : ::uavcan::Noncopyable { T* ptr; @@ -116,13 +117,16 @@ private: } }; + /* + * Data + */ LinkedListRoot list_; IPoolAllocator& allocator_; -#if !UAVCAN_TINY - Item* const static_; - const unsigned num_static_entries_; -#endif + Item static_[NumStaticEntries]; + /* + * Methods + */ Item* findOrCreateFreeSlot(); void compact(); @@ -186,28 +190,16 @@ private: } }; -protected: -#if UAVCAN_TINY - MultisetBase(IPoolAllocator& allocator) - : allocator_(allocator) - { - UAVCAN_ASSERT(Item() == Item()); - } -#else - MultisetBase(Item* static_buf, unsigned num_static_entries, IPoolAllocator& allocator) - : allocator_(allocator) - , static_(static_buf) - , num_static_entries_(num_static_entries) - { } -#endif - - /// Derived class destructor must call clear(); - ~MultisetBase() - { - UAVCAN_ASSERT(getSize() == 0); - } - 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. @@ -292,7 +284,7 @@ public: template const T* find(Predicate predicate) const { - return const_cast*>(this)->find(predicate); + return const_cast(this)->find(predicate); } /** @@ -329,7 +321,7 @@ public: const T* getByIndex(unsigned index) const { - return const_cast*>(this)->getByIndex(index); + return const_cast(this)->getByIndex(index); } /** @@ -350,53 +342,17 @@ public: unsigned getNumDynamicItems() const; }; - -template -class UAVCAN_EXPORT Multiset : public MultisetBase -{ - typename MultisetBase::Item static_[NumStaticEntries]; - -public: - -#if !UAVCAN_TINY - - // This instantiation will not be valid in UAVCAN_TINY mode - explicit Multiset(IPoolAllocator& allocator) - : MultisetBase(static_, NumStaticEntries, allocator) - { } - - ~Multiset() { this->clear(); } - -#endif // !UAVCAN_TINY -}; - - -template -class UAVCAN_EXPORT Multiset : public MultisetBase -{ -public: - explicit Multiset(IPoolAllocator& allocator) -#if UAVCAN_TINY - : MultisetBase(allocator) -#else - : MultisetBase(NULL, 0, allocator) -#endif - { } - - ~Multiset() { this->clear(); } -}; - // ---------------------------------------------------------------------------- /* - * MultisetBase<> + * Multiset<> */ -template -typename MultisetBase::Item* MultisetBase::findOrCreateFreeSlot() +template +typename Multiset::Item* Multiset::findOrCreateFreeSlot() { #if !UAVCAN_TINY // Search in static pool - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { if (!static_[i].isConstructed()) { @@ -429,8 +385,8 @@ typename MultisetBase::Item* MultisetBase::findOrCreateFreeSlot() return &chunk->items[0]; } -template -void MultisetBase::compact() +template +void Multiset::compact() { Chunk* p = list_.get(); while (p) @@ -454,14 +410,14 @@ void MultisetBase::compact() } } -template +template template -void MultisetBase::removeWhere(Predicate predicate, const RemoveStrategy strategy) +void Multiset::removeWhere(Predicate predicate, const RemoveStrategy strategy) { unsigned num_removed = 0; #if !UAVCAN_TINY - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { if (static_[i].isConstructed()) { @@ -512,12 +468,12 @@ void MultisetBase::removeWhere(Predicate predicate, const RemoveStrategy stra } } -template +template template -T* MultisetBase::find(Predicate predicate) +T* Multiset::find(Predicate predicate) { #if !UAVCAN_TINY - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { if (static_[i].isConstructed()) { @@ -547,12 +503,12 @@ T* MultisetBase::find(Predicate predicate) return NULL; } -template -unsigned MultisetBase::getNumStaticItems() const +template +unsigned Multiset::getNumStaticItems() const { unsigned num = 0; #if !UAVCAN_TINY - for (unsigned i = 0; i < num_static_entries_; i++) + for (unsigned i = 0; i < NumStaticEntries; i++) { num += static_[i].isConstructed() ? 1U : 0U; } @@ -560,8 +516,8 @@ unsigned MultisetBase::getNumStaticItems() const return num; } -template -unsigned MultisetBase::getNumDynamicItems() const +template +unsigned Multiset::getNumDynamicItems() const { unsigned num = 0; Chunk* p = list_.get(); diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp index 35e7376e75..8ff43e69b9 100644 --- a/libuavcan/test/util/multiset.cpp +++ b/libuavcan/test/util/multiset.cpp @@ -214,40 +214,6 @@ TEST(Multiset, Basic) } -TEST(Multiset, NoStatic) -{ - using uavcan::Multiset; - - static const int POOL_BLOCKS = 3; - uavcan::PoolAllocator pool; - uavcan::PoolManager<2> poolmgr; - poolmgr.addPool(&pool); - - typedef Multiset MultisetType; - std::auto_ptr mset(new MultisetType(poolmgr)); - - // Empty - mset->removeFirst("foo"); - ASSERT_EQ(0, pool.getNumUsedBlocks()); - ASSERT_FALSE(mset->getByIndex(0)); - - // Insertion - ASSERT_EQ("a", *mset->emplace("a")); - ASSERT_EQ("b", *mset->emplace("b")); - ASSERT_LE(1, pool.getNumUsedBlocks()); - ASSERT_EQ(0, mset->getNumStaticItems()); - ASSERT_EQ(2, mset->getNumDynamicItems()); - -#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 - // Only C++11 because C++03 uses one entry per pool block which breaks ordering - ASSERT_EQ("a", *mset->getByIndex(0)); - ASSERT_EQ("b", *mset->getByIndex(1)); - ASSERT_FALSE(mset->getByIndex(3)); - ASSERT_FALSE(mset->getByIndex(1000)); -#endif -} - - TEST(Multiset, PrimitiveKey) { using uavcan::Multiset; From fdf5100985e465d0e9f8042dd55c1624181642f5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 22:46:33 +0300 Subject: [PATCH 129/268] Safer list traversing in Multiset<> and Map<> --- libuavcan/include/uavcan/util/map.hpp | 21 +++++++++++++++------ libuavcan/include/uavcan/util/multiset.hpp | 13 +++++++++---- 2 files changed, 24 insertions(+), 10 deletions(-) diff --git a/libuavcan/include/uavcan/util/map.hpp b/libuavcan/include/uavcan/util/map.hpp index 1f805fa0a2..ece759af0e 100644 --- a/libuavcan/include/uavcan/util/map.hpp +++ b/libuavcan/include/uavcan/util/map.hpp @@ -417,8 +417,10 @@ void MapBase::removeAllWhere(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; @@ -431,7 +433,8 @@ void MapBase::removeAllWhere(Predicate predicate) } } } - p = p->getNextListNode(); + + p = next_group; } if (num_removed > 0) @@ -461,8 +464,10 @@ const Key* MapBase::find(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; @@ -474,7 +479,8 @@ const Key* MapBase::find(Predicate predicate) const } } } - p = p->getNextListNode(); + + p = next_group; } return NULL; } @@ -505,8 +511,10 @@ typename MapBase::KVPair* MapBase::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; @@ -519,7 +527,8 @@ typename MapBase::KVPair* MapBase::getByIndex(unsigned i index--; } } - p = p->getNextListNode(); + + p = next_group; } return NULL; diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index aae7d295f4..90e4bf3d6c 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -435,8 +435,10 @@ void Multiset::removeWhere(Predicate predicate, const Remov #endif Chunk* p = list_.get(); - while (p) + 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; @@ -459,7 +461,7 @@ void Multiset::removeWhere(Predicate predicate, const Remov } } - p = p->getNextListNode(); + p = next_chunk; } if (num_removed > 0) @@ -486,8 +488,10 @@ T* Multiset::find(Predicate predicate) #endif Chunk* p = list_.get(); - while (p) + 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()) @@ -498,7 +502,8 @@ T* Multiset::find(Predicate predicate) } } } - p = p->getNextListNode(); + + p = next_chunk; } return NULL; } From 3f9cad4f3b353cee381e09083c3c05ccd70c10f9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 23:17:26 +0300 Subject: [PATCH 130/268] Multiset: Simpler type handling in predicate adapter template --- libuavcan/include/uavcan/util/multiset.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp index 90e4bf3d6c..c1a99d31ea 100644 --- a/libuavcan/include/uavcan/util/multiset.hpp +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -171,9 +171,9 @@ private: template struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable { - const typename ParameterType::Type oper; + Operator oper; - OperatorToFalsePredicateAdapter(typename ParameterType::Type o) + OperatorToFalsePredicateAdapter(Operator o) : oper(o) { } From 7df9fb082076faaa206bfe66c9ee3b38829462ee Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 23:17:54 +0300 Subject: [PATCH 131/268] NodeInfoRetriever - using Multiset instead of Map<> --- .../uavcan/protocol/node_info_retriever.hpp | 44 ++++++++++++------- 1 file changed, 28 insertions(+), 16 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 29391a15a6..1d9d16322d 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -7,7 +7,7 @@ #include #include -#include +#include #include #include #include @@ -109,7 +109,7 @@ private: , node_info(arg_node_info) { } - bool operator()(INodeInfoListener* key, bool) + bool operator()(INodeInfoListener* key) { UAVCAN_ASSERT(key != NULL); key->handleNodeInfoRetrieved(node_id, node_info); @@ -128,7 +128,7 @@ private: , event(arg_event) { } - bool operator()(INodeInfoListener* key, bool) + bool operator()(INodeInfoListener* key) { UAVCAN_ASSERT(key != NULL); (key->*method)(event); @@ -141,7 +141,7 @@ private: */ Entry entries_[NodeID::Max]; // [1, NodeID::Max] - Map listeners_; // Only keys are used + Multiset listeners_; ServiceClient get_node_info_client_; @@ -253,7 +253,7 @@ private: } } - listeners_.removeAllWhere( + listeners_.forEach( GenericHandlerCaller(&INodeInfoListener::handleNodeStatusChange, event)); } @@ -271,7 +271,7 @@ private: entry.uptime_sec = msg.uptime_sec; entry.updated_since_last_attempt = true; - listeners_.removeAllWhere(GenericHandlerCaller&>( + listeners_.forEach(GenericHandlerCaller&>( &INodeInfoListener::handleNodeStatusMessage, msg)); } @@ -287,8 +287,8 @@ private: */ entry.uptime_sec = result.getResponse().status.uptime_sec; entry.request_needed = false; - listeners_.removeAllWhere(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, - result.getResponse())); + listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); } else { @@ -298,8 +298,8 @@ private: if (entry.num_attempts_made >= num_attempts_) { entry.request_needed = false; - listeners_.removeAllWhere(GenericHandlerCaller( - &INodeInfoListener::handleNodeInfoUnavailable, result.getCallID().server_node_id)); + listeners_.forEach(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.getCallID().server_node_id)); } } } @@ -340,14 +340,20 @@ public: } /** - * Adds one listener to the set. + * 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) { - UAVCAN_ASSERT(listener != NULL); - bool* value = listeners_.insert(listener, true); - return (value == NULL) ? -ErrMemory : 0; + if (listener != NULL) + { + removeListener(listener); + return (NULL == listeners_.emplace(listener)) ? -ErrMemory : 0; + } + else + { + return -ErrInvalidParam; + } } /** @@ -356,8 +362,14 @@ public: */ void removeListener(INodeInfoListener* listener) { - UAVCAN_ASSERT(listener != NULL); - listeners_.remove(listener); + if (listener != NULL) + { + listeners_.removeAll(listener); + } + else + { + UAVCAN_ASSERT(0); + } } /** From 5e5540b8ce3020b7ef826e994aeb70c4e08148e8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 16 May 2015 23:57:11 +0300 Subject: [PATCH 132/268] ServiceClient<>::hasPendingCallToServer() --- .../include/uavcan/node/service_client.hpp | 18 ++++++++++++++++++ libuavcan/test/node/service_client.cpp | 9 +++++++++ 2 files changed, 27 insertions(+) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 47035056a1..b5a84715fa 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -165,6 +165,13 @@ protected: 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) @@ -329,6 +336,11 @@ public: */ void cancelAllCalls(); + /** + * Checks whether there's currently a pending call addressed to the specified node ID. + */ + bool hasPendingCallToServer(NodeID server_node_id) const; + /** * Service response callback must be set prior service call. */ @@ -538,6 +550,12 @@ void ServiceClient::cancelAllCalls() SubscriberType::stop(); } +template +bool ServiceClient::hasPendingCallToServer(NodeID server_node_id) const +{ + return NULL != call_registry_.find(ServerSearchPredicate(server_node_id)); +} + } #endif // UAVCAN_NODE_SERVICE_CLIENT_HPP_INCLUDED diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 4022de062e..8b24848a30 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -109,6 +109,8 @@ TEST(ServiceClient, Basic) 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()); @@ -128,6 +130,11 @@ TEST(ServiceClient, Basic) 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! @@ -211,9 +218,11 @@ TEST(ServiceClient, Rejection) ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client1.hasPendingCallToServer(1)); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client1.hasPendingCallToServer(1)); ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out From 2123853cae81aaeb58745344266910093b6d943c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 00:01:49 +0300 Subject: [PATCH 133/268] Using concurrent calls in NodeInfoRetriever --- .../uavcan/protocol/node_info_retriever.hpp | 26 +++++++++++++++---- 1 file changed, 21 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 1d9d16322d..ceffa9bf28 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -69,7 +69,6 @@ class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor { public: enum { MaxNumRequestAttempts = 254 }; - enum { DefaultNumRequestAttempts = 30 }; enum { UnlimitedRequestAttempts = 0 }; private: @@ -136,6 +135,9 @@ private: } }; + enum { NumStaticCalls = 4 }; + enum { DefaultNumRequestAttempts = 30 }; + /* * State */ @@ -143,11 +145,12 @@ private: Multiset listeners_; - ServiceClient get_node_info_client_; + ServiceClient get_node_info_client_; mutable uint8_t last_picked_node_; uint8_t num_attempts_; + uint8_t max_concurrent_requests_; /* * Methods @@ -185,7 +188,9 @@ private: (last_picked_node_ <= NodeID::Max)); const Entry& entry = getEntry(last_picked_node_); - if (entry.request_needed && entry.updated_since_last_attempt) + if (entry.request_needed && + 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_); @@ -196,9 +201,9 @@ private: virtual void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.hasPendingCalls()) // If request is pending, this condition will fail every second time + if (get_node_info_client_.getNumPendingCalls() >= max_concurrent_requests_) { - return; // TODO FIXME Concurrent calls!! + return; } const NodeID next = pickNextNodeToQuery(); @@ -313,6 +318,7 @@ public: , get_node_info_client_(node) , last_picked_node_(1) , num_attempts_(DefaultNumRequestAttempts) + , max_concurrent_requests_(NumStaticCalls * 2) { } /** @@ -382,6 +388,16 @@ public: { num_attempts_ = min(static_cast(MaxNumRequestAttempts), num); } + + /** + * Number of concurrent requests limits the number of simultaneous service calls to different nodes. + * This value cannot be less than one. + */ + uint8_t getNumConcurrentRequests() const { return max_concurrent_requests_; } + void setNumConcurrentRequests(uint8_t num) + { + max_concurrent_requests_ = max(static_cast(1), num); + } }; } From 36dda9c0175f2eb4f9dd7ea772f01a2bf62baa67 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 13:49:40 +0300 Subject: [PATCH 134/268] NodeInfoRetriever basic test --- .../uavcan/protocol/node_info_retriever.hpp | 15 ++++ .../test/protocol/node_info_retriever.cpp | 70 +++++++++++++++++-- 2 files changed, 81 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index ceffa9bf28..4a10321e11 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -172,6 +172,8 @@ private: if (!TimerBase::isRunning()) { TimerBase::startPeriodic(getTimerPollInterval()); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %ld ms", + static_cast(getTimerPollInterval().toMSec())); } } @@ -230,6 +232,7 @@ private: if (!requests_needed) { TimerBase::stop(); + UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); } } } @@ -398,6 +401,18 @@ public: { max_concurrent_requests_ = max(static_cast(1), num); } + + /** + * 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(num); + } }; } diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 04f2d346ac..2b8f92d5d9 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -54,7 +54,8 @@ struct NodeInfoListener : public uavcan::INodeInfoListener virtual void handleNodeStatusChange(const uavcan::NodeStatusMonitor::NodeStatusChangeEvent& event) { - (void)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++; } @@ -102,10 +103,16 @@ TEST(NodeInfoRetriever, Basic) ASSERT_LE(0, provider->startAndPublish()); + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + /* * Waiting for discovery */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); + 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); @@ -120,6 +127,8 @@ TEST(NodeInfoRetriever, Basic) /* * Declaring a bunch of different nodes that don't support GetNodeInfo */ + ASSERT_FALSE(retr.isRetrievingInProgress()); + retr.setNumRequestAttempts(3); uavcan::TransferID tid; @@ -128,12 +137,65 @@ TEST(NodeInfoRetriever, Basic) 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(2100)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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); - // TODO finish the test when the logic is fixed + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + ASSERT_EQ(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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(110)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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); } From 600c29a9539787ee4de555dd1cbeeeae1854a93f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 16:29:19 +0300 Subject: [PATCH 135/268] NodeInfoRetriever - docs, logical fixes, tests --- .../uavcan/protocol/node_info_retriever.hpp | 73 +++++++++----- .../test/protocol/node_info_retriever.cpp | 94 ++++++++++++++++--- 2 files changed, 135 insertions(+), 32 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 4a10321e11..92a5e42471 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -61,8 +61,32 @@ public: /** * 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. - * Events from this class can be routed to many subscribers. + * 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] = foor(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 @@ -135,8 +159,9 @@ private: } }; - enum { NumStaticCalls = 4 }; - enum { DefaultNumRequestAttempts = 30 }; + enum { NumStaticCalls = 2 }; + enum { DefaultNumRequestAttempts = 16 }; + enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation /* * State @@ -147,16 +172,15 @@ private: ServiceClient get_node_info_client_; + MonotonicDuration request_interval_; + mutable uint8_t last_picked_node_; uint8_t num_attempts_; - uint8_t max_concurrent_requests_; /* * Methods */ - static MonotonicDuration getTimerPollInterval() { return MonotonicDuration::fromMSec(100); } - const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } Entry& getEntry(NodeID node_id) { @@ -171,9 +195,8 @@ private: { if (!TimerBase::isRunning()) { - TimerBase::startPeriodic(getTimerPollInterval()); - UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %ld ms", - static_cast(getTimerPollInterval().toMSec())); + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str()); } } @@ -203,11 +226,6 @@ private: virtual void handleTimerEvent(const TimerEvent&) { - if (get_node_info_client_.getNumPendingCalls() >= max_concurrent_requests_) - { - return; - } - const NodeID next = pickNextNodeToQuery(); if (next.isUnicast()) { @@ -319,9 +337,9 @@ public: , TimerBase(node) , listeners_(node.getAllocator()) , get_node_info_client_(node) + , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec)) , last_picked_node_(1) , num_attempts_(DefaultNumRequestAttempts) - , max_concurrent_requests_(NumStaticCalls * 2) { } /** @@ -381,6 +399,8 @@ public: } } + 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. @@ -393,13 +413,24 @@ public: } /** - * Number of concurrent requests limits the number of simultaneous service calls to different nodes. - * This value cannot be less than one. + * Request interval also implicitly defines the maximum number of concurrent requests. + * Read the class documentation for details. */ - uint8_t getNumConcurrentRequests() const { return max_concurrent_requests_; } - void setNumConcurrentRequests(uint8_t num) + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) { - max_concurrent_requests_ = max(static_cast(1), num); + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } } /** diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 2b8f92d5d9..71088ebbef 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -92,6 +92,9 @@ TEST(NodeInfoRetriever, Basic) 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; @@ -106,6 +109,8 @@ TEST(NodeInfoRetriever, Basic) ASSERT_FALSE(retr.isRetrievingInProgress()); ASSERT_EQ(0, retr.getNumPendingRequests()); + EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default + /* * Waiting for discovery */ @@ -137,11 +142,11 @@ TEST(NodeInfoRetriever, Basic) 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(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(2, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(3, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); ASSERT_TRUE(retr.isRetrievingInProgress()); @@ -151,11 +156,11 @@ TEST(NodeInfoRetriever, Basic) 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(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(2, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(3, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); ASSERT_TRUE(retr.isRetrievingInProgress()); @@ -165,11 +170,11 @@ TEST(NodeInfoRetriever, Basic) 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(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(2, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); ASSERT_EQ(3, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); ASSERT_TRUE(retr.isRetrievingInProgress()); @@ -181,9 +186,9 @@ TEST(NodeInfoRetriever, Basic) tid.increment(); publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); ASSERT_EQ(1, retr.getNumPendingRequests()); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(110)); + 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()); @@ -199,3 +204,70 @@ TEST(NodeInfoRetriever, Basic) 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 _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + + InterlinkedTestNodesWithSysClock nodes; + + uavcan::NodeInfoRetriever retr(nodes.a); + std::cout << "sizeof(uavcan::NodeInfoRetriever): " << sizeof(uavcan::NodeInfoRetriever) << std::endl; + std::cout << "sizeof(uavcan::ServiceClient): " + << sizeof(uavcan::ServiceClient) << 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 = 13; // 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()); +} From c089f4d72bfb8639cf367bd2e77e1a3cb831f2cd Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 16:34:04 +0300 Subject: [PATCH 136/268] Node info retriever - timer event optimization --- .../uavcan/protocol/node_info_retriever.hpp | 36 ++++++++++--------- 1 file changed, 19 insertions(+), 17 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 92a5e42471..af81caa4b4 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -200,8 +200,10 @@ private: } } - NodeID pickNextNodeToQuery() const + 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_++; @@ -213,22 +215,31 @@ private: (last_picked_node_ <= NodeID::Max)); const Entry& entry = getEntry(last_picked_node_); - if (entry.request_needed && - entry.updated_since_last_attempt && - !get_node_info_client_.hasPendingCallToServer(last_picked_node_)) + + if (entry.request_needed) { - UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_)); - return NodeID(last_picked_node_); + 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&) { - const NodeID next = pickNextNodeToQuery(); + 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) @@ -238,16 +249,7 @@ private: } else { - bool requests_needed = false; - for (uint8_t i = 1; i <= NodeID::Max; i++) - { - if (getEntry(i).request_needed) - { - requests_needed = true; - break; - } - } - if (!requests_needed) + if (!at_least_one_request_needed) { TimerBase::stop(); UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); From e5fddfdb666bf9b93f9601e314926451869a6f48 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 17:05:58 +0300 Subject: [PATCH 137/268] Node info retriever unit test fix --- .../test/protocol/node_info_retriever.cpp | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp index 71088ebbef..88fc4aace8 100644 --- a/libuavcan/test/protocol/node_info_retriever.cpp +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -142,10 +142,10 @@ TEST(NodeInfoRetriever, Basic) 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(45)); - ASSERT_EQ(1, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); - ASSERT_EQ(2, retr.getNumPendingRequests()); + 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)); @@ -156,10 +156,10 @@ TEST(NodeInfoRetriever, Basic) 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(45)); - ASSERT_EQ(1, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); - ASSERT_EQ(2, retr.getNumPendingRequests()); + 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)); @@ -170,10 +170,10 @@ TEST(NodeInfoRetriever, Basic) 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(45)); - ASSERT_EQ(1, retr.getNumPendingRequests()); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); - ASSERT_EQ(2, retr.getNumPendingRequests()); + 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)); @@ -186,7 +186,7 @@ TEST(NodeInfoRetriever, Basic) tid.increment(); publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(45)); + 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 @@ -235,7 +235,7 @@ TEST(NodeInfoRetriever, MaxConcurrentRequests) ASSERT_EQ(40, retr.getRequestInterval().toMSec()); - const unsigned MaxPendingRequests = 13; // See class docs + const unsigned MaxPendingRequests = 14; // See class docs const unsigned MinPendingRequestsAtFullLoad = 12; /* From cd41840f5919f2763c85f229aebe8faab7b44a87 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 17 May 2015 17:18:14 +0300 Subject: [PATCH 138/268] Multi-call client in RaftCore --- .../distributed/raft_core.hpp | 36 +++++++------------ 1 file changed, 12 insertions(+), 24 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 89f84c2260..b77dfadf9e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -138,9 +138,8 @@ class RaftCore : private TimerBase ServiceClient append_entries_client_; ServiceServer request_vote_srv_; - enum { NumRequestVoteClients = ClusterManager::MaxClusterSize - 1 }; - LazyConstructor > - request_vote_clients_[NumRequestVoteClients]; + enum { NumRequestVoteCalls = ClusterManager::MaxClusterSize - 1 }; + ServiceClient request_vote_client_; /* * Methods @@ -218,7 +217,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 < NumRequestVoteCalls; i++) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); if (!node_id.isUnicast()) @@ -230,7 +229,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); @@ -325,10 +324,7 @@ 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]->cancelAllCalls(); // TODO FIXME Concurrent calls!! - } + request_vote_client_.cancelAllCalls(); append_entries_client_.cancelAllCalls(); /* @@ -729,12 +725,8 @@ public: , 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(node); - } - } + , request_vote_client_(node) + { } /** * Once started, the logic runs in the background until destructor is called. @@ -790,17 +782,13 @@ public: &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)); + request_vote_client_.setRequestTimeout(update_interval_); startPeriodic(update_interval_); From 58ca7319dd12a85ebf6a72d2dabd0b2eba8d5365 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 14:00:26 +0300 Subject: [PATCH 139/268] File server implementation with test --- dsdl/uavcan/protocol/file/Error.uavcan | 19 +- .../include/uavcan/protocol/file_server.hpp | 180 ++++++++++++++++++ libuavcan/test/protocol/file_server.cpp | 118 ++++++++++++ 3 files changed, 308 insertions(+), 9 deletions(-) create mode 100644 libuavcan/include/uavcan/protocol/file_server.hpp create mode 100644 libuavcan/test/protocol/file_server.cpp diff --git a/dsdl/uavcan/protocol/file/Error.uavcan b/dsdl/uavcan/protocol/file/Error.uavcan index 448c0ce361..e44e2da39c 100644 --- a/dsdl/uavcan/protocol/file/Error.uavcan +++ b/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 diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp new file mode 100644 index 0000000000..2339eeb6b0 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -0,0 +1,180 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +// UAVCAN types +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * The file server backend should implement this interface. + */ +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(protocol::file::Path::SEPARATOR); } + + /** + * Backend 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) = 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. + * 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 + GetInfoCallback; + + typedef MethodBinder + ReadCallback; + + ServiceServer get_info_srv_; + ServiceServer 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 (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; + } +}; + +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp new file mode 100644 index 0000000000..641ad2f9d2 --- /dev/null +++ b/libuavcan/test/protocol/file_server.cpp @@ -0,0 +1,118 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#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(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 _reg1; + uavcan::DefaultDataTypeRegistrator _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 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(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); + } +} From 51a2ce39c5b5a225dade7ce434278ba98c64765d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 14:21:58 +0300 Subject: [PATCH 140/268] Full file server implementation --- .../include/uavcan/protocol/file_server.hpp | 84 +++++++++++++++++++ libuavcan/test/protocol/file_server.cpp | 24 ++++++ 2 files changed, 108 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index 2339eeb6b0..d525b70156 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -175,6 +175,90 @@ public: } }; +/** + * 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 + WriteCallback; + + typedef MethodBinder + DeleteCallback; + + typedef MethodBinder + GetDirectoryEntryInfoCallback; + + ServiceServer write_srv_; + ServiceServer delete_srv_; + ServiceServer 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 diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp index 641ad2f9d2..f3591e79e3 100644 --- a/libuavcan/test/protocol/file_server.cpp +++ b/libuavcan/test/protocol/file_server.cpp @@ -116,3 +116,27 @@ TEST(BasicFileServer, Basic) ASSERT_EQ(0, read.collector.result->getResponse().error.value); } } + + +TEST(FileServer, Basic) +{ + using namespace uavcan::protocol::file; + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _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 +} From e4886606f0966d6bec65d9ef7f93bd2b37cb6cb4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 14:31:10 +0300 Subject: [PATCH 141/268] Typo --- libuavcan/include/uavcan/protocol/node_info_retriever.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index af81caa4b4..5b27a9b68f 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -71,7 +71,7 @@ public: * 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] = foor(5000 [ms] bootloader timeout / 123 nodes) + * 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 From 46afa99b27a1e9b6ce0753d480408c1cd9d5e5ff Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 15:09:52 +0300 Subject: [PATCH 142/268] Refactored POSIX tools --- CMakeLists.txt | 1 + .../{posix_tools => posix}/include.mk | 0 .../file_event_tracer.hpp | 43 +++++++++++-------- .../file_storage_backend.hpp | 40 +++++++---------- 4 files changed, 41 insertions(+), 43 deletions(-) rename libuavcan_drivers/{posix_tools => posix}/include.mk (100%) rename libuavcan_drivers/{posix_tools/include/posix_tools => posix/include/uavcan_posix/dynamic_node_id_server}/file_event_tracer.hpp (65%) rename libuavcan_drivers/{posix_tools/include/posix_tools => posix/include/uavcan_posix/dynamic_node_id_server}/file_storage_backend.hpp (88%) diff --git a/CMakeLists.txt b/CMakeLists.txt index b889336f0d..6636e188d8 100644 --- a/CMakeLists.txt +++ b/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 () diff --git a/libuavcan_drivers/posix_tools/include.mk b/libuavcan_drivers/posix/include.mk similarity index 100% rename from libuavcan_drivers/posix_tools/include.mk rename to libuavcan_drivers/posix/include.mk diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp similarity index 65% rename from libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp rename to libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 719e34694d..60fe4448fb 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -1,11 +1,17 @@ -/* - * Copyright (C) 2015 Pavel Kirienko - */ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ -#pragma once +#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 #include +#include namespace uavcan_posix { @@ -19,9 +25,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer /** * Maximum length of full path to log file */ - - enum { MaxPathLength = 128, FormatBufferLength = 64 }; - + enum { MaxPathLength = 128 }; /** * This type is used for the path @@ -29,32 +33,32 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer typedef uavcan::Array, uavcan::ArrayModeDynamic, MaxPathLength> PathString; - PathString path_; - -public: - - FileEventTracer() { } - virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) { + using namespace std; + struct timespec ts; clock_gettime(CLOCK_REALTIME, &ts); + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND); if (fd >= 0 ) { + const int FormatBufferLength = 64; char buffer[FormatBufferLength + 1]; - int n = snprintf(buffer, FormatBufferLength, "%d.%ld,%d,%lld\n", ts.tv_sec, ts.tv_nsec, code, argument); + 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 - * - */ +public: + FileEventTracer() { } + + /** + * Initializes the file based event tracer. + */ int init(const PathString & path) { using namespace std; @@ -73,8 +77,9 @@ public: } return rv; } - }; } } + +#endif // Include guard diff --git a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp similarity index 88% rename from libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp rename to libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index e5233420cf..139004dccd 100644 --- a/libuavcan_drivers/posix_tools/include/posix_tools/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -6,15 +6,16 @@ * ****************************************************************************/ -#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 -#include +#include #include #include -#include #include #include +#include #include @@ -30,24 +31,16 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken /** * Maximum length of full path including / and key max */ - enum { MaxPathLength = 128 }; - /** * This type is used for the path */ typedef uavcan::Array, uavcan::ArrayModeDynamic, MaxPathLength> PathString; - PathString base_path; -public: - - FileStorageBackend() { } - - virtual String get(const String& key) const { using namespace std; @@ -75,7 +68,6 @@ public: } } return value; - } virtual void set(const String& key, const String& value) @@ -91,16 +83,17 @@ public: } } - /** - * Initializes the File based back end storage by passing to a path to - * the directory where the key named files will be stored. - * This the return result 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; - * - */ +public: + FileStorageBackend() { } - int init(const PathString & path) + /** + * Initializes the file based backend storage by passing a path to + * the directory where the key named files will be stored. + * 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. + */ + int init(const PathString& path) { using namespace std; @@ -108,7 +101,6 @@ public: if (path.size() > 0) { - base_path = path.c_str(); if (base_path.back() == '/') @@ -132,10 +124,10 @@ public: } } return rv; - } - }; } } + +#endif // Include guard From 09a96061adb47ed1e6e81856ec4cf42e8e6a920e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 17:02:17 +0300 Subject: [PATCH 143/268] Fixed and improved file event tracer + POSIX test --- libuavcan_drivers/linux/CMakeLists.txt | 5 ++ libuavcan_drivers/linux/apps/test_posix.cpp | 50 +++++++++++++++++++ libuavcan_drivers/posix/CMakeLists.txt | 12 +++++ .../file_event_tracer.hpp | 22 +++++--- .../file_storage_backend.hpp | 1 + 5 files changed, 82 insertions(+), 8 deletions(-) create mode 100644 libuavcan_drivers/linux/apps/test_posix.cpp create mode 100644 libuavcan_drivers/posix/CMakeLists.txt diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index a88c7da13a..fd8ed5eef1 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/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,9 @@ 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}) + # # Tools # diff --git a/libuavcan_drivers/linux/apps/test_posix.cpp b/libuavcan_drivers/linux/apps/test_posix.cpp new file mode 100644 index 0000000000..be432e1f36 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_posix.cpp @@ -0,0 +1,50 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#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(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(tracer).onEvent(TraceError, 789123); + ENFORCE(0 == std::system(("cat " + event_log_file).c_str())); + } + + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Exception: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/posix/CMakeLists.txt b/libuavcan_drivers/posix/CMakeLists.txt new file mode 100644 index 0000000000..49fbbfb8e1 --- /dev/null +++ b/libuavcan_drivers/posix/CMakeLists.txt @@ -0,0 +1,12 @@ +# +# Copyright (C) 2015 Pavel Kirienko +# + +cmake_minimum_required(VERSION 2.8) + +project(libuavcan_posix) + +# +# Library (header only) +# +install(DIRECTORY include/uavcan_posix DESTINATION include) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 60fe4448fb..3746592534 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -10,8 +10,10 @@ #define UAVCAN_POSIX_DYNAMIC_NODE_ID_SERVER_FILE_EVENT_TRACER_HPP_INCLUDED #include +#include #include #include +#include namespace uavcan_posix { @@ -27,6 +29,8 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer */ enum { MaxPathLength = 128 }; + enum { FilePermissions = 438 }; ///< 0o666 + /** * This type is used for the path */ @@ -39,15 +43,17 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer { using namespace std; - struct timespec ts; - clock_gettime(CLOCK_REALTIME, &ts); + 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); - if (fd >= 0 ) + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions); + if (fd >= 0) { - const int FormatBufferLength = 64; + const int FormatBufferLength = 63; char buffer[FormatBufferLength + 1]; - int n = snprintf(buffer, FormatBufferLength, "%d.%ld,%d,%lld\n", ts.tv_sec, ts.tv_nsec, code, argument); + int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", + static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), + static_cast(code), static_cast(argument)); write(fd, buffer, n); close(fd); } @@ -69,8 +75,8 @@ public: { rv = 0; path_ = path.c_str(); - int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC); - if ( fd >= 0) + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); + if (fd >= 0) { close(fd); } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index 139004dccd..20346e588c 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -15,6 +15,7 @@ #include #include #include +#include #include #include From 3c073ac9d421dff92480d2a26e2d106d9d5557a3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 17:08:49 +0300 Subject: [PATCH 144/268] Simple test for POSIX storage backend --- libuavcan_drivers/linux/apps/test_posix.cpp | 23 +++++++++++++++++++ .../file_storage_backend.hpp | 4 +++- 2 files changed, 26 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/test_posix.cpp b/libuavcan_drivers/linux/apps/test_posix.cpp index be432e1f36..a59aa1c3ef 100644 --- a/libuavcan_drivers/linux/apps/test_posix.cpp +++ b/libuavcan_drivers/linux/apps/test_posix.cpp @@ -40,6 +40,29 @@ int main(int argc, const char** argv) 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(backend).get(key).c_str() << std::endl; + }; + + print_key("foobar"); + + static_cast(backend).set("foobar", "0123456789abcdef0123456789abcdef"); + static_cast(backend).set("the_answer", "42"); + + print_key("foobar"); + print_key("the_answer"); + print_key("nonexistent"); + } + return 0; } catch (const std::exception& ex) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index 20346e588c..e0ccf7cd3d 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -34,6 +34,8 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken */ enum { MaxPathLength = 128 }; + enum { FilePermissions = 438 }; ///< 0o666 + /** * This type is used for the path */ @@ -76,7 +78,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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()); From 875c74d88e0c3fcf68e173498dafd422bc614031 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 17:11:05 +0300 Subject: [PATCH 145/268] Removed useless constructors --- .../uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp | 2 -- .../dynamic_node_id_server/file_storage_backend.hpp | 2 -- 2 files changed, 4 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 3746592534..c8e97244dd 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -60,8 +60,6 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer } public: - FileEventTracer() { } - /** * Initializes the file based event tracer. */ diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index e0ccf7cd3d..e4ce8c8e23 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -87,8 +87,6 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken } public: - FileStorageBackend() { } - /** * Initializes the file based backend storage by passing a path to * the directory where the key named files will be stored. From 5e458e918d7606ffa89dc79ee357391799b501a9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 18 May 2015 22:29:09 +0300 Subject: [PATCH 146/268] MakeString<> helper template --- libuavcan/include/uavcan/marshal/array.hpp | 13 +++++++++++++ .../dynamic_node_id_server/storage_backend.hpp | 2 +- .../dynamic_node_id_server/file_event_tracer.hpp | 3 +-- .../dynamic_node_id_server/file_storage_backend.hpp | 3 +-- 4 files changed, 16 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index 73277f4c1b..c7263d106e 100644 --- a/libuavcan/include/uavcan/marshal/array.hpp +++ b/libuavcan/include/uavcan/marshal/array.hpp @@ -1087,6 +1087,19 @@ inline bool operator!=(const R& rhs, const Array& 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 +class MakeString +{ + MakeString(); // This class is not instantiatable. +public: + typedef Array, ArrayModeDynamic, MaxSize> Type; +}; + /** * YAML streamer specification for any Array<> */ diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp index 9e970dd671..9e2395c92a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/storage_backend.hpp @@ -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, ArrayModeDynamic, MaxStringLength> String; + typedef MakeString::Type String; /** * Read one value from the storage. diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index c8e97244dd..3398a54418 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -34,8 +34,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer /** * This type is used for the path */ - typedef uavcan::Array, - uavcan::ArrayModeDynamic, MaxPathLength> PathString; + typedef uavcan::MakeString::Type PathString; PathString path_; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index e4ce8c8e23..fdf173cc13 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -39,8 +39,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken /** * This type is used for the path */ - typedef uavcan::Array, - uavcan::ArrayModeDynamic, MaxPathLength> PathString; + typedef uavcan::MakeString::Type PathString; PathString base_path; From 6b179d032bb146a6d5925ec2254cf9b082cb412f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 01:37:10 +0300 Subject: [PATCH 147/268] Firmware update trigger implementation. It is most likely broken, because I'm half asleep by now; proper tests will be added later --- .../protocol/firmware_update_trigger.hpp | 430 ++++++++++++++++++ .../test/protocol/firmware_update_trigger.cpp | 83 ++++ 2 files changed, 513 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp create mode 100644 libuavcan/test/protocol/firmware_update_trigger.cpp diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp new file mode 100644 index 0000000000..9f08371c98 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -0,0 +1,430 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +// UAVCAN types +#include + +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::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 shouldSendFirmwareUpdateRequest(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 a request simply + * times out, nothing will be sent. + * + * 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. + * + * @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 shouldRetryFirmwareUpdateRequest(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. + * + * @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) = 0; + + 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&)> + BeginFirmwareUpdateResponseCallback; + + typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath; + + enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout. + + struct NodeIDSelectorPredicate + { + const FirmwareUpdateTrigger& owner; + + NodeIDSelectorPredicate(const FirmwareUpdateTrigger& arg_owner) + : owner(arg_owner) + { } + + bool operator()(const NodeID node_id, const FirmwareFilePath&) + { + return + (node_id > owner.last_queried_node_id_) && + !owner.begin_fw_update_client_.hasPendingCallToServer(node_id); + } + }; + + /* + * State + */ + ServiceClient begin_fw_update_client_; + + IFirmwareVersionChecker& checker_; + + NodeInfoRetriever* node_info_retriever_; + + Map pending_nodes_; + + MonotonicDuration request_interval_; + + FirmwareFilePath common_path_prefix_; + + mutable uint8_t last_queried_node_id_; + + /* + * Methods of INodeInfoListener + */ + virtual void handleNodeInfoUnavailable(NodeID) { /* Not used */ } + + virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info) + { + FirmwareFilePath firmware_file_path; + const bool update_needed = checker_.shouldSendFirmwareUpdateRequest(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 + { + 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) + { + FirmwareFilePath* const value = pending_nodes_.access(node_id); + if (value != NULL) + { + *value = 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<> + const NodeID* found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); + if (found == NULL) + { + // Resetting the round-robin selector and trying again + last_queried_node_id_ = 0; + found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); + if (found == NULL) + { + return NodeID(); + } + } + + UAVCAN_ASSERT(found != NULL); + UAVCAN_ASSERT(found->get() >= last_queried_node_id_); + + last_queried_node_id_ = found->get(); + UAVCAN_ASSERT(NodeID(last_queried_node_id_).isUnicast()); + + UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d", int(last_queried_node_id_)); + return last_queried_node_id_; + } + + void handleBeginFirmwareUpdateResponse(const ServiceCallResult& result) + { + if (!result.isSuccessful()) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry", + int(result.getCallID().server_node_id.get())); + 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) + { + pending_nodes_.remove(result.getCallID().server_node_id); + checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse()); + } + else + { + FirmwareFilePath firmware_file_path; + const bool update_needed = + checker_.shouldRetryFirmwareUpdateRequest(result.getCallID().server_node_id, result.getResponse(), + firmware_file_path); + if (update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires retry; file path: %s", + int(result.getCallID().server_node_id.get()), firmware_file_path.c_str()); + trySetPendingNode(result.getCallID().server_node_id, firmware_file_path); + } + else + { + 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); + 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 class. 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 isTriggeringInProgress() const { return TimerBase::isRunning(); } +}; + +} + +#endif // Include guard diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp new file mode 100644 index 0000000000..566a25fc07 --- /dev/null +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -0,0 +1,83 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +using namespace uavcan::protocol::file; + +struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + virtual bool shouldSendFirmwareUpdateRequest(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + (void)node_id; + (void)node_info; + (void)out_firmware_file_path; + return false; + } + + virtual bool shouldRetryFirmwareUpdateRequest(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + (void)node_id; + (void)error_response; + (void)out_firmware_file_path; + return false; + } + + virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } +}; + +TEST(FirmwareUpdateTrigger, Basic) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _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 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()); + + /* + * Discovering one node + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); +} From 694d29ab47d0d943e69fee8aa3ca84e223a49c95 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 02:10:22 +0300 Subject: [PATCH 148/268] Misleading comment that somehow survived refactoring --- libuavcan/include/uavcan/protocol/node_info_retriever.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp index 5b27a9b68f..017102fa40 100644 --- a/libuavcan/include/uavcan/protocol/node_info_retriever.hpp +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -119,9 +119,6 @@ private: } }; - /* - * Callers are used with removeWhere() predicate. They don't actually remove anything. - */ struct NodeInfoRetrievedHandlerCaller { const NodeID node_id; From d7ae3f90c0a460bf07d5a1019abae30fe5a0167b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 02:13:50 +0300 Subject: [PATCH 149/268] Doc fix --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9f08371c98..6ab44f329a 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -54,8 +54,8 @@ public: FirmwareFilePath& out_firmware_file_path) = 0; /** - * This method will be invoked when a node responds to the update request with an error. If a request simply - * times out, nothing will be sent. + * 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. * * 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. From 0d60595d7c9025bd3db714b79421427002252a99 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 13:16:51 +0300 Subject: [PATCH 150/268] FirmwareUpdateTrigger - fixes and test --- .../protocol/firmware_update_trigger.hpp | 39 +++-- .../test/protocol/firmware_update_trigger.cpp | 145 ++++++++++++++++-- 2 files changed, 152 insertions(+), 32 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 6ab44f329a..9aaa65c97a 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -50,8 +50,8 @@ public: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ - virtual bool shouldSendFirmwareUpdateRequest(NodeID node_id, const protocol::GetNodeInfo::Response& node_info, - FirmwareFilePath& out_firmware_file_path) = 0; + 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 @@ -69,9 +69,9 @@ public: * @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 shouldRetryFirmwareUpdateRequest(NodeID node_id, - const protocol::file::BeginFirmwareUpdate::Response& error_response, - FirmwareFilePath& out_firmware_file_path) = 0; + 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. @@ -170,12 +170,16 @@ class FirmwareUpdateTrigger : public INodeInfoListener, /* * Methods of INodeInfoListener */ - virtual void handleNodeInfoUnavailable(NodeID) { /* Not used */ } + 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_.shouldSendFirmwareUpdateRequest(node_id, node_info, 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", @@ -205,10 +209,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path) { - FirmwareFilePath* const value = pending_nodes_.access(node_id); - if (value != NULL) + if (NULL != pending_nodes_.insert(node_id, path)) { - *value = path; if (!TimerBase::isRunning()) { TimerBase::startPeriodic(request_interval_); @@ -268,8 +270,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, { FirmwareFilePath firmware_file_path; const bool update_needed = - checker_.shouldRetryFirmwareUpdateRequest(result.getCallID().server_node_id, result.getResponse(), - firmware_file_path); + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), + firmware_file_path); if (update_needed) { UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires retry; file path: %s", @@ -301,7 +303,7 @@ class FirmwareUpdateTrigger : public INodeInfoListener, FirmwareFilePath* const path = pending_nodes_.access(node_id); if (path == NULL) { - UAVCAN_ASSERT(0); + UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map return; } @@ -345,7 +347,7 @@ public: } /** - * Starts the class. Once started, it can't be stopped unless destroyed. + * 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. @@ -422,7 +424,14 @@ public: * This method is mostly needed for testing. * When triggering is not in progress, the class consumes zero CPU time. */ - bool isTriggeringInProgress() const { return TimerBase::isRunning(); } + bool isTimerRunning() const { return TimerBase::isRunning(); } + + unsigned getNumPendingNodes() const + { + const unsigned ret = pending_nodes_.getSize(); + UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); + return ret; + } }; } diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index 566a25fc07..9b1910a701 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -11,34 +11,66 @@ using namespace uavcan::protocol::file; struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { - virtual bool shouldSendFirmwareUpdateRequest(uavcan::NodeID node_id, - const uavcan::protocol::GetNodeInfo::Response& node_info, - FirmwareFilePath& out_firmware_file_path) + unsigned should_request_cnt; + unsigned should_retry_cnt; + unsigned confirmation_cnt; + + std::string firmware_path; + + bool should_retry; + std::string expected_node_name_to_update; + + BeginFirmwareUpdate::Response last_error_response; + + FirmwareVersionChecker() + : should_request_cnt(0) + , should_retry_cnt(0) + , confirmation_cnt(0) + , should_retry(false) + { } + + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) { - (void)node_id; - (void)node_info; - (void)out_firmware_file_path; - return false; + 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 shouldRetryFirmwareUpdateRequest(uavcan::NodeID node_id, - const BeginFirmwareUpdate::Response& error_response, - FirmwareFilePath& out_firmware_file_path) + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) { - (void)node_id; - (void)error_response; - (void)out_firmware_file_path; - return false; + last_error_response = error_response; + std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; + should_retry_cnt++; + out_firmware_file_path = firmware_path.c_str(); + return should_retry; } virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, const BeginFirmwareUpdate::Response& response) { - (void)node_id; - (void)response; + confirmation_cnt++; + std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl; } }; +static uint8_t response_error_code = 0; + +static void beginFirmwareUpdateRequestCallback( + const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& res) +{ + std::cout << "REQUEST\n" << req << std::endl; + + res.error = response_error_code; + res.optional_error_message = "foobar"; +} + + TEST(FirmwareUpdateTrigger, Basic) { uavcan::GlobalDataTypeRegistry::instance().reset(); @@ -76,8 +108,87 @@ TEST(FirmwareUpdateTrigger, Basic) ASSERT_LE(0, provider->startAndPublish()); + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + /* - * Discovering one node + * 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.should_retry = true; + 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 srv(nodes.b); + + ASSERT_LE(0, srv.start(beginFirmwareUpdateRequestCallback)); + + response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.should_retry = true; + + 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 + */ + response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.should_retry = false; + + 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()); } From 1047a2587292e1f7c81dc973b38ad167c5de650d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 19 May 2015 13:22:52 +0300 Subject: [PATCH 151/268] FirmwareUpdateTrigger - fixes and test --- libuavcan/test/protocol/firmware_update_trigger.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index 9b1910a701..d37679c0c5 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -191,4 +191,10 @@ TEST(FirmwareUpdateTrigger, Basic) // 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); } From 228785b8f8921be705ed7d855bb7584602169f6d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 00:12:06 +0300 Subject: [PATCH 152/268] libuavcan testing: TestNetwork<> helper --- libuavcan/test/node/test_node.hpp | 78 +++++++++++++++++-- libuavcan/test/node/test_node_test.cpp | 32 ++++++++ .../test/protocol/global_time_sync_master.cpp | 6 +- .../test/protocol/global_time_sync_slave.cpp | 2 +- 4 files changed, 106 insertions(+), 12 deletions(-) create mode 100644 libuavcan/test/node/test_node_test.cpp diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 418159aa33..d36fcc8e2e 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -6,6 +6,8 @@ #include #include +#include +#include #include "../transport/can/can.hpp" @@ -43,21 +45,21 @@ struct TestNode : public uavcan::INode struct PairableCanDriver : public uavcan::ICanDriver, public uavcan::ICanIface { uavcan::ISystemClock& clock; - PairableCanDriver* other; + std::set others; std::queue read_queue; std::queue 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 +75,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 +92,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::iterator it = others.begin(); it != others.end(); ++it) + { + (*it)->read_queue.push(frame); + } if (flags & uavcan::CanIOFlagLoopback) { loopback_queue.push(frame); @@ -103,7 +107,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()) { @@ -186,3 +189,62 @@ struct InterlinkedTestNodes typedef InterlinkedTestNodes InterlinkedTestNodesWithSysClock; typedef InterlinkedTestNodes InterlinkedTestNodesWithClockMock; + + +template +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 nodes[NumNodes]; + + TestNetwork(uavcan::uint8_t first_node_id = 1) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + nodes[i].reset(new NodeEnvironment(first_node_id + i)); + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + for (uavcan::uint8_t k = 0; k < NumNodes; k++) + { + nodes[i]->linkTogether(nodes[k].get()); + } + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + assert(nodes[i]->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]->spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + } + return 0; + } +}; diff --git a/libuavcan/test/node/test_node_test.cpp b/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 0000000000..c5731640eb --- /dev/null +++ b/libuavcan/test/node/test_node_test.cpp @@ -0,0 +1,32 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "test_node.hpp" + + +TEST(TestNode, TestNetwork) +{ + TestNetwork<4> nwk; + + uavcan::CanFrame frame; + for (int 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); + } +} diff --git a/libuavcan/test/protocol/global_time_sync_master.cpp b/libuavcan/test/protocol/global_time_sync_master.cpp index 404d649027..bed4d908dc 100644 --- a/libuavcan/test/protocol/global_time_sync_master.cpp +++ b/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)) diff --git a/libuavcan/test/protocol/global_time_sync_slave.cpp b/libuavcan/test/protocol/global_time_sync_slave.cpp index 5bb2e91c0d..25c4e50975 100644 --- a/libuavcan/test/protocol/global_time_sync_slave.cpp +++ b/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 gts_pub2(master2_node); From 4398cceb4cc230ebf9bae9c161fd17636d93b81f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 00:41:44 +0300 Subject: [PATCH 153/268] Fixing the previous commit --- libuavcan/test/node/test_node.hpp | 20 +++++++++++++++++--- libuavcan/test/node/test_node_test.cpp | 2 +- 2 files changed, 18 insertions(+), 4 deletions(-) diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index d36fcc8e2e..4329fc7c21 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -4,6 +4,11 @@ #pragma once +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + #include #include #include @@ -212,20 +217,20 @@ struct TestNetwork { for (uavcan::uint8_t i = 0; i < NumNodes; i++) { - nodes[i].reset(new NodeEnvironment(first_node_id + 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]->linkTogether(nodes[k].get()); + nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver); } } for (uavcan::uint8_t i = 0; i < NumNodes; i++) { - assert(nodes[i]->others.size() == (NumNodes - 1)); + assert(nodes[i]->can_driver.others.size() == (NumNodes - 1)); } } @@ -247,4 +252,13 @@ struct TestNetwork } return 0; } + + TestNode& operator[](unsigned index) + { + if (index >= NumNodes) + { + throw std::out_of_range("No such test node"); + } + return nodes[index]->node; + } }; diff --git a/libuavcan/test/node/test_node_test.cpp b/libuavcan/test/node/test_node_test.cpp index c5731640eb..b40357be21 100644 --- a/libuavcan/test/node/test_node_test.cpp +++ b/libuavcan/test/node/test_node_test.cpp @@ -11,7 +11,7 @@ TEST(TestNode, TestNetwork) TestNetwork<4> nwk; uavcan::CanFrame frame; - for (int i = 0; i < 8; i++) + for (uint8_t i = 0; i < 8; i++) { frame.data[i] = i; } From 19cffa682f5632574f487d497c307be43734abe8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 01:20:42 +0300 Subject: [PATCH 154/268] TestNetwork<> fix --- libuavcan/test/node/test_node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 4329fc7c21..543f99a50d 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -243,7 +243,7 @@ struct TestNetwork { for (uavcan::uint8_t i = 0; i < NumNodes; i++) { - int ret = nodes[i]->spin(uavcan::MonotonicDuration::fromMSec(1)); + int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1)); if (ret < 0) { return ret; From ee25e3db45c5c133fe6725ef9781e9fe126ee805 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 May 2015 17:47:37 -1000 Subject: [PATCH 155/268] POSIX File Server Backend --- .../uavcan_posix/file_server_backend.hpp | 180 ++++++++++++++++++ 1 file changed, 180 insertions(+) create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp new file mode 100644 index 0000000000..54bf531ac4 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp @@ -0,0 +1,180 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class FileSeverBackend : public uavcan::IFileServerBackend +{ + /** + * Maximum length of full path including / the file name + */ + enum { MaxPathLength = Path::MaxSize + 128 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString base_path; + +public: + /** + * + * Backend 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) + { + + enum { MaxBufferLength = 256 }; + + int rv = -uavcan::ErrInvalidParam; + if (path.size()) { + + rv = -uavcan::ErrFailure; + + PathString root_path = base_path.c_str(); + root_path += path; + + uint32_t size = 0; + int fd = open(path.c_str(), O_RDONLY); + if (fd >= 0) + { + uavcan::DataTypeSignatureCRC crc; + ssize_t len; + + uint8_t bytes[MaxBufferLength]; + + do { + + len = ::read(fd, bytes, MaxBufferLength); + + if (len < 0) { + return rv; + } + + if (len > 0) { + size += len; + crc.add(bytes, len); + } + + } while(len); + + out_crc64 = crc.get(); + out_size = size; + EntryType t; + t.flags = uavcan::protocol::file::EntryType::FLAG_FILE; + out_type = t; + rv = 0; + } + } + return rv; + } + + /** + * 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) + { + + int rv = -uavcan::ErrInvalidParam; + + if (path.size()) { + + rv = -uavcan::ErrFailure; + + PathString root_path = base_path.c_str(); + root_path += path; + + int fd = open(path.c_str(), O_RDONLY); + + if (fd >= 0) + { + if (::lseek(fd, offset, SEEK_SET) >= 0) { + + ssize_t len = ::read(fd, out_buffer, inout_size); + + if (len < 0) { + return rv; + } + inout_size = len; + rv = 0; + } + } + } + return rv; + } + + /** + * Initializes the file serrver based backend storage by passing a path to + * the directory where files will be stored. + */ + int init(const PathString& path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) + { + base_path = path.c_str(); + + if (base_path.back() == '/') + { + base_path.pop_back(); + } + + rv = 0; + struct stat sb; + if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) + { + rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + if (rv >= 0 ) + { + base_path.push_back('/'); + if ((base_path.size() + Path::MaxSize) > MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + return rv; + } +}; + +} + +#endif // Include guard From 5a0bccf7872fd2f7916e2b46990adb7fd34da8dd Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 May 2015 18:28:19 -1000 Subject: [PATCH 156/268] Update file_server_backend.hpp --- .../posix/include/uavcan_posix/file_server_backend.hpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp index 54bf531ac4..963982bb23 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp @@ -87,7 +87,7 @@ public: } } while(len); - + close(fd); out_crc64 = crc.get(); out_size = size; EntryType t; @@ -125,6 +125,7 @@ public: if (::lseek(fd, offset, SEEK_SET) >= 0) { ssize_t len = ::read(fd, out_buffer, inout_size); + close(fd); if (len < 0) { return rv; From b8d3884eb68f63ea4a4e939b3a7ca70f9d33f546 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 19 May 2015 18:54:15 -1000 Subject: [PATCH 157/268] Ensured close in all paths. This still needed the EAGAIN logic added --- .../posix/include/uavcan_posix/file_server_backend.hpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp index 963982bb23..d6280e6bc3 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp @@ -78,7 +78,7 @@ public: len = ::read(fd, bytes, MaxBufferLength); if (len < 0) { - return rv; + goto out_close; } if (len > 0) { @@ -87,7 +87,6 @@ public: } } while(len); - close(fd); out_crc64 = crc.get(); out_size = size; EntryType t; @@ -95,6 +94,9 @@ public: out_type = t; rv = 0; } + +out_close: + close(fd); } return rv; } From 1f47596688b38c8847736cc2a902f9bd69d67e6c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 14:53:16 +0300 Subject: [PATCH 158/268] FirmwareUpdateTrigger node selector fix, more testing --- .../protocol/firmware_update_trigger.hpp | 62 +++++-- .../test/protocol/firmware_update_trigger.cpp | 172 ++++++++++++++++-- 2 files changed, 195 insertions(+), 39 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9aaa65c97a..d95a4b9656 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -134,19 +134,26 @@ class FirmwareUpdateTrigger : public INodeInfoListener, enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout. - struct NodeIDSelectorPredicate + struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable { - const FirmwareUpdateTrigger& owner; + enum { DefaultOutput = 0xFFU }; - NodeIDSelectorPredicate(const FirmwareUpdateTrigger& arg_owner) + const FirmwareUpdateTrigger& owner; + uint8_t output; + + NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner) : owner(arg_owner) + , output(DefaultOutput) { } bool operator()(const NodeID node_id, const FirmwareFilePath&) { - return - (node_id > owner.last_queried_node_id_) && - !owner.begin_fw_update_client_.hasPendingCallToServer(node_id); + 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; } }; @@ -188,6 +195,7 @@ class FirmwareUpdateTrigger : public INodeInfoListener, } else { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get())); pending_nodes_.remove(node_id); } } @@ -226,25 +234,35 @@ class FirmwareUpdateTrigger : public INodeInfoListener, NodeID pickNextNodeID() const { // We can't do index search because indices are unstable in Map<> - const NodeID* found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); - if (found == NULL) + // First try - from the current node up + NextNodeIDSearchPredicate s1(*this); + (void)pending_nodes_.find(s1); + + if (s1.output != NextNodeIDSearchPredicate::DefaultOutput) { - // Resetting the round-robin selector and trying again + 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; - found = pending_nodes_.find(NodeIDSelectorPredicate(*this)); - if (found == NULL) + + NextNodeIDSearchPredicate s2(*this); + (void)pending_nodes_.find(s2); + + if (s2.output != NextNodeIDSearchPredicate::DefaultOutput) { - return NodeID(); + last_queried_node_id_ = s2.output; } } - - UAVCAN_ASSERT(found != NULL); - UAVCAN_ASSERT(found->get() >= last_queried_node_id_); - - last_queried_node_id_ = found->get(); - UAVCAN_ASSERT(NodeID(last_queried_node_id_).isUnicast()); - - UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d", int(last_queried_node_id_)); + 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_; } @@ -263,6 +281,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, if (confirmed) { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %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()); } @@ -280,6 +300,8 @@ class FirmwareUpdateTrigger : public INodeInfoListener, } else { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need retry", + int(result.getCallID().server_node_id.get())); pending_nodes_.remove(result.getCallID().server_node_id); } } diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index d37679c0c5..c0ae718efe 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -17,7 +17,7 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker std::string firmware_path; - bool should_retry; + int retry_quota; std::string expected_node_name_to_update; BeginFirmwareUpdate::Response last_error_response; @@ -26,7 +26,7 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker : should_request_cnt(0) , should_retry_cnt(0) , confirmation_cnt(0) - , should_retry(false) + , retry_quota(0) { } virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, @@ -47,7 +47,15 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; should_retry_cnt++; out_firmware_file_path = firmware_path.c_str(); - return should_retry; + if (retry_quota > 0) + { + retry_quota--; + return true; + } + else + { + return false; + } } virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, @@ -58,17 +66,27 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker } }; -static uint8_t response_error_code = 0; - -static void beginFirmwareUpdateRequestCallback( - const uavcan::ReceivedDataStructure& req, - uavcan::ServiceResponseDataStructure& res) +struct BeginFirmwareUpdateServer { - std::cout << "REQUEST\n" << req << std::endl; + uint8_t response_error_code; - res.error = response_error_code; - res.optional_error_message = "foobar"; -} + BeginFirmwareUpdateServer() : response_error_code(0) { } + + void handleRequest(const uavcan::ReceivedDataStructure& req, + uavcan::ServiceResponseDataStructure& res) const + { + std::cout << "REQUEST\n" << req << std::endl; + res.error = response_error_code; + res.optional_error_message = "foobar"; + } + + typedef uavcan::MethodBinder&, + uavcan::ServiceResponseDataStructure&) const> Callback; + + Callback makeCallback() { return Callback(this, &BeginFirmwareUpdateServer::handleRequest); } +}; TEST(FirmwareUpdateTrigger, Basic) @@ -117,7 +135,7 @@ TEST(FirmwareUpdateTrigger, Basic) */ checker.firmware_path = "firmware_path"; checker.expected_node_name_to_update = "Ivan"; - checker.should_retry = true; + checker.retry_quota = 1000; nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); @@ -138,12 +156,13 @@ TEST(FirmwareUpdateTrigger, Basic) * Starting the firmware update server that returns an error * The checker will instruct the trigger to repeat */ - uavcan::ServiceServer srv(nodes.b); + uavcan::ServiceServer srv(nodes.b); + BeginFirmwareUpdateServer srv_impl; - ASSERT_LE(0, srv.start(beginFirmwareUpdateRequestCallback)); + ASSERT_LE(0, srv.start(srv_impl.makeCallback())); - response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; - checker.should_retry = true; + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.retry_quota = 1000; nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); @@ -158,8 +177,8 @@ TEST(FirmwareUpdateTrigger, Basic) /* * Trying again, this time with ERROR_IN_PROGRESS */ - response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; - checker.should_retry = false; + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.retry_quota = 0; nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); @@ -198,3 +217,118 @@ TEST(FirmwareUpdateTrigger, Basic) 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 _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _reg3; + uavcan::DefaultDataTypeRegistrator _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 provider_a(new uavcan::NodeStatusProvider(nodes[1])); + std::auto_ptr provider_b(new uavcan::NodeStatusProvider(nodes[2])); + std::auto_ptr provider_c(new uavcan::NodeStatusProvider(nodes[3])); + std::auto_ptr 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 srv_a(nodes[1]); + uavcan::ServiceServer srv_b(nodes[2]); + uavcan::ServiceServer srv_c(nodes[3]); + uavcan::ServiceServer 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(4100)); // 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()); +} From bb412f3f93817c3b256382d32b4483338a0841cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 15:13:31 +0300 Subject: [PATCH 159/268] FirmwareUpdateTrigger retry logic optimization --- .../protocol/firmware_update_trigger.hpp | 34 ++++++++++++------- .../test/protocol/firmware_update_trigger.cpp | 4 ++- 2 files changed, 24 insertions(+), 14 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index d95a4b9656..7380be1d9d 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -56,6 +56,7 @@ public: /** * 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. @@ -64,7 +65,9 @@ public: * * @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. + * @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. @@ -75,6 +78,7 @@ public: /** * 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. * * @param node_id Node ID that confirmed the request. * @@ -275,32 +279,36 @@ class FirmwareUpdateTrigger : public INodeInfoListener, 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 ID %d confirmed the update request", + 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 { - FirmwareFilePath firmware_file_path; + 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(), - firmware_file_path); - if (update_needed) + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path); + + if (!update_needed) { - UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires retry; file path: %s", - int(result.getCallID().server_node_id.get()), firmware_file_path.c_str()); - trySetPendingNode(result.getCallID().server_node_id, firmware_file_path); - } - else - { - UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need retry", + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry", int(result.getCallID().server_node_id.get())); pending_nodes_.remove(result.getCallID().server_node_id); } diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index c0ae718efe..2e6f286160 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -46,7 +46,9 @@ struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker last_error_response = error_response; std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; should_retry_cnt++; - out_firmware_file_path = firmware_path.c_str(); + + EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str()); + if (retry_quota > 0) { retry_quota--; From 6abe343f04837676d6927a5701849a2f911cd885 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 15:19:51 +0300 Subject: [PATCH 160/268] FirmwareUpdateTrigger handleFirmwareUpdateConfirmation() made optional --- .../include/uavcan/protocol/firmware_update_trigger.hpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 7380be1d9d..9bea80eaca 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -80,12 +80,18 @@ public: * 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) = 0; + const protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } virtual ~IFirmwareVersionChecker() { } }; From 9f17dca012f01f2783989c99c2ecc49c697f6671 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 02:57:20 -1000 Subject: [PATCH 161/268] Fixed guard --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9bea80eaca..9c46090598 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_FILE_SERVER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED #include #include From 8b87990c1b4104479b51c7d082a9005ffaf18bb6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 02:58:33 -1000 Subject: [PATCH 162/268] Fixed guard --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 9c46090598..7003aef075 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -2,8 +2,8 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED -#define UAVCAN_PROTOCOL_UPDATE_TRIGGER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_FIRMWAREUPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED #include #include From 5358c734efc7ff607db67aa65254eeb9c698dc90 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 02:59:36 -1000 Subject: [PATCH 163/268] Fixed guard --- libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index 7003aef075..f32e1598c7 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -2,7 +2,7 @@ * Copyright (C) 2015 Pavel Kirienko */ -#ifndef UAVCAN_PROTOCOL_FIRMWAREUPDATE_TRIGGER_HPP_INCLUDED +#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED #define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED #include From 489a27f70d41edcc29e60d4dc3e7cbd9452c72a0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 22:13:32 +0300 Subject: [PATCH 164/268] BasicFileServer::handleRead() error handling fix --- libuavcan/include/uavcan/protocol/file_server.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index d525b70156..ef6008766c 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -136,6 +136,11 @@ class BasicFileServer 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); From 45942eef1fb7685491a9051c47f57bdadbb9297e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 20 May 2015 22:15:19 +0300 Subject: [PATCH 165/268] Note on error codes --- libuavcan/include/uavcan/protocol/file_server.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index ef6008766c..ba458cefad 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -20,6 +20,8 @@ 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 { From 823b14c1211b3cef27a1ec3ee3eb256437db2f9c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 May 2015 01:28:41 +0300 Subject: [PATCH 166/268] POSIX dynamic ID storage backend: calling fsync() on set(), plus a minor style fix --- .../dynamic_node_id_server/file_storage_backend.hpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index fdf173cc13..a2834c6cfb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -53,9 +53,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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++) @@ -80,8 +80,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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()); + (void)fsync(fd); + (void)close(fd); } } From fa11a76143159f9934f69b877e0a5e17ee6a9990 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 20 May 2015 18:17:16 -1000 Subject: [PATCH 167/268] Refactoring paths --- .../protocol/firmware_update_trigger.hpp | 1 + .../basic_file_server_backend.hpp | 147 ++++++++++ .../uavcan_posix/file_server_backend.hpp | 183 ------------ .../include/uavcan_posix/firmware_common.hpp | 217 +++++++++++++++ .../uavcan_posix/firmware_version_checker.hpp | 262 ++++++++++++++++++ 5 files changed, 627 insertions(+), 183 deletions(-) create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp delete mode 100644 libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp diff --git a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp index f32e1598c7..788536c07c 100644 --- a/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -468,6 +468,7 @@ public: UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); return ret; } + }; } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp new file mode 100644 index 0000000000..c28e0cd381 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -0,0 +1,147 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED +#define UAVCAN_POSIX_BASIC_FILE_SERVER_BACKEND_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include "firmware_common.hpp" + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +class BasicFileSeverBackend : public uavcan::IFileServerBackend +{ + + enum { FilePermissions = 438 }; ///< 0o666 + + FirmwareCommon::BasePathString base_path; + +public: + /** + * + * Back-end for uavcan.protocol.file.GetInfo. + * Implementation of this method is required. + * On success the method must return zero. + */ + __attribute__((optimize("O0"))) + virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) + { + + enum { MaxBufferLength = 256 }; + + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( + base_path.c_str())); + + if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + { + + fw_full_path += path; + FirmwareCommon fw; + fw.getFileInfo(fw_full_path); + out_crc64 = fw.descriptor.image_crc; + out_size = fw.descriptor.image_size; + // todo Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + out_type.flags = uavcan::protocol::file::EntryType::FLAG_FILE | + uavcan::protocol::file::EntryType::FLAG_READABLE;; + rv = 0; + } + 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. + */ + __attribute__((optimize("O0"))) + 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; + + FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( + base_path.c_str())); + + if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + { + + fw_full_path += path; + + int fd = open(fw_full_path.c_str(), O_RDONLY); + + if (fd < 0) + { + rv = errno; + } + else + { + + if (::lseek(fd, offset, SEEK_SET) < 0) + { + rv = errno; + } + else + { + //todo uses 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; + } + } + close(fd); + } + } + return rv; + } + + /** + * Initializes the file server based back-end storage by passing a path to + * the directory where files will be stored. + */ + __attribute__((optimize("O0"))) + int init(const FirmwareCommon::BasePathString& path) + { + using namespace std; + base_path = path; + int rv = FirmwareCommon::create_fw_paths(base_path); + return rv; + } +}; + +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp deleted file mode 100644 index d6280e6bc3..0000000000 --- a/libuavcan_drivers/posix/include/uavcan_posix/file_server_backend.hpp +++ /dev/null @@ -1,183 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 PX4 Development Team. All rights reserved. -* Author: Pavel Kirienko -* David Sidrane -* -****************************************************************************/ - -#ifndef UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED -#define UAVCAN_POSIX_FILE_SERVER_BACKEND_HPP_INCLUDED - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include - -namespace uavcan_posix -{ -/** - * This interface implements a POSIX compliant IFileServerBackend interface - */ -class FileSeverBackend : public uavcan::IFileServerBackend -{ - /** - * Maximum length of full path including / the file name - */ - enum { MaxPathLength = Path::MaxSize + 128 }; - - enum { FilePermissions = 438 }; ///< 0o666 - - /** - * This type is used for the path - */ - typedef uavcan::MakeString::Type PathString; - - PathString base_path; - -public: - /** - * - * Backend 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) - { - - enum { MaxBufferLength = 256 }; - - int rv = -uavcan::ErrInvalidParam; - if (path.size()) { - - rv = -uavcan::ErrFailure; - - PathString root_path = base_path.c_str(); - root_path += path; - - uint32_t size = 0; - int fd = open(path.c_str(), O_RDONLY); - if (fd >= 0) - { - uavcan::DataTypeSignatureCRC crc; - ssize_t len; - - uint8_t bytes[MaxBufferLength]; - - do { - - len = ::read(fd, bytes, MaxBufferLength); - - if (len < 0) { - goto out_close; - } - - if (len > 0) { - size += len; - crc.add(bytes, len); - } - - } while(len); - out_crc64 = crc.get(); - out_size = size; - EntryType t; - t.flags = uavcan::protocol::file::EntryType::FLAG_FILE; - out_type = t; - rv = 0; - } - -out_close: - close(fd); - } - return rv; - } - - /** - * 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) - { - - int rv = -uavcan::ErrInvalidParam; - - if (path.size()) { - - rv = -uavcan::ErrFailure; - - PathString root_path = base_path.c_str(); - root_path += path; - - int fd = open(path.c_str(), O_RDONLY); - - if (fd >= 0) - { - if (::lseek(fd, offset, SEEK_SET) >= 0) { - - ssize_t len = ::read(fd, out_buffer, inout_size); - close(fd); - - if (len < 0) { - return rv; - } - inout_size = len; - rv = 0; - } - } - } - return rv; - } - - /** - * Initializes the file serrver based backend storage by passing a path to - * the directory where files will be stored. - */ - int init(const PathString& path) - { - using namespace std; - - int rv = -uavcan::ErrInvalidParam; - - if (path.size() > 0) - { - base_path = path.c_str(); - - if (base_path.back() == '/') - { - base_path.pop_back(); - } - - rv = 0; - struct stat sb; - if (stat(base_path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) - { - rv = mkdir(base_path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); - } - if (rv >= 0 ) - { - base_path.push_back('/'); - if ((base_path.size() + Path::MaxSize) > MaxPathLength) - { - rv = -uavcan::ErrInvalidConfiguration; - } - } - } - return rv; - } -}; - -} - -#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp new file mode 100644 index 0000000000..6973b45f6c --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -0,0 +1,217 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include + +#include + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareCommon +{ + /* The folder where the files will be copied and Read from */ + + static const char* get_cache_dir_() + { + return "c"; + } + +public: + + enum { MaxBasePathLength = 128 }; + + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::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::Type PathString; + + static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } + + typedef struct app_descriptor_t + { + uint8_t signature[sizeof(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]; + } app_descriptor_t; + + app_descriptor_t descriptor; + + /** + * This method Is used to get the app_descriptor_t from a firmware image + * + * @param path Complete path to the file + * + * @param add_separator Complete path with trailing separator + * + * + * @return 0 if the app_descriptor_t was found and out_descriptor + * has been updated. + * Otherwise -errno is returned + * -ENOENT Signature was not found. + */ + + static PathString getFirmwareCachePath(const PathString& p, bool add_separator = true) + { + PathString r; + r = p; + r.push_back(getPathSeparator()); + r += get_cache_dir_(); + if (add_separator) { + r.push_back(getPathSeparator()); + } + return r; + } + + + static BasePathString getFirmwareCachePath(const BasePathString& p, bool add_separator = true) + { + BasePathString r; + r = p; + r.push_back(getPathSeparator()); + r += get_cache_dir_(); + if (add_separator) { + r.push_back(getPathSeparator()); + } + return r; + } + + int getFileInfo(PathString& path) + { + enum { MaxChunk = (512 / sizeof(uint64_t)) }; + int rv = -ENOENT; + uint64_t chunk[MaxChunk]; + int fd = open(path.c_str(), O_RDONLY); + + if (fd >= 0) + { + app_descriptor_t *pdescriptor = 0; + + while(!pdescriptor) + { + 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 == sig.ull) + { + pdescriptor = (app_descriptor_t *)p; + descriptor = *pdescriptor; + rv = 0; + break; + } + } + while(p++ <= &chunk[MaxChunk - (sizeof(app_descriptor_t) / sizeof(chunk[0]))]); + } + out_close: + close(fd); + } + return rv; + } + + + /** + * Creates the Directories were the files will be stored + */ + + static int create_fw_paths(FirmwareCommon::BasePathString& path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) + { + if (path.back() == FirmwareCommon::getPathSeparator()) + { + path.pop_back(); + } + + rv = 0; + struct stat sb; + if (stat(path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) + { + rv = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + + PathString cache = getFirmwareCachePath(PathString(path.c_str()), false); + if (rv >= 0 && (stat(cache.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))) + { + rv = mkdir(cache.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); + } + + if (rv >= 0 ) + { + path.push_back(FirmwareCommon::getPathSeparator()); + if ((path.size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > + FirmwareCommon::MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + return rv; + } + + +private: + +#define APP_DESCRIPTOR_SIGNATURE_ID 'A', 'P', 'D', 'e', 's', 'c' +#define APP_DESCRIPTOR_SIGNATURE_REV '0', '0' +#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV + + union + { + uint64_t ull; + char text[sizeof(uint64_t)]; + } sig = { + .text = {APP_DESCRIPTOR_SIGNATURE} + }; + +}; +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp new file mode 100644 index 0000000000..69c71c3a39 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -0,0 +1,262 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#include +#include "firmware_common.hpp" + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + enum { FilePermissions = 438 }; ///< 0o666 + + FirmwareCommon::BasePathString base_path; + FirmwareCommon::BasePathString cache_path; + +public: + + /** + * 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. + */ + __attribute__((optimize("O0"))) + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const + uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + /* This is a work around for two issues. + * 1) FirmwareFilePath is 40 + * 2) Nuttx 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[FirmwareCommon::MaxBasePathLength + 1]; + int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", + base_path.c_str(), + node_info.name.c_str(), + node_info.hardware_version.major, + node_info.hardware_version.minor); + + if (n > 0) + { + FAR DIR *fwdir = opendir(fname_root); + + if (fwdir) + { + FAR struct dirent *pfile; + while((pfile = readdir(fwdir)) != NULL) + { + if (DIRENT_ISFILE(pfile->d_type)) + { + // Open any bin file in there. + if (strstr(pfile->d_name, ".bin")) + { + FirmwareCommon::PathString full_src_path = fname_root; + full_src_path.push_back(FirmwareCommon::getPathSeparator()); + full_src_path += pfile->d_name; + + FirmwareCommon::PathString full_dst_path = cache_path.c_str(); + full_dst_path += pfile->d_name; + + /* ease the burden on the user */ + int cr = copy_if_not(full_src_path.c_str(), full_dst_path.c_str()); + + /* We have a file, is it a valid image */ + + FirmwareCommon fw; + + if (cr == 0 && fw.getFileInfo(full_dst_path) == 0) + { + if ((node_info.software_version.major == 0 && + node_info.software_version.minor == 0) || + fw.descriptor.image_crc != + node_info.software_version.image_crc) + { + rv = true; + out_firmware_file_path = pfile->d_name; + } + break; + } + } + } + } + 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. + */ + __attribute__((optimize("O0"))) + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::file::BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + return true; + + } + + /** + * 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(uavcan::NodeID node_id, + const uavcan::protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } + + FirmwareVersionChecker() { } + + virtual ~FirmwareVersionChecker() { } + + /** + * Initializes the file server based back-end storage by passing a path to + * the directory where files will be stored. + */ + __attribute__((optimize("O0"))) + int init(const char * path) + { + base_path = path; + cache_path = FirmwareCommon::getFirmwareCachePath(base_path); + if (base_path.back() != FirmwareCommon::getPathSeparator()) + { + base_path.push_back(FirmwareCommon::getPathSeparator()); + } + return 0; + } + +private: + + + __attribute__((optimize("O0"))) + int copy_if_not(const char *srcpath, const char * destpath) + { + /* Does the file exits */ + int rv = 0; + int dfd = open(destpath, O_RDONLY, 0); + + if (dfd >= 0) + { + /* Close it and exit 0 */ + 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); + close(sfd); + } + close(dfd); + } + } + return rv; + } +}; +} + +#endif // Include guard From e5ce6f74c693b1722c97490c7817693d2a1c9e30 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 21 May 2015 17:16:20 +0300 Subject: [PATCH 168/268] POSIX file event tracer visibility fix --- .../uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 3398a54418..863d7465a1 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -38,6 +38,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer PathString path_; +protected: virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) { using namespace std; From 64b14db1f59b8ac151dacfc153e96f9831ba5607 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 21 May 2015 07:25:53 -1000 Subject: [PATCH 169/268] More string refactoring --- .../include/uavcan/protocol/file_server.hpp | 7 + .../basic_file_server_backend.hpp | 37 +--- .../file_event_tracer.hpp | 1 + .../file_storage_backend.hpp | 9 +- .../include/uavcan_posix/firmware_common.hpp | 117 +----------- .../include/uavcan_posix/firmware_path.hpp | 171 ++++++++++++++++++ .../uavcan_posix/firmware_version_checker.hpp | 47 ++--- 7 files changed, 222 insertions(+), 167 deletions(-) create mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index d525b70156..ba458cefad 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -20,6 +20,8 @@ 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 { @@ -136,6 +138,11 @@ class BasicFileServer 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); diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index c28e0cd381..c88bf8b731 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -36,8 +36,6 @@ class BasicFileSeverBackend : public uavcan::IFileServerBackend enum { FilePermissions = 438 }; ///< 0o666 - FirmwareCommon::BasePathString base_path; - public: /** * @@ -49,19 +47,13 @@ public: virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) { - enum { MaxBufferLength = 256 }; - int rv = uavcan::protocol::file::Error::INVALID_VALUE; - FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( - base_path.c_str())); - - if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + if (path.size() > 0) { - fw_full_path += path; FirmwareCommon fw; - fw.getFileInfo(fw_full_path); + fw.getFileInfo(path.c_str()); out_crc64 = fw.descriptor.image_crc; out_size = fw.descriptor.image_size; // todo Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. @@ -85,15 +77,11 @@ public: int rv = uavcan::protocol::file::Error::INVALID_VALUE; - FirmwareCommon::PathString fw_full_path = FirmwareCommon::getFirmwareCachePath(FirmwareCommon::PathString( - base_path.c_str())); - - if (path.size() > 0 && (fw_full_path.size() + path.size()) <= FirmwareCommon::PathString::MaxSize) + if (path.size() > 0) { - fw_full_path += path; - int fd = open(fw_full_path.c_str(), O_RDONLY); + int fd = open(path.c_str(), O_RDONLY); if (fd < 0) { @@ -128,18 +116,11 @@ public: return rv; } - /** - * Initializes the file server based back-end storage by passing a path to - * the directory where files will be stored. - */ - __attribute__((optimize("O0"))) - int init(const FirmwareCommon::BasePathString& path) - { - using namespace std; - base_path = path; - int rv = FirmwareCommon::create_fw_paths(base_path); - return rv; - } + BasicFileSeverBackend() { } + + + + }; } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 3398a54418..863d7465a1 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -38,6 +38,7 @@ class FileEventTracer : public uavcan::dynamic_node_id_server::IEventTracer PathString path_; +protected: virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) { using namespace std; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index fdf173cc13..a2834c6cfb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -53,9 +53,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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++) @@ -80,8 +80,9 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken 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()); + (void)fsync(fd); + (void)close(fd); } } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp index 6973b45f6c..33ebba88b8 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -17,43 +17,17 @@ #include #include +#include "firmware_path.hpp" namespace uavcan_posix { /** - * Firmware version checking logic. - * Refer to @ref FirmwareUpdateTrigger for details. + * Firmware file validation logic. */ class FirmwareCommon { - /* The folder where the files will be copied and Read from */ - - static const char* get_cache_dir_() - { - return "c"; - } - public: - enum { MaxBasePathLength = 128 }; - - /** - * This type is used for the base path - */ - typedef uavcan::MakeString::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::Type PathString; - - static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } - typedef struct app_descriptor_t { uint8_t signature[sizeof(uint64_t)]; @@ -67,51 +41,12 @@ public: app_descriptor_t descriptor; - /** - * This method Is used to get the app_descriptor_t from a firmware image - * - * @param path Complete path to the file - * - * @param add_separator Complete path with trailing separator - * - * - * @return 0 if the app_descriptor_t was found and out_descriptor - * has been updated. - * Otherwise -errno is returned - * -ENOENT Signature was not found. - */ - - static PathString getFirmwareCachePath(const PathString& p, bool add_separator = true) - { - PathString r; - r = p; - r.push_back(getPathSeparator()); - r += get_cache_dir_(); - if (add_separator) { - r.push_back(getPathSeparator()); - } - return r; - } - - - static BasePathString getFirmwareCachePath(const BasePathString& p, bool add_separator = true) - { - BasePathString r; - r = p; - r.push_back(getPathSeparator()); - r += get_cache_dir_(); - if (add_separator) { - r.push_back(getPathSeparator()); - } - return r; - } - - int getFileInfo(PathString& path) + int getFileInfo(const char *path) { enum { MaxChunk = (512 / sizeof(uint64_t)) }; int rv = -ENOENT; uint64_t chunk[MaxChunk]; - int fd = open(path.c_str(), O_RDONLY); + int fd = open(path, O_RDONLY); if (fd >= 0) { @@ -153,50 +88,6 @@ public: } - /** - * Creates the Directories were the files will be stored - */ - - static int create_fw_paths(FirmwareCommon::BasePathString& path) - { - using namespace std; - - int rv = -uavcan::ErrInvalidParam; - - if (path.size() > 0) - { - if (path.back() == FirmwareCommon::getPathSeparator()) - { - path.pop_back(); - } - - rv = 0; - struct stat sb; - if (stat(path.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode)) - { - rv = mkdir(path.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); - } - - PathString cache = getFirmwareCachePath(PathString(path.c_str()), false); - if (rv >= 0 && (stat(cache.c_str(), &sb) != 0 || !S_ISDIR(sb.st_mode))) - { - rv = mkdir(cache.c_str(), S_IRWXU | S_IRWXG | S_IRWXO); - } - - if (rv >= 0 ) - { - path.push_back(FirmwareCommon::getPathSeparator()); - if ((path.size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > - FirmwareCommon::MaxPathLength) - { - rv = -uavcan::ErrInvalidConfiguration; - } - } - } - return rv; - } - - private: #define APP_DESCRIPTOR_SIGNATURE_ID 'A', 'P', 'D', 'e', 's', 'c' diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp new file mode 100644 index 0000000000..31bb0baab4 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp @@ -0,0 +1,171 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#ifndef UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED +#define UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED + +#include + +#include + +namespace uavcan_posix +{ +/** + * Firmware Path Management. + */ +class FirmwarePath +{ + /* The folder where the files will be copied and Read from */ + + static const char* get_cache_dir_() + { + return "c"; + } + +public: + + enum { MaxBasePathLength = 128 }; + + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::Type BasePathString; + + /** + * Maximum length of full path including / the file name + */ + enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; + + BasePathString base_path_; + BasePathString cache_path_; + + /** + * This type is used internally for the full path to file + */ + typedef uavcan::MakeString::Type PathString; + + static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } + + + BasePathString& getFirmwareBasePath() + { + return base_path_; + } + + void setFirmwareBasePath(const char * path) + { + base_path_ = path; + } + + BasePathString& getFirmwareCachePath() + { + return cache_path_; + } + + void setFirmwareCachePath(const char *path) + { + cache_path_ = path; + + } + + + 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(); + } + + } + + /** + * 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 cashed here. + */ + + int CreateFwPaths(const char *base_path) + { + using namespace std; + int rv = -uavcan::ErrInvalidParam; + + if (base_path) + { + + int len = strlen(base_path); + + if (len > 0 && len < base_path_.MaxSize) + { + + setFirmwareBasePath(base_path); + removeSlash(getFirmwareBasePath()); + const char *path = getFirmwareBasePath().c_str(); + + setFirmwareCachePath(path); + addSlash(cache_path_); + cache_path_ += get_cache_dir_(); + + 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(getFirmwareBasePath()); + addSlash(getFirmwareCachePath()); + + + if (rv >= 0 ) + { + if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > + MaxPathLength) + { + rv = -uavcan::ErrInvalidConfiguration; + } + } + } + } + return rv; + } + + int init(const char *path) + { + return CreateFwPaths(path); + } + +}; + +} + +#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 69c71c3a39..3066c07a18 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -18,6 +18,7 @@ #include #include + #include "firmware_common.hpp" namespace uavcan_posix @@ -30,8 +31,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { enum { FilePermissions = 438 }; ///< 0o666 - FirmwareCommon::BasePathString base_path; - FirmwareCommon::BasePathString cache_path; + FirmwarePath *paths_; public: @@ -56,7 +56,7 @@ public: { /* This is a work around for two issues. * 1) FirmwareFilePath is 40 - * 2) Nuttx is using 32 for max file names. + * 2) OK using is using 32 for max file names. * * So for the file: * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin @@ -72,17 +72,21 @@ public: bool rv = false; - char fname_root[FirmwareCommon::MaxBasePathLength + 1]; + char fname_root[FirmwarePath::MaxBasePathLength + 1]; int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", - base_path.c_str(), + paths_->getFirmwareBasePath().c_str(), node_info.name.c_str(), node_info.hardware_version.major, node_info.hardware_version.minor); - if (n > 0) + if (n > 0 && n < (int) sizeof(fname_root) - 2) { + FAR DIR *fwdir = opendir(fname_root); + fname_root[n++] = FirmwarePath::getPathSeparator(); + fname_root[n++] = '\0'; + if (fwdir) { FAR struct dirent *pfile; @@ -93,11 +97,10 @@ public: // Open any bin file in there. if (strstr(pfile->d_name, ".bin")) { - FirmwareCommon::PathString full_src_path = fname_root; - full_src_path.push_back(FirmwareCommon::getPathSeparator()); + FirmwarePath::PathString full_src_path = fname_root; full_src_path += pfile->d_name; - FirmwareCommon::PathString full_dst_path = cache_path.c_str(); + FirmwarePath::PathString full_dst_path = paths_->getFirmwareCachePath().c_str(); full_dst_path += pfile->d_name; /* ease the burden on the user */ @@ -107,9 +110,10 @@ public: FirmwareCommon fw; - if (cr == 0 && fw.getFileInfo(full_dst_path) == 0) + if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) { - if ((node_info.software_version.major == 0 && + if (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || fw.descriptor.image_crc != node_info.software_version.image_crc) @@ -173,26 +177,25 @@ public: (void)response; } - FirmwareVersionChecker() { } + FirmwareVersionChecker() : + paths_(0) + { } virtual ~FirmwareVersionChecker() { } /** - * Initializes the file server based back-end storage by passing a path to - * the directory where files will be stored. + * Initializes the Firmware File back-end storage by passing a paths object + * that maintains the directory where files will be stored. */ - __attribute__((optimize("O0"))) - int init(const char * path) + + int init(FirmwarePath& paths) { - base_path = path; - cache_path = FirmwareCommon::getFirmwareCachePath(base_path); - if (base_path.back() != FirmwareCommon::getPathSeparator()) - { - base_path.push_back(FirmwareCommon::getPathSeparator()); - } + paths_ = &paths; return 0; } + const char * getFirmwarePath() { return paths_->getFirmwareCachePath().c_str(); } + private: From 5f4adbf1a3a40632d5d471e9acccd058220de0a5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 00:38:17 +0300 Subject: [PATCH 170/268] dynamic_node_id_server::distributed::StateReport structure --- .../distributed/raft_core.hpp | 25 +++++--- .../distributed/server.hpp | 62 +++++++++++++++++++ .../node_discoverer.hpp | 6 ++ 3 files changed, 86 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index b77dfadf9e..ab26635bbe 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -74,6 +74,15 @@ public: */ class RaftCore : private TimerBase { +public: + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + +private: typedef MethodBinder&, ServiceResponseDataStructure&)> AppendEntriesCallback; @@ -88,13 +97,6 @@ class RaftCore : private TimerBase typedef MethodBinder&)> RequestVoteResponseCallback; - enum ServerState - { - ServerStateFollower, - ServerStateCandidate, - ServerStateLeader - }; - struct PendingAppendEntriesFields { Log::Index prev_log_index; @@ -895,6 +897,15 @@ 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_; } + bool isInActiveMode() const { return active_mode_; } + ServerState getServerState() const { return server_state_; } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 2fd6e2158a..940529e358 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -295,6 +295,68 @@ 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; + bool is_active; + + Log::Index last_log_index; + Log::Index commit_index; + + Term last_log_term; + Term current_term; + + NodeID voted_for; + + MonotonicTime last_activity_timestamp; + + 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()) + , is_active (s.getRaftCore().isInActiveMode()) + , 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()) + , 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; + } + } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 03b4145759..d8042074a2 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -361,6 +361,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(node_map_.getSize()); } }; } From 66dc702a7e2c66a6edf6634ad4ff70542e0c8f0f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 03:26:52 +0300 Subject: [PATCH 171/268] New Linux tool: uavcan_dynamic_node_id_server (mostly complete) --- libuavcan_drivers/linux/CMakeLists.txt | 4 + .../apps/uavcan_dynamic_node_id_server.cpp | 546 ++++++++++++++++++ 2 files changed, 550 insertions(+) create mode 100644 libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index fd8ed5eef1..408fff617c 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -69,7 +69,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) \ No newline at end of file diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp new file mode 100644 index 0000000000..5883fb2a82 --- /dev/null +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -0,0 +1,546 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include "debug.hpp" +// UAVCAN +#include +// UAVCAN Linux drivers +#include +// UAVCAN POSIX drivers +#include +#include + +namespace +{ + +uavcan_linux::NodePtr initNode(const std::vector& 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 machine_id = uavcan_linux::MachineIDReader().read(); + + uavcan::protocol::HardwareVersion hwver; + std::copy(machine_id.begin(), machine_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())); + } + + return node; +} + + +class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer +{ +public: + struct RecentEvent + { + const uavcan::MonotonicDuration time_since_startup; + const uavcan::dynamic_node_id_server::TraceCode code; + const std::int64_t argument; + + RecentEvent(uavcan::MonotonicDuration arg_time_since_startup, + uavcan::dynamic_node_id_server::TraceCode arg_code, + std::int64_t arg_argument) + : time_since_startup(arg_time_since_startup) + , code(arg_code) + , argument(arg_argument) + { } + + uavcan::MakeString<81>::Type toString() const // Heapless return + { + const double ts = time_since_startup.toUSec() / 1e6; + decltype(toString()) out; + out.resize(out.capacity()); + (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, + "%-11.1f %-28s % -20lld %016llx", + ts, + getEventName(code), + static_cast(argument), + static_cast(argument)); + return out; + } + + static const char* getTableHeader() + { + // Matches the string format above + return "Rel. time 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 + std::size_t operator()(T t) const { return static_cast(t); } + }; + + uavcan_linux::SystemClock clock_; + const uavcan::MonotonicTime started_at_ = clock_.getMonotonic(); + const unsigned num_last_events_; + + std::deque last_events_; + std::unordered_map 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); + + const auto ts = clock_.getMonotonic(); + const auto time_since_startup = ts - started_at_; + + last_events_.emplace_front(time_since_startup, code, argument); + if (last_events_.size() > num_last_events_) + { + last_events_.pop_back(); + } + + event_counters_[code].hit(ts); + } + +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> +collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events) +{ + // First, creating a vector of pairs (event code, count) + typedef std::pair Pair; + const auto counters = event_tracer.getEventCounters(); + std::vector 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::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.count > b.second.count; + }); + + return pairs; +} + + +void redraw(const uavcan_linux::NodePtr& node, + const uavcan::MonotonicTime timestamp, + const EventTracer& event_tracer, + const uavcan::dynamic_node_id_server::DistributedServer& server) +{ + /* + * 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] = { }; + + 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(e.second.count)); + } + next_relevant_event_index++; + + 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) + { + 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) + { + char buf[21]; + std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value); + render_top_str(local_state_name, buf); + }; + + const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) + { + using uavcan::dynamic_node_id_server::distributed::RaftCore; + 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; + }; + + /* + * Rendering the data to the CLI + */ + std::printf("\x1b\x5b\x48\x1b\x5b\x32\x4a"); // Clear screen and move caret to the upper-left corner + + // Local state and relevant event counters - two columns + std::printf(" Local state | Event counters\n"); + + render_top_int("Node ID", node->getNodeID().get()); + render_top_str("State", raft_state_to_string(report.state)); + render_top_str("Mode", report.is_active ? "Active" : "Passive"); + render_top_int("Last log index", report.last_log_index); + render_top_int("Last log term", report.last_log_term); + render_top_int("Commit index", report.commit_index); + render_top_int("Current term", report.current_term); + render_top_int("Voted for", report.voted_for.get()); + render_top_str("Since activity", duration_to_string(time_since_last_activity).c_str()); + render_top_int("Unknown nodes", report.num_unknown_nodes); + + // 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 value_getter) + { + std::printf("%-17s", name); + for (std::uint8_t i = 0; i < 4; i++) + { + if (i < (report.cluster_size - 1)) + { + 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(); + }; + + render_followers_state("Node ID", [&](std::uint8_t i) + { + const auto nid = report.followers[i].node_id; + return nid.isValid() ? nid.get() : -1; + }); + render_followers_state("Next index", [&](std::uint8_t i) { return report.followers[i].next_index; }); + render_followers_state("Match index", [&](std::uint8_t i) { return report.followers[i].match_index; }); + + // Empty line before the next block + std::printf(" "); + render_next_event_counter(); + + assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed + + // Separator + std::printf("--------------------------------------+----------------------------------------\n"); + + // Event log + std::printf("%s", EventTracer::RecentEvent::getTableHeader()); // NO NEWLINE + const int num_events_to_render = static_cast(num_rows) - + static_cast(next_relevant_event_index) - + static_cast(NumRowsWithoutEvents); + for (int i = 0; + i < num_events_to_render && i < static_cast(event_tracer.getNumEvents()); + i++) + { + // NEWLINE IS PREPENDED + std::printf("\n%s", event_tracer.getEventByIndex(i).toString().c_str()); + } + // Note that the last line does not have trailing newline. This allows to use all available rows. + + 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 + */ + const unsigned num_last_events_to_keep = 100; + + EventTracer event_tracer(num_last_events_to_keep); + 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); + + ENFORCE(0 <= server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size)); + + /* + * Spinning the node + */ + uavcan::MonotonicTime last_redraw_at; + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + 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 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 + << " [can-iface-name-N...] [-c ] -s ]" + << 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 + { + const 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 + */ + (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; + } +} From 65db68a5146a3d67f4bec0581f04bb36d8fe7a82 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 20:13:23 +0300 Subject: [PATCH 172/268] uavcan_dynamic_node_id_server - a couple of fixes --- .../apps/uavcan_dynamic_node_id_server.cpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 5883fb2a82..6a07bccf0b 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -27,6 +27,9 @@ namespace { +constexpr int MaxNumLastEvents = 30; +constexpr int MinUpdateInterval = 100; + uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) { auto node = uavcan_linux::makeNode(ifaces); @@ -133,6 +136,8 @@ private: { uavcan_posix::dynamic_node_id_server::FileEventTracer::onEvent(code, argument); + had_events_ = true; + const auto ts = clock_.getMonotonic(); const auto time_since_startup = ts - started_at_; @@ -197,7 +202,7 @@ collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events pairs.resize(std::min(num_events, unsigned(pairs.size()))); // Sorting so that the most frequent events are on top of the list - std::sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + std::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { return a.second.count > b.second.count; }); @@ -352,18 +357,17 @@ void redraw(const uavcan_linux::NodePtr& node, std::printf("--------------------------------------+----------------------------------------\n"); // Event log - std::printf("%s", EventTracer::RecentEvent::getTableHeader()); // NO NEWLINE + std::printf("%s\n", EventTracer::RecentEvent::getTableHeader()); const int num_events_to_render = static_cast(num_rows) - static_cast(next_relevant_event_index) - - static_cast(NumRowsWithoutEvents); + static_cast(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(event_tracer.getNumEvents()); i++) { - // NEWLINE IS PREPENDED - std::printf("\n%s", event_tracer.getEventByIndex(i).toString().c_str()); + std::printf("%s\n", event_tracer.getEventByIndex(i).toString().c_str()); } - // Note that the last line does not have trailing newline. This allows to use all available rows. std::fflush(stdout); } @@ -377,9 +381,7 @@ void runForever(const uavcan_linux::NodePtr& node, /* * Event tracer */ - const unsigned num_last_events_to_keep = 100; - - EventTracer event_tracer(num_last_events_to_keep); + EventTracer event_tracer(MaxNumLastEvents); ENFORCE(0 <= event_tracer.init(event_log_file.c_str())); /* @@ -403,7 +405,7 @@ void runForever(const uavcan_linux::NodePtr& node, while (true) { - const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval)); if (res < 0) { std::cerr << "Spin error: " << res << std::endl; From bf0fd63bfed5a54e8de039e06e89e35cc6620301 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 20:54:12 +0300 Subject: [PATCH 173/268] uavcan_dynamic_node_id_server - simple output coloring --- .../apps/uavcan_dynamic_node_id_server.cpp | 120 +++++++++++++++--- 1 file changed, 100 insertions(+), 20 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 6a07bccf0b..96e5583dc2 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -209,12 +209,40 @@ collectRelevantEvents(const EventTracer& event_tracer, const unsigned num_events return pairs; } +enum class CLIColor : unsigned +{ + Red = 31, + Green = 32, + Yellow = 33, + Blue = 34, + Magenta = 35, + Cyan = 36, + Default = 39 +}; + +class CLIColorizer +{ + const CLIColor color_; +public: + explicit CLIColorizer(CLIColor c) : color_(c) + { + std::printf("\033[%um", static_cast(color_)); + } + + ~CLIColorizer() + { + std::printf("\033[%um", static_cast(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 */ @@ -254,22 +282,24 @@ void redraw(const uavcan_linux::NodePtr& node, 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) + const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color) { - std::printf("%-20s %-16s", local_state_name, local_state_value); + { + 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) + 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); + render_top_str(local_state_name, buf, color); }; const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) { - using uavcan::dynamic_node_id_server::distributed::RaftCore; switch (s) { case RaftCore::ServerStateFollower: return "Follower"; @@ -286,6 +316,11 @@ void redraw(const uavcan_linux::NodePtr& node, return str; }; + const auto colorize_if = [](bool condition, CLIColor color) + { + return condition ? color : CLIColor::Default; + }; + /* * Rendering the data to the CLI */ @@ -294,16 +329,45 @@ void redraw(const uavcan_linux::NodePtr& node, // Local state and relevant event counters - two columns std::printf(" Local state | Event counters\n"); - render_top_int("Node ID", node->getNodeID().get()); - render_top_str("State", raft_state_to_string(report.state)); - render_top_str("Mode", report.is_active ? "Active" : "Passive"); - render_top_int("Last log index", report.last_log_index); - render_top_int("Last log term", report.last_log_term); - render_top_int("Commit index", report.commit_index); - render_top_int("Current term", report.current_term); - render_top_int("Voted for", report.voted_for.get()); - render_top_str("Since activity", duration_to_string(time_since_last_activity).c_str()); - render_top_int("Unknown nodes", report.num_unknown_nodes); + render_top_int("Node ID", + node->getNodeID().get(), + CLIColor::Default); + + render_top_str("State", + raft_state_to_string(report.state), + colorize_if(report.state == RaftCore::ServerStateCandidate, CLIColor::Magenta)); + + render_top_str("Mode", + report.is_active ? "Active" : "Passive", + colorize_if(report.is_active, CLIColor::Magenta)); + + 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_int("Unknown nodes", + report.num_unknown_nodes, + colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta)); // Empty line before the next block std::printf(" "); @@ -313,14 +377,16 @@ void redraw(const uavcan_linux::NodePtr& node, std::printf(" Followers "); render_next_event_counter(); - const auto render_followers_state = - [&](const char* name, const std::function value_getter) + const auto render_followers_state = [&](const char* name, + const std::function value_getter, + const std::function 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) { @@ -339,13 +405,27 @@ void redraw(const uavcan_linux::NodePtr& node, render_next_event_counter(); }; + const auto follower_color_getter = [&](std::uint8_t i) + { + if (!report.followers[i].node_id.isValid()) { return CLIColor::Red; } + if (report.followers[i].match_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; - }); - render_followers_state("Next index", [&](std::uint8_t i) { return report.followers[i].next_index; }); - render_followers_state("Match index", [&](std::uint8_t i) { return report.followers[i].match_index; }); + }, + 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); // Empty line before the next block std::printf(" "); From a6618d9be0e45ee9d4ea4e5099d46fe6349b7b31 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:30:10 +0300 Subject: [PATCH 174/268] uavcan_dynamic_node_id_server - colored events --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 96e5583dc2..e45c57dd75 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -217,9 +217,12 @@ enum class CLIColor : unsigned Blue = 34, Magenta = 35, Cyan = 36, + White = 37, Default = 39 }; +CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); } + class CLIColorizer { const CLIColor color_; @@ -269,6 +272,7 @@ void redraw(const uavcan_linux::NodePtr& node, { const char* event_name = ""; char event_count_str[10] = { }; + CLIColor event_color = CLIColor::Default; if (next_relevant_event_index < relevant_events.size()) { @@ -276,10 +280,13 @@ void redraw(const uavcan_linux::NodePtr& node, event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first); std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu", static_cast(e.second.count)); + event_color = getColorHash(static_cast(e.first)); } next_relevant_event_index++; - std::printf(" | %-29s %-9s\n", event_name, event_count_str); + 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) @@ -446,7 +453,9 @@ void redraw(const uavcan_linux::NodePtr& node, i < num_events_to_render && i < static_cast(event_tracer.getNumEvents()); i++) { - std::printf("%s\n", event_tracer.getEventByIndex(i).toString().c_str()); + const auto e = event_tracer.getEventByIndex(i); + CLIColorizer colorizer(getColorHash(static_cast(e.code))); + std::printf("%s\n", e.toString().c_str()); } std::fflush(stdout); From 9b5074051fb989b4dc905ccbc4648da5eb3294da Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:32:00 +0300 Subject: [PATCH 175/268] uavcan_dynamic_node_id_server - setting status OK --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index e45c57dd75..8f778d7ba8 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -58,6 +58,8 @@ uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::N throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get())); } + node->setStatusOk(); + return node; } From aaa3c225c4c11287c94a8491b7b2fec04a14d6d9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:35:12 +0300 Subject: [PATCH 176/268] uavcan_dynamic_node_id_server - posfixing the storage path with self node ID --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 8f778d7ba8..c24426def8 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -603,7 +603,7 @@ int main(int argc, const char** argv) { try { - const auto options = parseOptions(argc, argv); + auto options = parseOptions(argc, argv); std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" "Cluster size: " << int(options.cluster_size) << "\n" @@ -619,6 +619,8 @@ int main(int argc, const char** argv) /* * 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"; From 0348b22b1e6d2bf6b87f5cff55934a9e28b2e68d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 21:44:29 +0300 Subject: [PATCH 177/268] distributed allocation server - StateReport fix --- .../dynamic_node_id_server/distributed/server.hpp | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 940529e358..22b3a4b9be 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -356,6 +356,18 @@ struct StateReport { 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); + } + } } }; From ce4ae983a3d1bea9e6c6dc66737add4a1aa7d886 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 22:46:16 +0300 Subject: [PATCH 178/268] Event logs uses local time --- .../apps/uavcan_dynamic_node_id_server.cpp | 32 +++++++++++++------ 1 file changed, 22 insertions(+), 10 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index c24426def8..4c23536f4b 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -70,25 +70,36 @@ 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::dynamic_node_id_server::TraceCode arg_code, - std::int64_t arg_argument) + 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 { - const double ts = time_since_startup.toUSec() / 1e6; + char timerbuf[11] = { }; + { + const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6; + const auto tm = localtime(&rawtime); + std::strftime(timerbuf, sizeof(timerbuf) - 1U, "%H:%M:%S.", tm); + timerbuf[9] = '0' + (utc_timestamp.toMSec() % 1000) / 100; + timerbuf[10] = '\0'; + } + decltype(toString()) out; out.resize(out.capacity()); (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, - "%-11.1f %-28s % -20lld %016llx", - ts, + "%-10s %-28s % -20lld %016llx", + timerbuf, getEventName(code), static_cast(argument), static_cast(argument)); @@ -98,7 +109,7 @@ public: static const char* getTableHeader() { // Matches the string format above - return "Rel. time Event name Argument (dec) Argument (hex)"; + return "Timestamp Event name Argument (dec) Argument (hex)"; } }; @@ -140,16 +151,17 @@ private: had_events_ = true; - const auto ts = clock_.getMonotonic(); - const auto time_since_startup = ts - started_at_; + 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, code, argument); + 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); + event_counters_[code].hit(ts_m); } public: From ed96e9f0fdbf34f28d6943348e8ff8b2ece549c7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 23:21:03 +0300 Subject: [PATCH 179/268] uavcan_dynamic_node_id_server - Fixed output coloring --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 4c23536f4b..fb275f68d0 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -428,6 +428,7 @@ void redraw(const uavcan_linux::NodePtr& node, 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) { return CLIColor::Magenta; } return CLIColor::Default; From 132ab39c49cc67c7f5575f4c3b21a09e6fbd1bb9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 23 May 2015 23:25:08 +0300 Subject: [PATCH 180/268] uavcan_dynamic_node_id_server - Highliting Leader state in green --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index fb275f68d0..b71bb9dba9 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -356,7 +356,9 @@ void redraw(const uavcan_linux::NodePtr& node, render_top_str("State", raft_state_to_string(report.state), - colorize_if(report.state == RaftCore::ServerStateCandidate, CLIColor::Magenta)); + (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta : + (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green : + CLIColor::Default); render_top_str("Mode", report.is_active ? "Active" : "Passive", From 0bb767c42f0bd6812aa9a3a4aa46537eca6a3526 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 13:34:05 +0300 Subject: [PATCH 181/268] uavcan_dynamic_node_id_server - improved CLI rendering --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index b71bb9dba9..7ccb690af1 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -345,7 +345,8 @@ void redraw(const uavcan_linux::NodePtr& node, /* * Rendering the data to the CLI */ - std::printf("\x1b\x5b\x48\x1b\x5b\x32\x4a"); // Clear screen and move caret to the upper-left corner + 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"); From 5361d7bbeb146b1bce30a9d7a2d434ca5dc01a97 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 13:36:39 +0300 Subject: [PATCH 182/268] uavcan_status_monitor - improved CLI rendering --- libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp b/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp index 540c68dce6..db05275a54 100644 --- a/libuavcan_drivers/linux/apps/uavcan_status_monitor.cpp +++ b/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++) From 7cac4cd4fa221b3479c8ee72d551d51e651e3c9f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 14:23:04 +0300 Subject: [PATCH 183/268] uavcan_dynamic_node_id_server - cleaner time formatting --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 7ccb690af1..a922230892 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -86,20 +86,19 @@ public: uavcan::MakeString<81>::Type toString() const // Heapless return { - char timerbuf[11] = { }; + char timebuf[11] = { }; { const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6; const auto tm = localtime(&rawtime); - std::strftime(timerbuf, sizeof(timerbuf) - 1U, "%H:%M:%S.", tm); - timerbuf[9] = '0' + (utc_timestamp.toMSec() % 1000) / 100; - timerbuf[10] = '\0'; + std::strftime(timebuf, 10, "%H:%M:%S.", tm); + std::snprintf(&timebuf[9], 3, "%02u", static_cast((utc_timestamp.toMSec() % 1000U) / 10U)); } decltype(toString()) out; out.resize(out.capacity()); (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, - "%-10s %-28s % -20lld %016llx", - timerbuf, + "%-11s %-28s %-20lld %016llx", + timebuf, getEventName(code), static_cast(argument), static_cast(argument)); @@ -109,7 +108,7 @@ public: static const char* getTableHeader() { // Matches the string format above - return "Timestamp Event name Argument (dec) Argument (hex)"; + return "Timestamp Event name Argument (dec) Argument (hex)"; } }; From 9e9ade0055eaf65c59119e324b2faa2302576b6e Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 14:25:56 +0300 Subject: [PATCH 184/268] Node ID allocation server - allocation response TX timeout set to DEFAULT_REQUEST_PERIOD_MS --- .../dynamic_node_id_server/allocation_request_manager.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index 8c63132adc..cd0f44832e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -251,6 +251,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) From 4f64e2378e25106812801f66db616d0ba84d0cf4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 15:41:51 +0300 Subject: [PATCH 185/268] RaftCore - runtime assertions --- .../distributed/raft_core.hpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index ab26635bbe..1d0d676d74 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -545,6 +545,14 @@ private: if (!result.isSuccessful()) { + /* + * This callback WILL NEVER be invoked by timeout, because: + * - Any pending request will be cancelled on the next update of the Leader. + * - When the state switches (i.e. the node is not Leader anymore), all pending calls will be cancelled. + * Also note that 'pending_append_entries_fields_' invalidates after every update of the Leader, so + * if there were timeout callbacks, they would be using outdated state. + */ + UAVCAN_ASSERT(0); return; } @@ -650,6 +658,12 @@ private: if (!result.isSuccessful()) { + /* + * This callback WILL NEVER be invoked by timeout, because all pending calls will be cancelled on + * state switch, which ALWAYS happens 100ms after elections (either to Follower or to Leader, depending + * on the number of votes collected). + */ + UAVCAN_ASSERT(0); return; } From 8729d6a2d652403de9226dc67978eda8a7a2722b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 15:59:32 +0300 Subject: [PATCH 186/268] ServiceClient<>::getCallIDByIndex() --- libuavcan/include/uavcan/node/service_client.hpp | 13 +++++++++++++ libuavcan/test/node/service_client.cpp | 6 ++++++ 2 files changed, 19 insertions(+) diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b5a84715fa..b8467c4694 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -341,6 +341,12 @@ public: */ 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. */ @@ -556,6 +562,13 @@ bool ServiceClient::hasPendingCallToServe return NULL != call_registry_.find(ServerSearchPredicate(server_node_id)); } +template +ServiceCallID ServiceClient::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 diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 8b24848a30..82d99a66ac 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -147,6 +147,9 @@ TEST(ServiceClient, Basic) 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; @@ -216,6 +219,9 @@ 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.hasPendingCalls()); ASSERT_TRUE(client1.hasPendingCallToServer(1)); From 546be2b89b7ebb74f55573c2b1f316b059d5e958 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 16:26:14 +0300 Subject: [PATCH 187/268] Fixed RaftCore. The logic is even more complicated. --- .../distributed/raft_core.hpp | 52 +++++++++++++++---- 1 file changed, 43 insertions(+), 9 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 1d0d676d74..aaaa563381 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -64,13 +64,13 @@ public: * - 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) + * - log is not fully replicated or there are uncommitted entries * * Deactivation: * - switch to follower state * - persistent state update error * - only if leader: - * - all log entries are fully replicated + * - all log entries are fully replicated and committed */ class RaftCore : private TimerBase { @@ -373,25 +373,59 @@ private: if (commit_index_ == persistent_state_.getLog().getLastIndex()) { - // All local entries are committed - bool commit_index_fully_replicated = true; + /* + * All local entries are committed. + * Deciding if it is safe to go into passive mode now. + * + * We can go into passive mode if the log is known to be fully replicated and all entries are committed. + * The high-level conditions above are guaranteed to be met if all of the following lower-level conditions + * are met: + * - All local entries are committed (already checked here). + * - Match index on all nodes equals local commit index. + * - Next index on all nodes is strictly greater than the local commit index. + * + * The following code checks if the last two conditions are met. + */ + bool match_index_equals_commit_index = true; + bool next_index_greater_than_commit_index = true; for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) { - const Log::Index match_index = cluster_.getServerMatchIndex(cluster_.getRemoteServerNodeIDAtIndex(i)); + const NodeID server_node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + + const Log::Index match_index = cluster_.getServerMatchIndex(server_node_id); if (match_index != commit_index_) { - commit_index_fully_replicated = false; + match_index_equals_commit_index = false; + break; + } + + const Log::Index next_index = cluster_.getServerNextIndex(server_node_id); + if (next_index <= commit_index_) + { + next_index_greater_than_commit_index = 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 + /* + * Now we know whether the log is replicated and whether all entries are committed, so we can make + * the decision. Remember that since we ended up in this branch, it is already known that all local + * log entries are committed. + */ + const bool all_done = + match_index_equals_commit_index && + next_index_greater_than_commit_index && + cluster_.isClusterDiscovered(); + + setActiveMode(!all_done); } else { - // Not all local entries are committed + /* + * Not all local entries are committed. + * Deciding if it is safe to increment commit index. + */ setActiveMode(true); uint8_t num_nodes_with_next_log_entry_available = 1; // Local node From e5f3a96476562c6ccc1b3bb757b266a1f2685947 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 16:32:52 +0300 Subject: [PATCH 188/268] RaftCore implementation fix --- .../dynamic_node_id_server/distributed/raft_core.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index aaaa563381..a7abd0bcc4 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -247,6 +247,11 @@ private: setActiveMode(false); // Haha } + if (append_entries_client_.hasPendingCalls()) + { + append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why + } + if (active_mode_ || (next_server_index_ > 0)) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(next_server_index_); From 1234494e7711137755be9d738654352d2ad57497 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 16:53:58 +0300 Subject: [PATCH 189/268] uavcan_dynamic_node_id_server will not run if stdout is not a tty --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index a922230892..6545ed2d30 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -618,6 +618,12 @@ int main(int argc, const char** argv) { try { + 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" From 11161e7b1f4c3749e1d5965c480606177262975d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 17:18:14 +0300 Subject: [PATCH 190/268] NodeDiscoverer logic fix --- .../dynamic_node_id_server/node_discoverer.hpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index d8042074a2..ef7f901749 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -267,11 +267,6 @@ class NodeDiscoverer : TimerBase 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()) { @@ -280,6 +275,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", From 73273ab06dcce4898a401d2a0d0b865447f24080 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 17:21:26 +0300 Subject: [PATCH 191/268] Optimized NodeDiscoverer - picking any node to query, without any preference --- .../node_discoverer.hpp | 31 ++----------------- 1 file changed, 3 insertions(+), 28 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index ef7f901749..827551250c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -86,27 +86,6 @@ class NodeDiscoverer : TimerBase typedef Map NodeMap; - class HighestUptimeSearcher : ::uavcan::Noncopyable - { - uint32_t highest_uptime_sec_; - NodeID node_id_; - - public: - HighestUptimeSearcher() : highest_uptime_sec_(0) { } - - bool operator()(const NodeID& key, const NodeData& value) - { - if (value.last_seen_uptime >= highest_uptime_sec_) - { - highest_uptime_sec_ = value.last_seen_uptime; - node_id_ = key; - } - return false; - } - - NodeID getNodeWithHighestUptime() const { return node_id_; } - }; - /** * When this number of attempts has been made, the discoverer will give up and assume that the node * does not implement this service. @@ -144,13 +123,9 @@ class NodeDiscoverer : TimerBase NodeID pickNextNodeToQuery() const { - HighestUptimeSearcher searcher; - - const NodeID* const out = node_map_.find(searcher); - (void)out; - UAVCAN_ASSERT(out == NULL); - - return searcher.getNodeWithHighestUptime(); + // 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) From a2104f0bba6296269789309da0d4529b8c68c043 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 17:53:23 +0300 Subject: [PATCH 192/268] Fixed Raft timings --- .../dynamic_node_id/server/220.AppendEntries.uavcan | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index f650e91306..77c32edf59 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -3,16 +3,17 @@ # Please refer to the specification for details. # -uint16 DEFAULT_REQUEST_TIMEOUT_MS = 100 +uint16 DEFAULT_REQUEST_TIMEOUT_MS = 200 -uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1000 # request timeout * max cluster size * 2 requests +uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1600 # request timeout * (max cluster size - 1) * 2 requests uint32 term uint32 prev_log_term uint8 prev_log_index uint8 leader_commit -Entry[<=5] entries +# Full replication: 127 requests * 0.2 sec interval * 4 followers * 2 trips of next_index value ~ 3.5 min +Entry[<=1] entries --- From 2952608ffaca86c41b8052f16282ba7b1f005a12 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 18:20:57 +0300 Subject: [PATCH 193/268] RaftCore: new event code --- .../protocol/dynamic_node_id_server/distributed/raft_core.hpp | 1 + .../include/uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index a7abd0bcc4..97a92f70aa 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -612,6 +612,7 @@ private: else { cluster_.decrementServerNextIndex(result.getCallID().server_node_id); + trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get()); } } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index a6b452aaa0..1a75ef7657 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -52,7 +52,7 @@ enum UAVCAN_EXPORT TraceCode // 25 TraceRaftAppendEntriesCallFailure, // error code (may be negated) TraceRaftElectionComplete, // number of votes collected - Trace1, + TraceRaftAppendEntriesRespUnsucfl, // node ID of the client Trace2, Trace3, // 30 @@ -127,7 +127,7 @@ public: "RaftNewEntryCommitted", "RaftAppendEntriesCallFailure", "RaftElectionComplete", - "", + "RaftAppendEntriesRespUnsucfl", "", "", "AllocationFollowupResponse", From fc173aca445a7a62c5c2ac1028737230815781f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 18:32:27 +0300 Subject: [PATCH 194/268] uavcan_dynamic_node_id_server fixed coloring --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 6545ed2d30..b1be286fc4 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -432,7 +432,11 @@ void redraw(const uavcan_linux::NodePtr& node, { 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) { return CLIColor::Magenta; } + 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; }; From 59dd6d090532141040367c25646621d2dc9c78f0 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 20:06:13 +0300 Subject: [PATCH 195/268] Raft fix --- .../protocol/dynamic_node_id_server/distributed/raft_core.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 97a92f70aa..3066d8da88 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -642,6 +642,8 @@ private: */ 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) { @@ -679,6 +681,7 @@ private: 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()); From c323d8e724a24a66a591ca550adb7421c3e42b42 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 20:12:33 +0300 Subject: [PATCH 196/268] Raft - ignoring Allocation activity if it is a response --- .../dynamic_node_id_server/distributed/raft_core.hpp | 2 +- .../protocol/dynamic_node_id_server/distributed/server.hpp | 7 +++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 3066d8da88..4f3b0a9d05 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -61,7 +61,7 @@ public: * Active state switch logic: * Activation (this is default state): * - vote request - * - allocation activity detected + * - allocation request at any stage * - only if leader: * - discovery activity detected * - log is not fully replicated or there are uncommitted entries diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 22b3a4b9be..03c4ede47c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -135,9 +135,12 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) + virtual void handleAllocationActivityDetection(const ReceivedDataStructure& msg) { - raft_core_.forceActiveMode(); + if (msg.isAnonymousTransfer()) + { + raft_core_.forceActiveMode(); + } } /* From b7f7defd85435e2857bbc8aa52f700d53c879c5d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 20:19:56 +0300 Subject: [PATCH 197/268] Raft implementation fix --- .../dynamic_node_id_server/distributed/cluster_manager.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 50a5c5cefb..6e0a8e038c 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -91,7 +91,7 @@ private: void addServer(NodeID node_id) { - UAVCAN_ASSERT((num_known_servers_ + 1) < (MaxClusterSize - 2)); + UAVCAN_ASSERT((num_known_servers_ + 1) < MaxClusterSize); if (!isKnownServer(node_id) && node_id.isUnicast()) { tracer_.onEvent(TraceRaftNewServerDiscovered, node_id.get()); From eb6102a9d49831492312910d690bcdc65bbb80e1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:09:56 +0300 Subject: [PATCH 198/268] Raft - minor timing fix, no changes to the logic --- .../dynamic_node_id_server/distributed/raft_core.hpp | 7 ++----- 1 file changed, 2 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 4f3b0a9d05..a1600a9e16 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -156,10 +156,7 @@ private: bool isActivityTimedOut() const { const int multiplier = static_cast(getNode().getNodeID().get()) - 1; - - const MonotonicDuration activity_timeout = - MonotonicDuration::fromUSec(base_activity_timeout_.toUSec() + update_interval_.toUSec() * multiplier); - + const MonotonicDuration activity_timeout = base_activity_timeout_ + update_interval_ * multiplier; return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout); } @@ -798,7 +795,7 @@ public: /* * Initializing state variables */ - last_activity_timestamp_ = getNode().getMonotonicTime(); + last_activity_timestamp_ = getNode().getMonotonicTime() + update_interval_; active_mode_ = true; server_state_ = ServerStateFollower; next_server_index_ = 0; From 8c777938926c3fd45f2bb2acfe3265ce7f5356c1 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:14:59 +0300 Subject: [PATCH 199/268] Raft logic fix: auto-discovery on AE request --- .../distributed/cluster_manager.hpp | 35 ++++++++++--------- .../distributed/raft_core.hpp | 13 +++++-- 2 files changed, 29 insertions(+), 19 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index 6e0a8e038c..f237b19436 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -89,22 +89,6 @@ private: return NULL; } - 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(num_known_servers_ + 1U); - } - else - { - UAVCAN_ASSERT(0); - } - } - virtual void handleTimerEvent(const TimerEvent&) { UAVCAN_ASSERT(num_known_servers_ < cluster_size_); @@ -301,6 +285,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(num_known_servers_ + 1U); + } + else + { + UAVCAN_ASSERT(0); + } + } + /** * Whether such server has been discovered. */ diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index a1600a9e16..8abdb8919e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -457,9 +457,16 @@ private: { 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 From 702c96a1923ab1b3d4a0e5f7a1f9d0a8f30f1ba5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:21:36 +0300 Subject: [PATCH 200/268] Node<>::getInternalFailureCount() --- libuavcan/include/uavcan/node/node.hpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 930ecf419d..65b478b7a7 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/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. From 78a380062c7fa23a229db0102cc4f5564ed1a685 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 21:25:38 +0300 Subject: [PATCH 201/268] uavcan_dynamic_node_id_server - printing the number of internal failures --- .../linux/apps/uavcan_dynamic_node_id_server.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index b1be286fc4..a46c398801 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -392,6 +392,10 @@ void redraw(const uavcan_linux::NodePtr& node, 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(); @@ -455,10 +459,6 @@ void redraw(const uavcan_linux::NodePtr& node, [&](std::uint8_t i) { return report.followers[i].match_index; }, follower_color_getter); - // Empty line before the next block - std::printf(" "); - render_next_event_counter(); - assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed // Separator From 17c4b975acf5a0d70181c7a763dcca5c9d33f981 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 24 May 2015 23:13:10 +0300 Subject: [PATCH 202/268] Test fix --- .../protocol/dynamic_node_id_server/distributed/server.cpp | 2 +- .../protocol/dynamic_node_id_server/node_discoverer.cpp | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index e46cf27a4e..eac00afa31 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -166,7 +166,7 @@ TEST(dynamic_node_id_server_Server, Basic) /* * Fire */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(4000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 7d1d692976..304070bf3f 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -226,7 +226,7 @@ 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(1650)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -244,7 +244,7 @@ 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(1650)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); @@ -258,7 +258,7 @@ TEST(dynamic_node_id_server_NodeDiscoverer, RestartAndMaxAttempts) /* * Waiting for timeout */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1650)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); From 258da95d12dc8efafd93f3eba15677d0382e3c16 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 00:15:18 +0300 Subject: [PATCH 203/268] RaftCore::checkInvariants() --- .../distributed/raft_core.hpp | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 8abdb8919e..18ba5f8c52 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -151,6 +151,28 @@ private: INode& getNode() { return append_entries_srv_.getNode(); } const INode& getNode() const { return append_entries_srv_.getNode(); } + 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(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(); } bool isActivityTimedOut() const @@ -455,6 +477,8 @@ private: void handleAppendEntriesRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { + checkInvariants(); + if (!cluster_.isKnownServer(request.getSrcNodeID())) { if (cluster_.isClusterDiscovered()) @@ -585,6 +609,7 @@ private: void handleAppendEntriesResponse(const ServiceCallResult& result) { UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + checkInvariants(); if (!result.isSuccessful()) { @@ -627,6 +652,7 @@ private: void handleRequestVoteRequest(const ReceivedDataStructure& request, ServiceResponseDataStructure& response) { + checkInvariants(); trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); if (!cluster_.isKnownServer(request.getSrcNodeID())) @@ -702,6 +728,7 @@ private: void handleRequestVoteResponse(const ServiceCallResult& result) { UAVCAN_ASSERT(server_state_ == ServerStateCandidate); // When state switches, all requests must be cancelled + checkInvariants(); if (!result.isSuccessful()) { @@ -733,6 +760,8 @@ private: virtual void handleTimerEvent(const TimerEvent&) { + checkInvariants(); + if (cluster_.hadDiscoveryActivity() && isLeader()) { setActiveMode(true); From e289a1e09c3777ce1ae12b9aa1b6b0097bd162d7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 01:22:26 +0300 Subject: [PATCH 204/268] uavcan_linux::makeApplicationID() --- .../apps/uavcan_dynamic_node_id_server.cpp | 16 ++++++-- .../include/uavcan_linux/system_utils.hpp | 41 +++++++++++++++++++ 2 files changed, 53 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index a46c398801..3fe3dac64c 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -39,10 +39,10 @@ uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::N node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); { - const auto machine_id = uavcan_linux::MachineIDReader().read(); + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); uavcan::protocol::HardwareVersion hwver; - std::copy(machine_id.begin(), machine_id.end(), hwver.unique_id.begin()); + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); std::cout << hwver << std::endl; node->setHardwareVersion(hwver); @@ -498,7 +498,6 @@ void runForever(const uavcan_linux::NodePtr& node, * Storage backend */ uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; - ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str())); /* @@ -506,7 +505,16 @@ void runForever(const uavcan_linux::NodePtr& node, */ uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer); - ENFORCE(0 <= server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size)); + 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 diff --git a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp index 14aa91ad63..b4d6ce0a5c 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp @@ -14,6 +14,7 @@ #include #include #include +#include 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 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 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(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(node_name.c_str()), static_cast(node_name.length())); + crc.add(instance_id); + HalfID half(crc.get()); + std::copy_n(half.bytes, 8, out.begin() + 8); + } + + return out; +} } From d89a8dcbcc7b2c4e6677308232485045cce74158 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 01:50:24 +0300 Subject: [PATCH 205/268] Linux test app - Dynamic node ID client --- libuavcan_drivers/linux/CMakeLists.txt | 3 + .../apps/test_dynamic_node_id_client.cpp | 121 ++++++++++++++++++ 2 files changed, 124 insertions(+) create mode 100644 libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 408fff617c..b33af01b8e 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -60,6 +60,9 @@ target_link_libraries(test_system_utils ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INI 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 # diff --git a/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp new file mode 100644 index 0000000000..8743577730 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_dynamic_node_id_client.cpp @@ -0,0 +1,121 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include "debug.hpp" +#include +#include + +namespace +{ + +uavcan_linux::NodePtr initNodeWithDynamicID(const std::vector& 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] << " [can-iface-name-N...]\n" + << "Where 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(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; + } +} From 97b35cd09e553e097be68a37d00c2e767157aa4f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 01:57:07 +0300 Subject: [PATCH 206/268] NodeIDSelector fix --- .../uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp index 97c86ab21b..bd0b55c0f9 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp +++ b/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) From 388c023168404da0bdce36e06a0d471d6794fb56 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 02:11:02 +0300 Subject: [PATCH 207/268] uavcan_dynamic_node_id_server - longer log --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 3fe3dac64c..4b2d13f1de 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -27,7 +27,7 @@ namespace { -constexpr int MaxNumLastEvents = 30; +constexpr int MaxNumLastEvents = 50; constexpr int MinUpdateInterval = 100; uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) From 2231b0063743aab06b09aa4888db42d444284baf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:04:59 +0300 Subject: [PATCH 208/268] Raft active state extension removed --- .../server/220.AppendEntries.uavcan | 17 +- .../distributed/cluster_manager.hpp | 18 -- .../distributed/raft_core.hpp | 200 +++++------------- .../distributed/server.hpp | 11 +- .../distributed/server.cpp | 19 +- .../apps/uavcan_dynamic_node_id_server.cpp | 10 +- 6 files changed, 87 insertions(+), 188 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index 77c32edf59..e3009286d2 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -3,16 +3,25 @@ # Please refer to the specification for details. # -uint16 DEFAULT_REQUEST_TIMEOUT_MS = 200 - -uint16 DEFAULT_BASE_ELECTION_TIMEOUT_MS = 1600 # request timeout * (max cluster size - 1) * 2 requests +# +# Given min election timeout and cluster size, the maximum request interval can be derived as follows: +# max request interval = (min election timeout) / 2 requests / (cluster size - 1) +# Obviously, request interval can be lower than that if needed, but not higher. +# +# Real timeout is randomized in the range (MIN, MAX]. +# +uint16 MIN_ELECTION_TIMEOUT_MS = 4000 +uint16 MAX_ELECTION_TIMEOUT_MS = 6000 uint32 term uint32 prev_log_term uint8 prev_log_index uint8 leader_commit -# Full replication: 127 requests * 0.2 sec interval * 4 followers * 2 trips of next_index value ~ 3.5 min +# +# Worst-case replication time can be computed as: +# worst replication time = (127 log entries) * (2 trips of next_index) * (cluster size - 1) * (request interval) +# Entry[<=1] entries --- diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp index f237b19436..0fa18fc17d 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp +++ b/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(); } @@ -151,8 +149,6 @@ private: return; } - had_discovery_activity_ = true; - /* * Updating the set of known servers */ @@ -204,7 +200,6 @@ public: , discovery_pub_(node) , cluster_size_(0) , num_known_servers_(0) - , had_discovery_activity_(false) { } /** @@ -417,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. diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 18ba5f8c52..5b776553c8 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/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 #include #include #include @@ -51,26 +52,14 @@ 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 request at any stage - * - only if leader: - * - discovery activity detected - * - log is not fully replicated or there are uncommitted entries - * - * Deactivation: - * - switch to follower state - * - persistent state update error - * - only if leader: - * - all log entries are fully replicated and committed */ class RaftCore : private TimerBase { @@ -111,8 +100,7 @@ private: /* * 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_; @@ -125,7 +113,7 @@ private: 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 @@ -139,9 +127,7 @@ private: ServiceServer append_entries_srv_; ServiceClient append_entries_client_; ServiceServer request_vote_srv_; - - enum { NumRequestVoteCalls = ClusterManager::MaxClusterSize - 1 }; - ServiceClient request_vote_client_; + ServiceClient request_vote_client_; /* * Methods @@ -167,19 +153,33 @@ private: 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(); } + void registerActivity() + { + last_activity_timestamp_ = getNode().getMonotonicTime(); + + static const int32_t randomization_range_msec = + AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS - AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS; + + const int32_t random_msec = (std::rand() % randomization_range_msec) + 1; + + randomized_activity_timeout_ = + MonotonicDuration::fromMSec(AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS + random_msec); + + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS); + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS); + } bool isActivityTimedOut() const { - const int multiplier = static_cast(getNode().getNodeID().get()) - 1; - const MonotonicDuration activity_timeout = base_activity_timeout_ + update_interval_ * multiplier; - return getNode().getMonotonicTime() > (last_activity_timestamp_ + activity_timeout); + return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_); } void handlePersistentStateUpdateError(int error) @@ -187,13 +187,12 @@ private: 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(); @@ -202,8 +201,6 @@ private: void updateCandidate() { - UAVCAN_ASSERT(active_mode_); - if (num_votes_received_in_this_campaign_ > 0) { trace(TraceRaftElectionComplete, num_votes_received_in_this_campaign_); @@ -238,7 +235,7 @@ private: req.last_log_term = persistent_state_.getLog().getEntryAtIndex(req.last_log_index)->term; req.term = persistent_state_.getCurrentTerm(); - for (uint8_t i = 0; i < NumRequestVoteCalls; i++) + for (uint8_t i = 0; i < MaxNumFollowers; i++) { const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); if (!node_id.isUnicast()) @@ -261,17 +258,12 @@ private: void updateLeader() { - if (cluster_.getClusterSize() == 1) - { - setActiveMode(false); // Haha - } - if (append_entries_client_.hasPendingCalls()) { 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()); @@ -364,18 +356,6 @@ private: } } - 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); @@ -386,7 +366,6 @@ private: } registerActivity(); // Deferring future elections switchState(ServerStateFollower); - setActiveMode(false); } void propagateCommitIndex() @@ -395,63 +374,12 @@ private: UAVCAN_ASSERT(server_state_ == ServerStateLeader); UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); - if (commit_index_ == persistent_state_.getLog().getLastIndex()) - { - /* - * All local entries are committed. - * Deciding if it is safe to go into passive mode now. - * - * We can go into passive mode if the log is known to be fully replicated and all entries are committed. - * The high-level conditions above are guaranteed to be met if all of the following lower-level conditions - * are met: - * - All local entries are committed (already checked here). - * - Match index on all nodes equals local commit index. - * - Next index on all nodes is strictly greater than the local commit index. - * - * The following code checks if the last two conditions are met. - */ - bool match_index_equals_commit_index = true; - bool next_index_greater_than_commit_index = true; - - for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) - { - const NodeID server_node_id = cluster_.getRemoteServerNodeIDAtIndex(i); - - const Log::Index match_index = cluster_.getServerMatchIndex(server_node_id); - if (match_index != commit_index_) - { - match_index_equals_commit_index = false; - break; - } - - const Log::Index next_index = cluster_.getServerNextIndex(server_node_id); - if (next_index <= commit_index_) - { - next_index_greater_than_commit_index = false; - break; - } - } - - /* - * Now we know whether the log is replicated and whether all entries are committed, so we can make - * the decision. Remember that since we ended up in this branch, it is already known that all local - * log entries are committed. - */ - const bool all_done = - match_index_equals_commit_index && - next_index_greater_than_commit_index && - cluster_.isClusterDiscovered(); - - setActiveMode(!all_done); - } - else + if (commit_index_ < persistent_state_.getLog().getLastIndex()) { /* * Not all local entries are committed. * Deciding if it is safe to increment commit index. */ - setActiveMode(true); - uint8_t num_nodes_with_next_log_entry_available = 1; // Local node for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) { @@ -536,7 +464,6 @@ private: registerActivity(); switchState(ServerStateFollower); - setActiveMode(false); /* * Step 2 @@ -613,14 +540,6 @@ private: if (!result.isSuccessful()) { - /* - * This callback WILL NEVER be invoked by timeout, because: - * - Any pending request will be cancelled on the next update of the Leader. - * - When the state switches (i.e. the node is not Leader anymore), all pending calls will be cancelled. - * Also note that 'pending_append_entries_fields_' invalidates after every update of the Leader, so - * if there were timeout callbacks, they would be using outdated state. - */ - UAVCAN_ASSERT(0); return; } @@ -664,8 +583,6 @@ private: 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. @@ -732,12 +649,6 @@ private: if (!result.isSuccessful()) { - /* - * This callback WILL NEVER be invoked by timeout, because all pending calls will be cancelled on - * state switch, which ALWAYS happens 100ms after elections (either to Follower or to Leader, depending - * on the number of votes collected). - */ - UAVCAN_ASSERT(0); return; } @@ -762,11 +673,6 @@ private: { checkInvariants(); - if (cluster_.hadDiscoveryActivity() && isLeader()) - { - setActiveMode(true); - } - switch (server_state_) { case ServerStateFollower: @@ -796,21 +702,15 @@ 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::MAX_ELECTION_TIMEOUT_MS)) , server_state_(ServerStateFollower) , next_server_index_(0) , num_votes_received_in_this_campaign_(0) @@ -831,13 +731,13 @@ public: /* * Initializing state variables */ - last_activity_timestamp_ = getNode().getMonotonicTime() + update_interval_; - active_mode_ = true; server_state_ = ServerStateFollower; next_server_index_ = 0; num_votes_received_in_this_campaign_ = 0; commit_index_ = 0; + registerActivity(); + /* * Initializing internals */ @@ -872,7 +772,6 @@ public: } append_entries_client_.setCallback(AppendEntriesResponseCallback(this, &RaftCore::handleAppendEntriesResponse)); - append_entries_client_.setRequestTimeout(update_interval_); res = request_vote_client_.init(); if (res < 0) @@ -880,24 +779,34 @@ public: return res; } request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); - request_vote_client_.setRequestTimeout(update_interval_); - startPeriodic(update_interval_); + /* + * Initializing timing constants + * Refer to the specification for the formula + */ + const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); - trace(TraceRaftCoreInited, update_interval_.toUSec()); + const MonotonicDuration update_interval = + MonotonicDuration::fromMSec(AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS / + 2 / max(static_cast(3), num_followers)); + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Update interval: %ld msec", static_cast(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. */ @@ -994,8 +903,9 @@ public: const PersistentState& getPersistentState() const { return persistent_state_; } const ClusterManager& getClusterManager() const { return cluster_; } MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; } - bool isInActiveMode() const { return active_mode_; } ServerState getServerState() const { return server_state_; } + MonotonicDuration getUpdateInterval() const { return getPeriod(); } + MonotonicDuration getRandomizedTimeout() const { return randomized_activity_timeout_; } }; } diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 03c4ede47c..31b709d441 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -135,12 +135,9 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure& msg) + virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) { - if (msg.isAnonymousTransfer()) - { - raft_core_.forceActiveMode(); - } + // TODO: remove this method } /* @@ -315,7 +312,6 @@ struct StateReport uint8_t cluster_size; RaftCore::ServerState state; - bool is_active; Log::Index last_log_index; Log::Index commit_index; @@ -326,6 +322,7 @@ struct StateReport NodeID voted_for; MonotonicTime last_activity_timestamp; + MonotonicDuration randomized_timeout; uint8_t num_unknown_nodes; @@ -344,13 +341,13 @@ struct StateReport StateReport(const Server& s) : cluster_size (s.getRaftCore().getClusterManager().getClusterSize()) , state (s.getRaftCore().getServerState()) - , is_active (s.getRaftCore().isInActiveMode()) , 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); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index eac00afa31..cd4a67d393 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/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(6000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 4b2d13f1de..03372ec396 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -360,10 +360,6 @@ void redraw(const uavcan_linux::NodePtr& node, (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green : CLIColor::Default); - render_top_str("Mode", - report.is_active ? "Active" : "Passive", - colorize_if(report.is_active, CLIColor::Magenta)); - render_top_int("Last log index", report.last_log_index, CLIColor::Default); @@ -388,6 +384,10 @@ void redraw(const uavcan_linux::NodePtr& node, 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)); @@ -630,6 +630,8 @@ 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; From e60a76d5621a6cbf01de736aad4d5463ca707941 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:08:38 +0300 Subject: [PATCH 209/268] Test timing fix --- libuavcan/test/protocol/firmware_update_trigger.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp index 2e6f286160..08ef9c4fd8 100644 --- a/libuavcan/test/protocol/firmware_update_trigger.cpp +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -318,7 +318,7 @@ TEST(FirmwareUpdateTrigger, MultiNode) * This also checks correctness of the round-robin selector */ checker.retry_quota = 2; - nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4100)); // Two will retry, one drop, one confirm + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(4200)); // Two will retry, one drop, one confirm ASSERT_TRUE(trigger.isTimerRunning()); ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed From a97762ae212f5519cf9eaec9712afeaf4f6bd968 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:13:13 +0300 Subject: [PATCH 210/268] Dead code removal --- .../dynamic_node_id_server/allocation_request_manager.hpp | 6 ------ .../protocol/dynamic_node_id_server/distributed/server.hpp | 5 ----- .../dynamic_node_id_server/allocation_request_manager.cpp | 6 ------ 3 files changed, 17 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp index cd0f44832e..2c0aa98670 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp +++ b/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& msg) = 0; - virtual ~IAllocationRequestHandler() { } }; @@ -132,7 +127,6 @@ class AllocationRequestManager void handleAllocation(const ReceivedDataStructure& msg) { trace(TraceAllocationActivity, msg.getSrcNodeID().get()); - handler_.handleAllocationActivityDetection(msg); if (!msg.isAnonymousTransfer()) { diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 31b709d441..5fd35bb50a 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -135,11 +135,6 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler } } - virtual void handleAllocationActivityDetection(const ReceivedDataStructure&) - { - // TODO: remove this method - } - /* * Methods of INodeDiscoveryHandler */ diff --git a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp index 037d73bc3a..dfecf86b6a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp +++ b/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& msg) - { - std::cout << "ALLOCATION ACTIVITY\n" << msg << std::endl; - } - bool matchAndPopLastRequest(const UniqueID& unique_id, uavcan::NodeID preferred_node_id) { if (requests_.empty()) From 9faf8470e6e5cdd430ba68bc5e00aa23786e90ef Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:18:07 +0300 Subject: [PATCH 211/268] Fixed Raft definitions --- .../server/220.AppendEntries.uavcan | 4 ++-- .../distributed/raft_core.hpp | 17 +++++++++-------- .../distributed/server.cpp | 2 +- .../apps/uavcan_dynamic_node_id_server.cpp | 2 +- 4 files changed, 13 insertions(+), 12 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index e3009286d2..329bfff8e6 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -10,8 +10,8 @@ # # Real timeout is randomized in the range (MIN, MAX]. # -uint16 MIN_ELECTION_TIMEOUT_MS = 4000 -uint16 MAX_ELECTION_TIMEOUT_MS = 6000 +uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000 +uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000 uint32 term uint32 prev_log_term diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp index 5b776553c8..f00fd1ec2f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -165,16 +165,16 @@ private: { last_activity_timestamp_ = getNode().getMonotonicTime(); - static const int32_t randomization_range_msec = - AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS - AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS; + 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::MIN_ELECTION_TIMEOUT_MS + random_msec); + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec); - UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS); - UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS); + 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 @@ -710,7 +710,8 @@ public: , cluster_(node, storage, persistent_state_.getLog(), tracer) , commit_index_(0) // Per Raft paper, commitIndex must be initialized to zero , last_activity_timestamp_(node.getMonotonicTime()) - , randomized_activity_timeout_(MonotonicDuration::fromMSec(AppendEntries::Request::MAX_ELECTION_TIMEOUT_MS)) + , 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) @@ -787,8 +788,8 @@ public: const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); const MonotonicDuration update_interval = - MonotonicDuration::fromMSec(AppendEntries::Request::MIN_ELECTION_TIMEOUT_MS / - 2 / max(static_cast(3), num_followers)); + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS / + 2 / max(static_cast(2), num_followers)); UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "Update interval: %ld msec", static_cast(update_interval.toMSec())); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index cd4a67d393..59a4178ebd 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -165,7 +165,7 @@ TEST(dynamic_node_id_server_Server, Basic) /* * Fire */ - nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(9000)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(15000)); ASSERT_TRUE(client.isAllocationComplete()); ASSERT_EQ(PreferredNodeID, client.getAllocatedNodeID()); diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 03372ec396..39634e313a 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -27,7 +27,7 @@ namespace { -constexpr int MaxNumLastEvents = 50; +constexpr int MaxNumLastEvents = 30; constexpr int MinUpdateInterval = 100; uavcan_linux::NodePtr initNode(const std::vector& ifaces, uavcan::NodeID nid, const std::string& name) From aafdf81f29735283ad6c904932618ae2cb7e4dc2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:25:29 +0300 Subject: [PATCH 212/268] Removed unused event code --- .../include/uavcan/protocol/dynamic_node_id_server/event.hpp | 4 ++-- libuavcan/test/protocol/dynamic_node_id_server/event.cpp | 1 - 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp index 1a75ef7657..d447e01ac6 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -38,7 +38,7 @@ enum UAVCAN_EXPORT 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 @@ -115,7 +115,7 @@ public: "RaftBadClusterSizeReceived", "RaftCoreInited", "RaftStateSwitch", - "RaftActiveSwitch", + "", "RaftNewLogEntry", "RaftRequestIgnored", "RaftVoteRequestReceived", diff --git a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp index e8f2c457e6..7405f5d850 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/event.cpp +++ b/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)); From 6a34d19cc5136ce193b733971e07738549d5651a Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:36:41 +0300 Subject: [PATCH 213/268] Style fixes in uavcan_posix/dynamic_node_id_server --- .../dynamic_node_id_server/file_event_tracer.hpp | 6 +++--- .../dynamic_node_id_server/file_storage_backend.hpp | 3 ++- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 863d7465a1..45a6d11c12 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -54,8 +54,8 @@ protected: int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), static_cast(code), static_cast(argument)); - write(fd, buffer, n); - close(fd); + (void)write(fd, buffer, n); // TODO FIXME Write loop + (void)close(fd); } } @@ -76,7 +76,7 @@ public: int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); if (fd >= 0) { - close(fd); + (void)close(fd); } } return rv; diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp index a2834c6cfb..76c515a8cb 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -43,6 +43,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken PathString base_path; +protected: virtual String get(const String& key) const { using namespace std; @@ -80,7 +81,7 @@ class FileStorageBackend : public uavcan::dynamic_node_id_server::IStorageBacken int fd = open(path.c_str(), O_WRONLY | O_CREAT | O_TRUNC, FilePermissions); if (fd >= 0) { - (void)write(fd, value.c_str(), value.size()); + (void)write(fd, value.c_str(), value.size()); // TODO FIXME Write loop (void)fsync(fd); (void)close(fd); } From 53317eb90249f4f3df6501f68761e1c4c99e86c3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 20:37:58 +0300 Subject: [PATCH 214/268] BasicFileSeverBackend style fixes --- .../basic_file_server_backend.hpp | 34 +++++-------------- 1 file changed, 9 insertions(+), 25 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index c88bf8b731..7a02ee11e1 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -33,32 +33,28 @@ namespace uavcan_posix */ class BasicFileSeverBackend : public uavcan::IFileServerBackend { - enum { FilePermissions = 438 }; ///< 0o666 -public: +protected: /** - * * Back-end for uavcan.protocol.file.GetInfo. * Implementation of this method is required. * On success the method must return zero. */ - __attribute__((optimize("O0"))) 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; if (path.size() > 0) { - FirmwareCommon fw; fw.getFileInfo(path.c_str()); out_crc64 = fw.descriptor.image_crc; out_size = fw.descriptor.image_size; - // todo Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. + // 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 out_type.flags = uavcan::protocol::file::EntryType::FLAG_FILE | - uavcan::protocol::file::EntryType::FLAG_READABLE;; + uavcan::protocol::file::EntryType::FLAG_READABLE; rv = 0; } return rv; @@ -71,16 +67,12 @@ public: * if the end of file is reached. * On success the method must return zero. */ - __attribute__((optimize("O0"))) 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) @@ -89,38 +81,30 @@ public: } else { - if (::lseek(fd, offset, SEEK_SET) < 0) { rv = errno; } else { - //todo uses a read at offset to fill on EAGAIN - ssize_t len = ::read(fd, out_buffer, inout_size); + // TODO use a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); - if (len < 0) + if (len < 0) { rv = errno; } else { - - inout_size = len; + inout_size = len; rv = 0; } } - close(fd); + (void)close(fd); } } return rv; } - - BasicFileSeverBackend() { } - - - - }; } From 85f498bbe0c01137246975fa91a12e38e70a22a5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:05:35 +0300 Subject: [PATCH 215/268] FirmwareVersionChecker formatting fix --- .../uavcan_posix/firmware_version_checker.hpp | 328 ++++++++---------- 1 file changed, 151 insertions(+), 177 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 3066c07a18..c53b6e2c08 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -10,10 +10,9 @@ #define UAVCAN_POSIX_FIRMWARE_VERSION_CHECKER_HPP_INCLUDED #include -#include #include #include -#include +#include #include #include @@ -31,191 +30,24 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { enum { FilePermissions = 438 }; ///< 0o666 - FirmwarePath *paths_; + FirmwarePath* paths_; -public: - - /** - * 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. - */ - __attribute__((optimize("O0"))) - virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const - uavcan::protocol::GetNodeInfo::Response& node_info, - FirmwareFilePath& out_firmware_file_path) + int copyIfNot(const char* srcpath, const char* destpath) { - /* 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[FirmwarePath::MaxBasePathLength + 1]; - int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", - paths_->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) - { - - FAR DIR *fwdir = opendir(fname_root); - - fname_root[n++] = FirmwarePath::getPathSeparator(); - fname_root[n++] = '\0'; - - if (fwdir) - { - FAR struct dirent *pfile; - while((pfile = readdir(fwdir)) != NULL) - { - if (DIRENT_ISFILE(pfile->d_type)) - { - // Open any bin file in there. - if (strstr(pfile->d_name, ".bin")) - { - FirmwarePath::PathString full_src_path = fname_root; - full_src_path += pfile->d_name; - - FirmwarePath::PathString full_dst_path = paths_->getFirmwareCachePath().c_str(); - full_dst_path += pfile->d_name; - - /* ease the burden on the user */ - int cr = copy_if_not(full_src_path.c_str(), full_dst_path.c_str()); - - /* We have a file, is it a valid image */ - - FirmwareCommon fw; - - if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) - { - if (node_info.software_version.image_crc == 0 || - (node_info.software_version.major == 0 && - node_info.software_version.minor == 0) || - fw.descriptor.image_crc != - node_info.software_version.image_crc) - { - rv = true; - out_firmware_file_path = pfile->d_name; - } - break; - } - } - } - } - 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. - */ - __attribute__((optimize("O0"))) - virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, - const uavcan::protocol::file::BeginFirmwareUpdate::Response& error_response, - FirmwareFilePath& out_firmware_file_path) - { - return true; - - } - - /** - * 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(uavcan::NodeID node_id, - const uavcan::protocol::file::BeginFirmwareUpdate::Response& response) - { - (void)node_id; - (void)response; - } - - FirmwareVersionChecker() : - paths_(0) - { } - - virtual ~FirmwareVersionChecker() { } - - /** - * Initializes the Firmware File back-end storage by passing a paths object - * that maintains the directory where files will be stored. - */ - - int init(FirmwarePath& paths) - { - paths_ = &paths; - return 0; - } - - const char * getFirmwarePath() { return paths_->getFirmwareCachePath().c_str(); } - -private: - - - __attribute__((optimize("O0"))) - int copy_if_not(const char *srcpath, const char * destpath) - { - /* Does the file exits */ + // Does the file exist int rv = 0; int dfd = open(destpath, O_RDONLY, 0); if (dfd >= 0) { - /* Close it and exit 0 */ - close(dfd); + // Close it and exit 0 + (void)close(dfd); } else { uint8_t buffer[512]; - dfd = open(destpath, O_WRONLY | O_CREAT , FilePermissions); + dfd = open(destpath, O_WRONLY | O_CREAT, FilePermissions); if (dfd < 0) { rv = -errno; @@ -252,14 +84,156 @@ private: } } while (rv == 0 && size); - close(sfd); + + (void)close(sfd); } - close(dfd); + (void)close(dfd); } } 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) + { + /* 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[FirmwarePath::MaxBasePathLength + 1]; + int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", + paths_->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++] = FirmwarePath::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) + { + FirmwarePath::PathString full_src_path = fname_root; + full_src_path += pfile->d_name; + + FirmwarePath::PathString full_dst_path = paths_->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 + FirmwareCommon fw; + + if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) + { + if (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || + fw.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: + FirmwareVersionChecker() + : paths_(NULL) + { } + + /** + * Initializes the Firmware File back-end storage by passing a paths object + * that maintains the directory where files will be stored. + */ + int init(FirmwarePath& paths) + { + paths_ = &paths; + return 0; + } + + const char* getFirmwarePath() const + { + return paths_->getFirmwareCachePath().c_str(); + } }; + } #endif // Include guard From adbf0595850cb7ca86b6967473352a06d2098190 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:27:33 +0300 Subject: [PATCH 216/268] Fixes and notes in FirmwarePath --- .../include/uavcan_posix/firmware_path.hpp | 87 ++++++++----------- 1 file changed, 35 insertions(+), 52 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp index 31bb0baab4..a59ce1ec0e 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp @@ -17,18 +17,11 @@ namespace uavcan_posix { /** * Firmware Path Management. + * FIXME Seems like the only purpose of this class is to initialize some directory. It probably should be replaced to + * a member function inside the firmware version checker class. */ class FirmwarePath { - /* The folder where the files will be copied and Read from */ - - static const char* get_cache_dir_() - { - return "c"; - } - -public: - enum { MaxBasePathLength = 128 }; /** @@ -39,7 +32,7 @@ public: /** * Maximum length of full path including / the file name */ - enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; + enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; BasePathString base_path_; BasePathString cache_path_; @@ -49,34 +42,18 @@ public: */ typedef uavcan::MakeString::Type PathString; - static char getPathSeparator() { return static_cast(uavcan::protocol::file::Path::SEPARATOR); } + /** + * The folder where the files will be copied and read from + */ + static const char* getCacheDir() { return "c"; } - - BasePathString& getFirmwareBasePath() + static char getPathSeparator() { - return base_path_; + return static_cast(uavcan::protocol::file::Path::SEPARATOR); } - void setFirmwareBasePath(const char * path) - { - base_path_ = path; - } - - BasePathString& getFirmwareCachePath() - { - return cache_path_; - } - - void setFirmwareCachePath(const char *path) - { - cache_path_ = path; - - } - - static void addSlash(BasePathString& path) { - if (path.back() != getPathSeparator()) { path.push_back(getPathSeparator()); @@ -85,48 +62,40 @@ public: static void removeSlash(BasePathString& path) { - if (path.back() == getPathSeparator()) { path.pop_back(); } - } /** * Creates the Directories were the files will be stored - */ - - /* This is directory structure is in support of a workaround + * + * 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 cashed here. + * +-c <----------- Files are cached here. */ - - int CreateFwPaths(const char *base_path) + int createFwPaths(const char* base_path) { using namespace std; int rv = -uavcan::ErrInvalidParam; if (base_path) { - - int len = strlen(base_path); + const int len = strlen(base_path); if (len > 0 && len < base_path_.MaxSize) { - setFirmwareBasePath(base_path); removeSlash(getFirmwareBasePath()); - const char *path = getFirmwareBasePath().c_str(); + const char* path = getFirmwareBasePath().c_str(); setFirmwareCachePath(path); addSlash(cache_path_); - cache_path_ += get_cache_dir_(); + cache_path_ += getCacheDir(); rv = 0; struct stat sb; @@ -137,7 +106,7 @@ public: path = getFirmwareCachePath().c_str(); - if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))) + if (rv == 0 && (stat(path, &sb) != 0 || !S_ISDIR(sb.st_mode))) { rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); } @@ -145,8 +114,7 @@ public: addSlash(getFirmwareBasePath()); addSlash(getFirmwareCachePath()); - - if (rv >= 0 ) + if (rv >= 0) { if ((getFirmwareCachePath().size() + uavcan::protocol::file::Path::FieldTypes::path::MaxSize) > MaxPathLength) @@ -159,11 +127,26 @@ public: return rv; } - int init(const char *path) + void setFirmwareBasePath(const char* path) { - return CreateFwPaths(path); + base_path_ = path; } + void setFirmwareCachePath(const char* path) + { + cache_path_ = path; + } + +public: + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + + /// TODO Rename. Init is a bad name, as it not initializes the object, but modifies the FS. + int init(const char* path) + { + return createFwPaths(path); + } }; } From 3a07bde393c0815fabaa62a468415d2b11064ea7 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:38:14 +0300 Subject: [PATCH 217/268] Fixes and notes in FirmwareCommon --- .../include/uavcan_posix/firmware_common.hpp | 63 +++++++++---------- 1 file changed, 30 insertions(+), 33 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp index 33ebba88b8..bee2ff4190 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -10,10 +10,9 @@ #define UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED #include -#include #include #include -#include +#include #include #include @@ -23,36 +22,48 @@ namespace uavcan_posix { /** * Firmware file validation logic. + * TODO Rename - FirmwareCommon is a bad name as it doesn't reflect the purpose of this class. + * TODO Returning value via member variable is not a proper way of doing it. Probably the whole class should + * be replaced with a static function. */ class FirmwareCommon { -public: - - typedef struct app_descriptor_t + static uavcan::uint64_t getAppDescriptorSignature() { - uint8_t signature[sizeof(uint64_t)]; + uavcan::uint64_t ull = 0; + std::memcpy(&ull, "APDesc00", 8); + return ull; + } + +public: + 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]; - } app_descriptor_t; + }; - app_descriptor_t descriptor; + AppDescriptor descriptor; - int getFileInfo(const char *path) + int getFileInfo(const char* path) { - enum { MaxChunk = (512 / sizeof(uint64_t)) }; + using namespace std; + + const unsigned MaxChunk = 512 / sizeof(uint64_t); + int rv = -ENOENT; uint64_t chunk[MaxChunk]; int fd = open(path, O_RDONLY); if (fd >= 0) { - app_descriptor_t *pdescriptor = 0; + AppDescriptor* pdescriptor = NULL; - while(!pdescriptor) + while (pdescriptor == NULL) { int len = read(fd, chunk, sizeof(chunk)); @@ -67,42 +78,28 @@ public: goto out_close; } - uint64_t *p = &chunk[0]; + uint64_t* p = &chunk[0]; do { - if (*p == sig.ull) + if (*p == getAppDescriptorSignature()) { - pdescriptor = (app_descriptor_t *)p; + pdescriptor = (AppDescriptor*) p; descriptor = *pdescriptor; rv = 0; break; } } - while(p++ <= &chunk[MaxChunk - (sizeof(app_descriptor_t) / sizeof(chunk[0]))]); + while (p++ <= &chunk[MaxChunk - (sizeof(AppDescriptor) / sizeof(chunk[0]))]); } + out_close: - close(fd); + (void)close(fd); } return rv; } - - -private: - -#define APP_DESCRIPTOR_SIGNATURE_ID 'A', 'P', 'D', 'e', 's', 'c' -#define APP_DESCRIPTOR_SIGNATURE_REV '0', '0' -#define APP_DESCRIPTOR_SIGNATURE APP_DESCRIPTOR_SIGNATURE_ID, APP_DESCRIPTOR_SIGNATURE_REV - - union - { - uint64_t ull; - char text[sizeof(uint64_t)]; - } sig = { - .text = {APP_DESCRIPTOR_SIGNATURE} - }; - }; + } #endif // Include guard From fcca97d71c7036f01a34f2cfbf53bf8ffacd7f8c Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Tue, 26 May 2015 21:53:26 +0300 Subject: [PATCH 218/268] FirmwareCommon signature fix --- .../posix/include/uavcan_posix/firmware_common.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp index bee2ff4190..31a139349f 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp @@ -55,6 +55,8 @@ public: const unsigned MaxChunk = 512 / sizeof(uint64_t); + const uint64_t signature = getAppDescriptorSignature(); + int rv = -ENOENT; uint64_t chunk[MaxChunk]; int fd = open(path, O_RDONLY); @@ -82,7 +84,7 @@ public: do { - if (*p == getAppDescriptorSignature()) + if (*p == signature) { pdescriptor = (AppDescriptor*) p; descriptor = *pdescriptor; From 28802a38ddcb4e2213b4bac978ba81e956e9fb78 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 26 May 2015 14:00:49 -1000 Subject: [PATCH 219/268] removed firmware common, made GetInfo part of the firmware checke class fixed firmware path so that code compiles --- .../basic_file_server_backend.hpp | 90 ++++++++++++--- .../include/uavcan_posix/firmware_common.hpp | 107 ------------------ .../include/uavcan_posix/firmware_path.hpp | 67 +++++------ .../uavcan_posix/firmware_version_checker.hpp | 81 ++++++++++++- 4 files changed, 184 insertions(+), 161 deletions(-) delete mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 7a02ee11e1..c71a177bb9 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -23,8 +23,7 @@ #include #include #include - -#include "firmware_common.hpp" +#include namespace uavcan_posix { @@ -33,29 +32,74 @@ namespace uavcan_posix */ class BasicFileSeverBackend : public uavcan::IFileServerBackend { + enum { FilePermissions = 438 }; ///< 0o666 -protected: +public: /** + * * 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; + uavcan::DataTypeSignatureCRC crc; if (path.size() > 0) { - FirmwareCommon fw; - fw.getFileInfo(path.c_str()); - out_crc64 = fw.descriptor.image_crc; - out_size = fw.descriptor.image_size; - // 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 - out_type.flags = uavcan::protocol::file::EntryType::FLAG_FILE | - uavcan::protocol::file::EntryType::FLAG_READABLE; - rv = 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; } @@ -67,12 +111,16 @@ protected: * 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) @@ -81,30 +129,38 @@ protected: } 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); + //todo uses a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); - if (len < 0) + if (len < 0) { rv = errno; } else { - inout_size = len; + + inout_size = len; rv = 0; } } - (void)close(fd); + close(fd); } } return rv; } + + BasicFileSeverBackend() { } + + + + }; } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp deleted file mode 100644 index 31a139349f..0000000000 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_common.hpp +++ /dev/null @@ -1,107 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 PX4 Development Team. All rights reserved. -* Author: Pavel Kirienko -* David Sidrane -* -****************************************************************************/ - -#ifndef UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED -#define UAVCAN_POSIX_FIRMWARE_COMMON_HPP_INCLUDED - -#include -#include -#include -#include -#include - -#include -#include "firmware_path.hpp" - -namespace uavcan_posix -{ -/** - * Firmware file validation logic. - * TODO Rename - FirmwareCommon is a bad name as it doesn't reflect the purpose of this class. - * TODO Returning value via member variable is not a proper way of doing it. Probably the whole class should - * be replaced with a static function. - */ -class FirmwareCommon -{ - static uavcan::uint64_t getAppDescriptorSignature() - { - uavcan::uint64_t ull = 0; - std::memcpy(&ull, "APDesc00", 8); - return ull; - } - -public: - 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]; - }; - - AppDescriptor descriptor; - - int getFileInfo(const char* path) - { - using namespace std; - - const unsigned MaxChunk = 512 / sizeof(uint64_t); - - const uint64_t signature = getAppDescriptorSignature(); - - 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; - } -}; - -} - -#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp index a59ce1ec0e..4777a11a36 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp @@ -17,11 +17,14 @@ namespace uavcan_posix { /** * Firmware Path Management. - * FIXME Seems like the only purpose of this class is to initialize some directory. It probably should be replaced to - * a member function inside the firmware version checker class. + * This class manages the sole copies of the cache path and the base path of the fw + * This prevent having to have large stack allocations to manipulate the 2 paths. + * It ensures that the paths have the proper slash as endings for concatenation. + * It also provides the creation of the folders and encapsulates the paths to */ class FirmwarePath { +public: enum { MaxBasePathLength = 128 }; /** @@ -34,24 +37,28 @@ class FirmwarePath */ enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; - BasePathString base_path_; - BasePathString cache_path_; + static char getPathSeparator() + { + return static_cast(uavcan::protocol::file::Path::SEPARATOR); + } /** * This type is used internally for the full path to file */ typedef uavcan::MakeString::Type PathString; + + +private: + + BasePathString base_path_; + BasePathString cache_path_; + /** * The folder where the files will be copied and read from */ static const char* getCacheDir() { return "c"; } - static char getPathSeparator() - { - return static_cast(uavcan::protocol::file::Path::SEPARATOR); - } - static void addSlash(BasePathString& path) { if (path.back() != getPathSeparator()) @@ -68,6 +75,21 @@ class FirmwarePath } } + void setFirmwareBasePath(const char* path) + { + base_path_ = path; + } + + void setFirmwareCachePath(const char* path) + { + cache_path_ = path; + } + +public: + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + /** * Creates the Directories were the files will be stored * @@ -90,7 +112,7 @@ class FirmwarePath if (len > 0 && len < base_path_.MaxSize) { setFirmwareBasePath(base_path); - removeSlash(getFirmwareBasePath()); + removeSlash(base_path_); const char* path = getFirmwareBasePath().c_str(); setFirmwareCachePath(path); @@ -111,8 +133,8 @@ class FirmwarePath rv = mkdir(path, S_IRWXU | S_IRWXG | S_IRWXO); } - addSlash(getFirmwareBasePath()); - addSlash(getFirmwareCachePath()); + addSlash(base_path_); + addSlash(cache_path_); if (rv >= 0) { @@ -126,27 +148,6 @@ class FirmwarePath } return rv; } - - void setFirmwareBasePath(const char* path) - { - base_path_ = path; - } - - void setFirmwareCachePath(const char* path) - { - cache_path_ = path; - } - -public: - const BasePathString& getFirmwareBasePath() const { return base_path_; } - - const BasePathString& getFirmwareCachePath() const { return cache_path_; } - - /// TODO Rename. Init is a bad name, as it not initializes the object, but modifies the FS. - int init(const char* path) - { - return createFwPaths(path); - } }; } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index c53b6e2c08..beeb9ce8c1 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -17,8 +17,8 @@ #include #include +#include "firmware_path.hpp" -#include "firmware_common.hpp" namespace uavcan_posix { @@ -32,6 +32,18 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker FirmwarePath* paths_; + 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]; + }; + + int copyIfNot(const char* srcpath, const char* destpath) { // Does the file exist @@ -93,6 +105,61 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker return rv; } + __attribute__((optimize("O0"))) + 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; + } + protected: /** * This method will be invoked when the class obtains a response to GetNodeInfo request. @@ -108,6 +175,7 @@ protected: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ + __attribute__((optimize("O0"))) virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const uavcan::protocol::GetNodeInfo::Response& node_info, FirmwareFilePath& out_firmware_file_path) @@ -164,13 +232,18 @@ protected: int cr = copyIfNot(full_src_path.c_str(), full_dst_path.c_str()); // We have a file, is it a valid image - FirmwareCommon fw; + AppDescriptor descriptor; - if (cr == 0 && fw.getFileInfo(full_dst_path.c_str()) == 0) + 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) || - fw.descriptor.image_crc != node_info.software_version.image_crc) + descriptor.image_crc != node_info.software_version.image_crc) { rv = true; out_firmware_file_path = pfile->d_name; From 75b8831f21f5c789ef49f4d98bb381da9df57614 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 26 May 2015 14:21:16 -1000 Subject: [PATCH 220/268] Removed perfunction optimization setting --- .../posix/include/uavcan_posix/firmware_version_checker.hpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index beeb9ce8c1..cfeaa03ec5 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -105,7 +105,6 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker return rv; } - __attribute__((optimize("O0"))) static int getFileInfo(const char* path, AppDescriptor & descriptor) { using namespace std; @@ -175,7 +174,7 @@ protected: * @return True - the class will begin sending update requests. * False - the node will be ignored, no request will be sent. */ - __attribute__((optimize("O0"))) + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, const uavcan::protocol::GetNodeInfo::Response& node_info, FirmwareFilePath& out_firmware_file_path) From 89c8ed0cf152c8b77aeb95690276b2fbe0bdd4c5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 11:06:23 +0300 Subject: [PATCH 221/268] Docs for dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan --- .../server/220.AppendEntries.uavcan | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan index 329bfff8e6..30a966eb78 100644 --- a/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -4,11 +4,14 @@ # # -# Given min election timeout and cluster size, the maximum request interval can be derived as follows: -# max request interval = (min election timeout) / 2 requests / (cluster size - 1) -# Obviously, request interval can be lower than that if needed, but not higher. +# Given min election timeout and cluster size, the maximum recommended request interval can be derived as follows: +# max recommended request interval = (min election timeout) / 2 requests / (cluster size - 1) +# The equation assumes that the Leader requests one Follower at a time, so that there's at most one pending call +# at any moment. Such behavior is optimal as it creates uniform bus load, but it is actually implementation-specific. +# Obviously, request interval can be lower than that if needed, but higher values are not recommended as they may +# cause Followers to initiate premature elections in case of intensive frame losses or delays. # -# Real timeout is randomized in the range (MIN, MAX]. +# Real timeout is randomized in the range (MIN, MAX], according to the Raft paper. # uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000 uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000 @@ -19,8 +22,8 @@ uint8 prev_log_index uint8 leader_commit # -# Worst-case replication time can be computed as: -# worst replication time = (127 log entries) * (2 trips of next_index) * (cluster size - 1) * (request interval) +# Worst-case replication time per Follower can be computed as: +# worst replication time = (127 log entries) * (2 trips of next_index) * (request interval per Follower) # Entry[<=1] entries From 74298b1860ecee06773551bf5d300734772ea5fc Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 11:53:12 +0300 Subject: [PATCH 222/268] spinOnce(), fixes #31 --- .../include/uavcan/node/abstract_node.hpp | 12 +++++++ libuavcan/include/uavcan/node/scheduler.hpp | 19 +++++++++++ .../include/uavcan/transport/dispatcher.hpp | 8 +++++ libuavcan/src/node/uc_scheduler.cpp | 26 ++++++++++++-- libuavcan/src/transport/uc_dispatcher.cpp | 34 +++++++++++++++++++ libuavcan/test/transport/dispatcher.cpp | 4 +-- 6 files changed, 99 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/node/abstract_node.hpp b/libuavcan/include/uavcan/node/abstract_node.hpp index cebdcec7fc..7fddbe32b6 100644 --- a/libuavcan/include/uavcan/node/abstract_node.hpp +++ b/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(); + } }; } diff --git a/libuavcan/include/uavcan/node/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 0d1d82553c..01e58eb877 100644 --- a/libuavcan/include/uavcan/node/scheduler.hpp +++ b/libuavcan/include/uavcan/node/scheduler.hpp @@ -77,6 +77,17 @@ class UAVCAN_EXPORT Scheduler : Noncopyable MonotonicDuration cleanup_period_; bool inside_spin_; + struct InsideSpinSetter + { + Scheduler& owner; + InsideSpinSetter(Scheduler& o) + : owner(o) + { + owner.inside_spin_ = true; + } + ~InsideSpinSetter() { owner.inside_spin_ = false; } + }; + MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const; void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin); @@ -91,10 +102,18 @@ public: /** * Spin until the deadline, or until some error occurs. + * This function will return strictly when the deadline is reached, even if there are unprocessed frames. * Returns negative error code. */ int spin(MonotonicTime deadline); + /** + * Non-blocking version of @ref spin() - spins until all pending frames and events are processed, + * or until some error occurs. If there's nothing to do, returns immediately. + * Returns negative error code. + */ + int spinOnce(); + DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; } Dispatcher& getDispatcher() { return dispatcher_; } diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index a671f4ac85..0405d904fd 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/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 untill all available frames are processed. + */ + int spinOnce(); + /** * Refer to CanIOManager::send() for the parameter description */ diff --git a/libuavcan/src/node/uc_scheduler.cpp b/libuavcan/src/node/uc_scheduler.cpp index 94414f5f24..5901fe75c5 100644 --- a/libuavcan/src/node/uc_scheduler.cpp +++ b/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; } diff --git a/libuavcan/src/transport/uc_dispatcher.cpp b/libuavcan/src/transport/uc_dispatcher.cpp index b0ece03a7a..eb41b83e52 100644 --- a/libuavcan/src/transport/uc_dispatcher.cpp +++ b/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) { diff --git a/libuavcan/test/transport/dispatcher.cpp b/libuavcan/test/transport/dispatcher.cpp index 95bc5c359b..ce30738e21 100644 --- a/libuavcan/test/transport/dispatcher.cpp +++ b/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); From 75e2bed3c2ba5b63d2fd95cd93d6c459fabc60f5 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 11:56:18 +0300 Subject: [PATCH 223/268] Typo --- libuavcan/include/uavcan/transport/dispatcher.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index 0405d904fd..d179b32193 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -125,7 +125,7 @@ public: int spin(MonotonicTime deadline); /** - * This version does not return untill all available frames are processed. + * This version does not return until all available frames are processed. */ int spinOnce(); From 82c24967e77b5cd65bec360bd50d0ef3c9d1bc7f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 12:55:49 +0300 Subject: [PATCH 224/268] Minor fixes in Raft server --- .../protocol/dynamic_node_id_server/distributed/log.hpp | 8 -------- .../distributed/persistent_state.hpp | 8 ++++++++ .../dynamic_node_id_server/distributed/server.hpp | 1 - 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp index d829d293e0..20f957b3e7 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp +++ b/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_); diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp index 91869679c1..0152b1851e 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp +++ b/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() { /* diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 5fd35bb50a..98ed66eec5 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -7,7 +7,6 @@ #include #include -#include #include #include #include From 638de081150ef4f333accae5577057ae3d5cd682 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 14:52:41 +0300 Subject: [PATCH 225/268] CentralizedServer - storage implementation and test --- .../centralized/storage.hpp | 241 ++++++++++++++++++ .../centralized/storage.cpp | 115 +++++++++ 2 files changed, 356 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp new file mode 100644 index 0000000000..280e077f38 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -0,0 +1,241 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_STORAGE_HPP_INCLUDED + +#include +#include +#include +#include + +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; + + 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(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(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; + } + + 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 diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp new file mode 100644 index 0000000000..b2d4e83819 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -0,0 +1,115 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_centralized_Storage, Basic) +{ + 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); + } +} + + From a78c01593401287c64bfdf26c6f9ff163cf05228 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 15:16:17 +0300 Subject: [PATCH 226/268] Distributed server tracing fix --- .../protocol/dynamic_node_id_server/distributed/server.hpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 98ed66eec5..314fc40334 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -64,6 +64,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler * States */ INode& node_; + IEventTracer& tracer_; RaftCore raft_core_; AllocationRequestManager allocation_request_manager_; NodeDiscoverer node_discoverer_; @@ -229,6 +230,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler const int res = allocation_request_manager_.broadcastAllocationResponse(entry.unique_id, entry.node_id); if (res < 0) { + tracer_.onEvent(TraceError, res); node_.registerInternalFailure("Dynamic allocation final broadcast"); } } @@ -238,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) From abe2401e38a9a33ec44c0c02652c0ed11bdef9c8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 15:24:55 +0300 Subject: [PATCH 227/268] Distributed server logging fix --- .../protocol/dynamic_node_id_server/distributed/server.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp index 314fc40334..a8affdb8a2 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -231,7 +231,7 @@ class UAVCAN_EXPORT Server : IAllocationRequestHandler if (res < 0) { tracer_.onEvent(TraceError, res); - node_.registerInternalFailure("Dynamic allocation final broadcast"); + node_.registerInternalFailure("Dynamic allocation response"); } } From bdc0a327b702a31e01663511a3df9e16d14a686d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 16:10:14 +0300 Subject: [PATCH 228/268] centralized::Storage - new test --- .../centralized/storage.hpp | 14 ++++- .../centralized/storage.cpp | 61 ++++++++++++++++++- 2 files changed, 73 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp index 280e077f38..99e04eb183 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -25,13 +25,20 @@ class Storage public: typedef uint8_t Size; - enum { Capacity = NodeID::Max }; + enum { Capacity = NodeID::Max - 1 }; 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 && @@ -171,6 +178,11 @@ public: return -ErrLogic; } + if (!node_id.isUnicast()) + { + return -ErrInvalidParam; + } + Entry entry; entry.node_id = node_id; entry.unique_id = unique_id; diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp index b2d4e83819..615dc73f11 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -8,7 +8,7 @@ #include "../memory_storage_backend.hpp" -TEST(dynamic_node_id_server_centralized_Storage, Basic) +TEST(dynamic_node_id_server_centralized_Storage, Initialization) { using namespace uavcan::dynamic_node_id_server::centralized; @@ -113,3 +113,62 @@ TEST(dynamic_node_id_server_centralized_Storage, Basic) } +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(entry.node_id.get() + 1U); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, stor.add(entry.node_id, entry.unique_id)); // Failing because full + + storage.print(); +} From 5930bf3ed415255e4658c94a8e5da39597329909 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 16:10:38 +0300 Subject: [PATCH 229/268] CentralizedServer --- .../dynamic_node_id_server/centralized.hpp | 20 ++ .../centralized/server.hpp | 208 ++++++++++++++++++ .../centralized/server.cpp | 77 +++++++ 3 files changed, 305 insertions(+) create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp create mode 100644 libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp create mode 100644 libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp new file mode 100644 index 0000000000..e0f9e5bcae --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized.hpp @@ -0,0 +1,20 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED + +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ + +typedef centralized::Server CentralizedServer; + +} +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp new file mode 100644 index 0000000000..de9b99ac61 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/server.hpp @@ -0,0 +1,208 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_CENTRALIZED_SERVER_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +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(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 diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp new file mode 100644 index 0000000000..c1da6f09d1 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#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 _reg1; + uavcan::DefaultDataTypeRegistrator _reg2; + uavcan::DefaultDataTypeRegistrator _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; +} From 5205d13f37af06005b2023568eca7106d7276205 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 27 May 2015 16:28:31 +0300 Subject: [PATCH 230/268] centralized::Storage fix --- .../protocol/dynamic_node_id_server/centralized/storage.hpp | 2 +- .../protocol/dynamic_node_id_server/centralized/storage.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp index 99e04eb183..c03957d14f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -25,7 +25,7 @@ class Storage public: typedef uint8_t Size; - enum { Capacity = NodeID::Max - 1 }; + enum { Capacity = NodeID::Max }; struct Entry { diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp index 615dc73f11..51ada6d104 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -164,11 +164,11 @@ TEST(dynamic_node_id_server_centralized_Storage, Basic) { ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id)); - entry.node_id = uint8_t(entry.node_id.get() + 1U); + 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(entry.node_id, entry.unique_id)); // Failing because full + ASSERT_GT(0, stor.add(123, entry.unique_id)); // Failing because full storage.print(); } From be50b94cba0f73962fb6988ec16736a08f7a7634 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 May 2015 03:53:54 -1000 Subject: [PATCH 231/268] Revert of format changes --- .../basic_file_server_backend.hpp | 26 +++++-------------- 1 file changed, 6 insertions(+), 20 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index c71a177bb9..2ad056e7be 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -32,23 +32,18 @@ namespace uavcan_posix */ class BasicFileSeverBackend : public uavcan::IFileServerBackend { - enum { FilePermissions = 438 }; ///< 0o666 -public: +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; uavcan::DataTypeSignatureCRC crc; - if (path.size() > 0) { using namespace std; @@ -114,13 +109,10 @@ public: 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) @@ -129,38 +121,32 @@ public: } else { - if (::lseek(fd, offset, SEEK_SET) < 0) { rv = errno; } else { - //todo uses a read at offset to fill on EAGAIN - ssize_t len = ::read(fd, out_buffer, inout_size); + // TODO use a read at offset to fill on EAGAIN + ssize_t len = ::read(fd, out_buffer, inout_size); - if (len < 0) + if (len < 0) { rv = errno; } else { - inout_size = len; + inout_size = len; rv = 0; } } - close(fd); + (void)close(fd); } } return rv; } - BasicFileSeverBackend() { } - - - - }; } From 436cfcefa96f336eee89030224ff469acf621352 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 May 2015 04:32:50 -1000 Subject: [PATCH 232/268] Use FileCRC typedef --- .../posix/include/uavcan_posix/basic_file_server_backend.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 2ad056e7be..32226c3b38 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -43,7 +43,7 @@ protected: 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; - uavcan::DataTypeSignatureCRC crc; + FileCRC crc; if (path.size() > 0) { using namespace std; From f2fe415e55b31c0ca0e94793bbea3c4b37e40c92 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Wed, 27 May 2015 06:07:47 -1000 Subject: [PATCH 233/268] Code consolidation --- .../include/uavcan_posix/firmware_path.hpp | 155 ---------------- .../uavcan_posix/firmware_version_checker.hpp | 175 ++++++++++++++---- 2 files changed, 144 insertions(+), 186 deletions(-) delete mode 100644 libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp deleted file mode 100644 index 4777a11a36..0000000000 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_path.hpp +++ /dev/null @@ -1,155 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2015 PX4 Development Team. All rights reserved. -* Author: Pavel Kirienko -* David Sidrane -* -****************************************************************************/ - -#ifndef UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED -#define UAVCAN_POSIX_FIRMWARE_PATH_HPP_INCLUDED - -#include - -#include - -namespace uavcan_posix -{ -/** - * Firmware Path Management. - * This class manages the sole copies of the cache path and the base path of the fw - * This prevent having to have large stack allocations to manipulate the 2 paths. - * It ensures that the paths have the proper slash as endings for concatenation. - * It also provides the creation of the folders and encapsulates the paths to - */ -class FirmwarePath -{ -public: - enum { MaxBasePathLength = 128 }; - - /** - * This type is used for the base path - */ - typedef uavcan::MakeString::Type BasePathString; - - /** - * Maximum length of full path including / the file name - */ - enum { MaxPathLength = uavcan::protocol::file::Path::FieldTypes::path::MaxSize + MaxBasePathLength }; - - static char getPathSeparator() - { - return static_cast(uavcan::protocol::file::Path::SEPARATOR); - } - - /** - * This type is used internally for the full path to file - */ - typedef uavcan::MakeString::Type PathString; - - - -private: - - 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; - } - -public: - const BasePathString& getFirmwareBasePath() const { return base_path_; } - - const BasePathString& getFirmwareCachePath() const { return cache_path_; } - - /** - * 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; - } -}; - -} - -#endif // Include guard diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index cfeaa03ec5..d38bf1a1f2 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -17,8 +17,10 @@ #include #include -#include "firmware_path.hpp" +#if !defined(DIRENT_ISFILE) && defined(DT_REG) +# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) +#endif namespace uavcan_posix { @@ -30,19 +32,57 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { enum { FilePermissions = 438 }; ///< 0o666 - FirmwarePath* paths_; + enum { MaxBasePathLength = 128 }; - struct AppDescriptor + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::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::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) { - 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]; - }; + 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) { @@ -105,6 +145,18 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker 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; @@ -159,6 +211,78 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker return rv; } +public: + + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + + static char getPathSeparator() + { + return static_cast(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. @@ -179,6 +303,8 @@ protected: 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. @@ -196,9 +322,9 @@ protected: */ bool rv = false; - char fname_root[FirmwarePath::MaxBasePathLength + 1]; + char fname_root[MaxBasePathLength + 1]; int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", - paths_->getFirmwareBasePath().c_str(), + getFirmwareBasePath().c_str(), node_info.name.c_str(), node_info.hardware_version.major, node_info.hardware_version.minor); @@ -207,7 +333,7 @@ protected: { DIR* const fwdir = opendir(fname_root); - fname_root[n++] = FirmwarePath::getPathSeparator(); + fname_root[n++] = getPathSeparator(); fname_root[n++] = '\0'; if (fwdir != NULL) @@ -221,10 +347,10 @@ protected: // Open any bin file in there. if (strstr(pfile->d_name, ".bin") != NULL) { - FirmwarePath::PathString full_src_path = fname_root; + PathString full_src_path = fname_root; full_src_path += pfile->d_name; - FirmwarePath::PathString full_dst_path = paths_->getFirmwareCachePath().c_str(); + PathString full_dst_path = getFirmwareCachePath().c_str(); full_dst_path += pfile->d_name; // ease the burden on the user @@ -233,7 +359,7 @@ protected: // We have a file, is it a valid image AppDescriptor descriptor; - std::memset(&descriptor,0, sizeof(descriptor)); + std::memset(&descriptor, 0, sizeof(descriptor)); if (cr == 0 && getFileInfo(full_dst_path.c_str(), descriptor) == 0) { @@ -286,23 +412,10 @@ protected: } public: - FirmwareVersionChecker() - : paths_(NULL) - { } - - /** - * Initializes the Firmware File back-end storage by passing a paths object - * that maintains the directory where files will be stored. - */ - int init(FirmwarePath& paths) - { - paths_ = &paths; - return 0; - } const char* getFirmwarePath() const { - return paths_->getFirmwareCachePath().c_str(); + return getFirmwareCachePath().c_str(); } }; From 34bcfb21df0097839b42cf265d7861956f955deb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 May 2015 11:57:00 +0300 Subject: [PATCH 234/268] Improved docs for IFileServerBackend --- libuavcan/include/uavcan/protocol/file_server.hpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index ba458cefad..1e5fed04a2 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -47,6 +47,7 @@ public: /** * 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. */ @@ -90,6 +91,7 @@ public: /** * 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. */ From 954ab2491a93456380ee955e691cf4cf3aad5079 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 May 2015 12:50:12 +0300 Subject: [PATCH 235/268] Fruitless attempt to optimize memory use --- .../dynamic_node_id_server/node_discoverer.hpp | 2 +- .../dynamic_node_id_server/centralized/server.cpp | 1 + .../dynamic_node_id_server/node_discoverer.cpp | 13 +++++++++++++ 3 files changed, 15 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp index 827551250c..e5e4da5e1f 100644 --- a/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -106,7 +106,7 @@ class NodeDiscoverer : TimerBase NodeMap node_map_; ///< Will not work in UAVCAN_TINY ServiceClient get_node_info_client_; - Subscriber node_status_sub_; + Subscriber node_status_sub_; /* * Methods diff --git a/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp index c1da6f09d1..5e83b19d05 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -74,4 +74,5 @@ 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; } diff --git a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp index 304070bf3f..7313bf0b0a 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -275,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: " << sizeof(BitSet) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "protocol::GetNodeInfo::Response: " << sizeof(protocol::GetNodeInfo::Response) << std::endl; + std::cout << "Subscriber: " + << sizeof(Subscriber&), 64, 0>) << std::endl; +} From d4e49d518a13c01bf15c6c34f6e64bff1fec80a8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 May 2015 13:49:01 +0300 Subject: [PATCH 236/268] First step towards introducing the global RX object buffer --- .../include/uavcan/node/marshal_buffer.hpp | 11 ++++--- libuavcan/include/uavcan/node/node.hpp | 31 ++++++++++--------- libuavcan/src/node/uc_generic_publisher.cpp | 4 +-- libuavcan/test/node/test_node.hpp | 2 +- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index 35a7e0885a..3dad463a83 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -18,7 +18,8 @@ class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer { public: virtual const uint8_t* getDataPtr() const = 0; - virtual unsigned getDataLength() const = 0; + virtual unsigned getMaxWritePos() const = 0; + virtual unsigned getSize() const = 0; }; /** @@ -43,10 +44,8 @@ public: /** * Default implementation of marshal buffer provider. - * This implementation provides the buffer large enough to - * serialize any UAVCAN data structure. */ -template +template class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider { class Buffer : public IMarshalBuffer @@ -65,7 +64,9 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } - virtual unsigned getDataLength() const { return buf_.getMaxWritePos(); } + virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } + + virtual unsigned getSize() const { return MaxSize_; } public: void reset() { buf_.reset(); } diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 65b478b7a7..5175bd2115 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -47,21 +47,24 @@ namespace uavcan * be allocated in the memory pool if needed. * Default value is acceptable for any use case. * - * @tparam OutgoingTransferMaxPayloadLen Maximum outgoing transfer payload length. - * It's pointless to make this value larger than - * @ref MaxTransferPayloadLen, which is default. - * Note that in tiny mode the default value is actually - * smaller than @ref MaxTransferPayloadLen (may cause - * run-time failures). + * @tparam MarshalBufferSize Size of the marshal buffer, that is used to provide short-term temporary storage for: + * 1. Serialized data for TX transfers; + * 2. De-serialized data for RX transfers. + * The buffer must be large enough to accommodate largest serialized TX transfer and + * largest de-serialized RX data structure. The former value is constant for UAVCAN, the + * latter is platform-dependent (depends on the field padding, memory alignment, pointer + * size, etc.). + * The default value should be enough for all use cases on virtually all platforms. + * If this value is not large enough, transport objects (such as Subscriber<>, + * Publisher<>, Service*<>) will be failing at run time during initialization. */ template class UAVCAN_EXPORT Node : public INode { @@ -73,7 +76,7 @@ class UAVCAN_EXPORT Node : public INode typedef PoolAllocator Allocator; Allocator pool_allocator_; - MarshalBufferProvider marsh_buf_; + MarshalBufferProvider marsh_buf_; OutgoingTransferRegistry outgoing_trans_reg_; Scheduler scheduler_; @@ -265,9 +268,8 @@ public: // ---------------------------------------------------------------------------- -template -int Node::start( +template +int Node::start( const TransferPriority priority) { if (started_) @@ -313,9 +315,8 @@ fail: #if !UAVCAN_TINY -template -int Node:: +template +int Node:: checkNetworkCompatibility(NetworkCompatibilityCheckResult& result) { if (!started_) diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp index 3ebc7bd699..09f361f5b0 100644 --- a/libuavcan/src/node/uc_generic_publisher.cpp +++ b/libuavcan/src/node/uc_generic_publisher.cpp @@ -47,12 +47,12 @@ int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferT { if (tid) { - return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), + return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), blocking_deadline, transfer_type, dst_node_id, *tid); } else { - return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), + return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), blocking_deadline, transfer_type, dst_node_id); } } diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 543f99a50d..5a943ae6da 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -20,7 +20,7 @@ struct TestNode : public uavcan::INode { uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; - uavcan::MarshalBufferProvider<> buffer_provider; + uavcan::MarshalBufferProvider<512> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; uavcan::Scheduler scheduler; uint64_t internal_failure_count; From 4191710febbf8ae426333515604ffa072d06a9a6 Mon Sep 17 00:00:00 2001 From: Riccardo Miccini Date: Thu, 28 May 2015 17:53:19 +0200 Subject: [PATCH 237/268] no data type info provider for TINY --- libuavcan/include/uavcan/node/node.hpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 65b478b7a7..ac8ca8b6aa 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -13,10 +13,10 @@ #include // High-level functionality available by default -#include #include #if !UAVCAN_TINY +#include //moved to save space # include # include # include @@ -77,9 +77,9 @@ class UAVCAN_EXPORT Node : public INode OutgoingTransferRegistry outgoing_trans_reg_; Scheduler scheduler_; - DataTypeInfoProvider proto_dtp_; NodeStatusProvider proto_nsp_; #if !UAVCAN_TINY + DataTypeInfoProvider proto_dtp_; // moved to save space Logger proto_logger_; RestartRequestServer proto_rrs_; TransportStatsProvider proto_tsp_; @@ -106,9 +106,9 @@ public: Node(ICanDriver& can_driver, ISystemClock& system_clock) : outgoing_trans_reg_(pool_allocator_) , scheduler_(can_driver, pool_allocator_, system_clock, outgoing_trans_reg_) - , proto_dtp_(*this) , proto_nsp_(*this) #if !UAVCAN_TINY + , proto_dtp_(*this) // moved , proto_logger_(*this) , proto_rrs_(*this) , proto_tsp_(*this) @@ -277,17 +277,17 @@ int Node Date: Thu, 28 May 2015 18:00:03 +0200 Subject: [PATCH 238/268] no data type info provider for TINY --- libuavcan/include/uavcan/node/node.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index ac8ca8b6aa..878a365082 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -16,7 +16,7 @@ #include #if !UAVCAN_TINY -#include //moved to save space +#include # include # include # include @@ -79,7 +79,7 @@ class UAVCAN_EXPORT Node : public INode NodeStatusProvider proto_nsp_; #if !UAVCAN_TINY - DataTypeInfoProvider proto_dtp_; // moved to save space + DataTypeInfoProvider proto_dtp_; Logger proto_logger_; RestartRequestServer proto_rrs_; TransportStatsProvider proto_tsp_; @@ -108,7 +108,7 @@ public: , scheduler_(can_driver, pool_allocator_, system_clock, outgoing_trans_reg_) , proto_nsp_(*this) #if !UAVCAN_TINY - , proto_dtp_(*this) // moved + , proto_dtp_(*this) , proto_logger_(*this) , proto_rrs_(*this) , proto_tsp_(*this) @@ -283,7 +283,7 @@ int Node Date: Thu, 28 May 2015 18:10:42 +0200 Subject: [PATCH 239/268] no data type info provider for TINY --- libuavcan/include/uavcan/node/node.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 878a365082..0197241f62 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -16,7 +16,7 @@ #include #if !UAVCAN_TINY -#include +# include # include # include # include From fe3fce49964e147aed27a2159286435c80844294 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 03:34:37 +0300 Subject: [PATCH 240/268] CanIOManager::makePendingTxMask() made public --- libuavcan/include/uavcan/transport/can_io.hpp | 3 ++- libuavcan/src/transport/uc_can_io.cpp | 26 +++++++++---------- 2 files changed, 15 insertions(+), 14 deletions(-) diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index 867407d056..0cc4e79761 100644 --- a/libuavcan/include/uavcan/transport/can_io.hpp +++ b/libuavcan/include/uavcan/transport/can_io.hpp @@ -154,7 +154,6 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags); int sendFromTxQueue(uint8_t iface_index); - uint8_t makePendingTxMask() const; int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline); public: @@ -168,6 +167,8 @@ public: const ICanDriver& getCanDriver() const { return driver_; } ICanDriver& getCanDriver() { return driver_; } + uint8_t makePendingTxMask() const; + /** * Returns: * 0 - rejected/timedout/enqueued diff --git a/libuavcan/src/transport/uc_can_io.cpp b/libuavcan/src/transport/uc_can_io.cpp index 0195eb2280..210ea3822e 100644 --- a/libuavcan/src/transport/uc_can_io.cpp +++ b/libuavcan/src/transport/uc_can_io.cpp @@ -261,19 +261,6 @@ int CanIOManager::sendFromTxQueue(uint8_t iface_index) return res; } -uint8_t CanIOManager::makePendingTxMask() const -{ - uint8_t write_mask = 0; - for (uint8_t i = 0; i < getNumIfaces(); i++) - { - if (!tx_queues_[i]->isEmpty()) - { - write_mask |= uint8_t(1 << i); - } - } - return write_mask; -} - int CanIOManager::callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline) { const CanSelectMasks in_masks = inout_masks; @@ -312,6 +299,19 @@ CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISyste } } +uint8_t CanIOManager::makePendingTxMask() const +{ + uint8_t write_mask = 0; + for (uint8_t i = 0; i < getNumIfaces(); i++) + { + if (!tx_queues_[i]->isEmpty()) + { + write_mask |= uint8_t(1 << i); + } + } + return write_mask; +} + CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const { ICanIface* const iface = driver_.getIface(iface_index); From d20f8e7356def145b7f70d7a9fd913018bd493ba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 16:47:02 +0300 Subject: [PATCH 241/268] Using global RX object buffer --- libuavcan/include/uavcan/error.hpp | 1 + .../uavcan/node/generic_subscriber.hpp | 130 +++++++++++++----- .../include/uavcan/node/marshal_buffer.hpp | 6 +- .../include/uavcan/node/service_client.hpp | 12 +- libuavcan/test/node/service_client.cpp | 7 +- .../test/protocol/data_type_info_provider.cpp | 3 +- libuavcan/test/protocol/helpers.hpp | 39 +++++- 7 files changed, 152 insertions(+), 46 deletions(-) diff --git a/libuavcan/include/uavcan/error.hpp b/libuavcan/include/uavcan/error.hpp index 4f67e9a6ec..db9b3992e8 100644 --- a/libuavcan/include/uavcan/error.hpp +++ b/libuavcan/include/uavcan/error.hpp @@ -35,6 +35,7 @@ const int16_t ErrLogic = 10; const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type const int16_t ErrInvalidConfiguration = 13; +const int16_t ErrBufferTooSmall = 14; ///< Statically allocated buffer is not large enough /** * @} */ diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 235f5518df..5b958f0a97 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -40,7 +40,6 @@ class UAVCAN_EXPORT ReceivedDataStructure : public DataType_ { if (_transfer_ == NULL) { - UAVCAN_ASSERT(0); return Ret(); } return (_transfer_->*Fun)(); @@ -162,23 +161,71 @@ class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase { } }; - struct ReceivedDataStructureSpec : public ReceivedDataStructure - { - using ReceivedDataStructure::setTransfer; - }; - LazyConstructor forwarder_; - ReceivedDataStructureSpec message_; int checkInit(); - bool decodeTransfer(IncomingTransfer& transfer); - void handleIncomingTransfer(IncomingTransfer& transfer); int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase*)); + /* + * Received object and its allocators + */ + struct ReceivedDataStructureSpec : public ReceivedDataStructure + { + using ReceivedDataStructure::setTransfer; + }; + + class StructBufferStatic + { + ReceivedDataStructureSpec object_; + + public: + explicit StructBufferStatic(GenericSubscriber&) { } + + ReceivedDataStructureSpec* getObjectPtr() { return &object_; } + }; + + class StructBufferShared + { + ReceivedDataStructureSpec* object_; + + public: + explicit StructBufferShared(GenericSubscriber& owner) + : object_(NULL) + { + IMarshalBuffer* const buf = + owner.getNode().getMarshalBufferProvider().getBuffer(sizeof(ReceivedDataStructureSpec)); + if (buf != NULL && buf->getSize() >= sizeof(ReceivedDataStructureSpec)) + { + object_ = new (buf->getDataPtr()) ReceivedDataStructureSpec; + } + } + + /* + * Destructor MUST NOT be invoked + * TODO explain why + */ + + ReceivedDataStructureSpec* getObjectPtr() { return object_; } + }; + protected: + /** + * This value defines the threshold for stack allocation. See the typedef below. + */ + enum { MaxObjectSizeForStackAllocation = 80 }; + + /** + * This type resolves to either type of buffer, depending on the object size: + * - if the object is small, it will be allocated on the stack (StructBufferStatic); + * - if the object is large, it will be allocated in the shared buffer (StructBufferShared). + */ + typedef typename Select<(sizeof(DataStruct) > MaxObjectSizeForStackAllocation), + StructBufferShared, + StructBufferStatic>::Result ReceivedDataStructureBuffer; + explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) { } @@ -227,15 +274,6 @@ protected: } TransferListenerType* getTransferListener() { return forwarder_; } - - /** - * Returns a reference to the temporary storage for decoded received messages. - * Reference to this storage is used as a parameter for subscription callbacks. - * This storage is guaranteed to stay intact after the last message was decoded, i.e. - * the application can use it to access the last received message object. - */ - ReceivedDataStructure& getReceivedStructStorage() { return message_; } - const ReceivedDataStructure& getReceivedStructStorage() const { return message_; } }; // ---------------------------------------------------------------------------- @@ -250,48 +288,76 @@ int GenericSubscriber::checkInit() { return 0; } + + /* + * Making sure that the buffer is large enough + * This check MUST be performed BEFORE initialization, otherwise we may encounter some terrible runtime failures. + */ + if (ReceivedDataStructureBuffer(*this).getObjectPtr() == NULL) + { + return -ErrBufferTooSmall; + } + + /* + * Initializing the transfer forwarder + */ GlobalDataTypeRegistry::instance().freeze(); const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName()); - if (!descr) + if (descr == NULL) { UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName()); return -ErrUnknownDataType; } + forwarder_.template construct (*this, *descr, node_.getAllocator()); + return 0; } template -bool GenericSubscriber::decodeTransfer(IncomingTransfer& transfer) +void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) { + /* + * Initializing the temporary RX structure + */ + ReceivedDataStructureBuffer rx_struct_buffer(*this); + + if (rx_struct_buffer.getObjectPtr() == NULL) + { + UAVCAN_ASSERT(0); // The init method should have caught this error. + failure_count_++; + node_.getDispatcher().getTransferPerfCounter().addError(); + return; + } + + rx_struct_buffer.getObjectPtr()->setTransfer(&transfer); + + /* + * Decoding into the temporary storage + */ BitStream bitstream(transfer); ScalarCodec codec(bitstream); - message_.setTransfer(&transfer); + const int decode_res = DataStruct::decode(*rx_struct_buffer.getObjectPtr(), codec); - const int decode_res = DataStruct::decode(message_, codec); // We don't need the data anymore, the memory can be reused from the callback: transfer.release(); + if (decode_res <= 0) { UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]", decode_res, DataSpec::getDataTypeFullName()); failure_count_++; node_.getDispatcher().getTransferPerfCounter().addError(); - return false; + return; } - return true; -} -template -void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) -{ - if (decodeTransfer(transfer)) - { - handleReceivedDataStruct(message_); - } + /* + * Invoking the callback + */ + handleReceivedDataStruct(*rx_struct_buffer.getObjectPtr()); } template diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index 3dad463a83..e90a9b396f 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -17,9 +17,11 @@ namespace uavcan class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer { public: - virtual const uint8_t* getDataPtr() const = 0; + virtual uint8_t* getDataPtr() = 0; virtual unsigned getMaxWritePos() const = 0; virtual unsigned getSize() const = 0; + + const uint8_t* getDataPtr() const { return const_cast(this)->getDataPtr(); } }; /** @@ -62,7 +64,7 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider return buf_.write(offset, data, len); } - virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } + virtual uint8_t* getDataPtr() { return buf_.getRawPtr(); } virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b8467c4694..e166c07420 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -61,7 +61,7 @@ struct ServiceCallID * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. */ template -class UAVCAN_EXPORT ServiceCallResult +class UAVCAN_EXPORT ServiceCallResult : Noncopyable { public: typedef ReceivedDataStructure ResponseFieldType; @@ -257,8 +257,16 @@ private: 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()); + + typename SubscriberType::ReceivedDataStructureBuffer rx_struct_buffer(owner); + if (rx_struct_buffer.getObjectPtr() == NULL) + { + handleFatalError("RX obj buf"); + } + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), - owner.getReceivedStructStorage()); // Mutable! + *rx_struct_buffer.getObjectPtr()); // Mutable! + owner.invokeCallback(result); } } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 82d99a66ac..8306ea1c16 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -178,8 +178,7 @@ TEST(ServiceClient, Basic) ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :( // Validating - // 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)); + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); // Stray request ASSERT_LT(0, client3.call(99, request)); // Will timeout! @@ -341,7 +340,7 @@ TEST(ServiceClient, ConcurrentCalls) 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)); + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, root_ns_a::StringService::Response())); /* * Validating the 500 ms timeout @@ -352,7 +351,7 @@ TEST(ServiceClient, ConcurrentCalls) 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)); + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); } diff --git a/libuavcan/test/protocol/data_type_info_provider.cpp b/libuavcan/test/protocol/data_type_info_provider.cpp index 1780d5f801..fc1bd40cc3 100644 --- a/libuavcan/test/protocol/data_type_info_provider.cpp +++ b/libuavcan/test/protocol/data_type_info_provider.cpp @@ -19,7 +19,8 @@ using uavcan::DefaultDataTypeRegistrator; using uavcan::MonotonicDuration; template -static bool validateDataTypeInfoResponse(const std::auto_ptr >& resp, unsigned mask) +static bool validateDataTypeInfoResponse(const std::auto_ptr::Result>& resp, + unsigned mask) { if (!resp.get()) { diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index e0fb164f2f..3c2bc6c7ec 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -51,18 +51,47 @@ struct SubscriberWithCollector template class ServiceCallResultCollector : uavcan::Noncopyable { - typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceCallResult ServiceCallResult; - void handler(const ResultType& result) +public: + class Result { - this->result.reset(new ResultType(result)); + const typename ServiceCallResult::Status status_; + uavcan::ServiceCallID call_id_; + typename DataType::Response response_; + + public: + Result(typename ServiceCallResult::Status arg_status, + uavcan::ServiceCallID arg_call_id, + const typename DataType::Response& arg_response) + : status_(arg_status) + , call_id_(arg_call_id) + , response_(arg_response) + { } + + bool isSuccessful() const { return status_ == ServiceCallResult::Success; } + + typename ServiceCallResult::Status getStatus() const { return status_; } + + uavcan::ServiceCallID getCallID() const { return call_id_; } + + const typename DataType::Response& getResponse() const { return response_; } + typename DataType::Response& getResponse() { return response_; } + }; + +private: + void handler(const uavcan::ServiceCallResult& tmp_result) + { + std::cout << tmp_result << std::endl; + result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse())); } public: - std::auto_ptr result; + std::auto_ptr result; typedef uavcan::MethodBinder Binder; + void (ServiceCallResultCollector::*)(const uavcan::ServiceCallResult&)> + Binder; Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); } }; From af09237dd22a6ab9b3feebc69fd7e1004db5db5b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 22:04:19 +0300 Subject: [PATCH 242/268] Stack-allocating RX objects --- libuavcan/include/uavcan/error.hpp | 1 - .../uavcan/node/generic_subscriber.hpp | 100 +++--------------- .../include/uavcan/node/marshal_buffer.hpp | 9 +- libuavcan/include/uavcan/node/node.hpp | 19 ++-- .../include/uavcan/node/service_client.hpp | 9 +- 5 files changed, 28 insertions(+), 110 deletions(-) diff --git a/libuavcan/include/uavcan/error.hpp b/libuavcan/include/uavcan/error.hpp index db9b3992e8..4f67e9a6ec 100644 --- a/libuavcan/include/uavcan/error.hpp +++ b/libuavcan/include/uavcan/error.hpp @@ -35,7 +35,6 @@ const int16_t ErrLogic = 10; const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type const int16_t ErrInvalidConfiguration = 13; -const int16_t ErrBufferTooSmall = 14; ///< Statically allocated buffer is not large enough /** * @} */ diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 5b958f0a97..b749f8614e 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -33,7 +33,7 @@ namespace uavcan template class UAVCAN_EXPORT ReceivedDataStructure : public DataType_ { - const IncomingTransfer* _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields + const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields template Ret safeget() const @@ -46,12 +46,14 @@ class UAVCAN_EXPORT ReceivedDataStructure : public DataType_ } protected: - ReceivedDataStructure() : _transfer_(NULL) { } + ReceivedDataStructure() + : _transfer_(NULL) + { } - void setTransfer(const IncomingTransfer* arg_transfer) + ReceivedDataStructure(const IncomingTransfer* arg_transfer) + : _transfer_(arg_transfer) { UAVCAN_ASSERT(arg_transfer != NULL); - _transfer_ = arg_transfer; } public: @@ -169,63 +171,16 @@ class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase*)); - /* - * Received object and its allocators - */ +protected: struct ReceivedDataStructureSpec : public ReceivedDataStructure { - using ReceivedDataStructure::setTransfer; + ReceivedDataStructureSpec() { } + + ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) + : ReceivedDataStructure(arg_transfer) + { } }; - class StructBufferStatic - { - ReceivedDataStructureSpec object_; - - public: - explicit StructBufferStatic(GenericSubscriber&) { } - - ReceivedDataStructureSpec* getObjectPtr() { return &object_; } - }; - - class StructBufferShared - { - ReceivedDataStructureSpec* object_; - - public: - explicit StructBufferShared(GenericSubscriber& owner) - : object_(NULL) - { - IMarshalBuffer* const buf = - owner.getNode().getMarshalBufferProvider().getBuffer(sizeof(ReceivedDataStructureSpec)); - if (buf != NULL && buf->getSize() >= sizeof(ReceivedDataStructureSpec)) - { - object_ = new (buf->getDataPtr()) ReceivedDataStructureSpec; - } - } - - /* - * Destructor MUST NOT be invoked - * TODO explain why - */ - - ReceivedDataStructureSpec* getObjectPtr() { return object_; } - }; - -protected: - /** - * This value defines the threshold for stack allocation. See the typedef below. - */ - enum { MaxObjectSizeForStackAllocation = 80 }; - - /** - * This type resolves to either type of buffer, depending on the object size: - * - if the object is small, it will be allocated on the stack (StructBufferStatic); - * - if the object is large, it will be allocated in the shared buffer (StructBufferShared). - */ - typedef typename Select<(sizeof(DataStruct) > MaxObjectSizeForStackAllocation), - StructBufferShared, - StructBufferStatic>::Result ReceivedDataStructureBuffer; - explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) { } @@ -289,18 +244,6 @@ int GenericSubscriber::checkInit() return 0; } - /* - * Making sure that the buffer is large enough - * This check MUST be performed BEFORE initialization, otherwise we may encounter some terrible runtime failures. - */ - if (ReceivedDataStructureBuffer(*this).getObjectPtr() == NULL) - { - return -ErrBufferTooSmall; - } - - /* - * Initializing the transfer forwarder - */ GlobalDataTypeRegistry::instance().freeze(); const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName()); @@ -319,20 +262,7 @@ int GenericSubscriber::checkInit() template void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) { - /* - * Initializing the temporary RX structure - */ - ReceivedDataStructureBuffer rx_struct_buffer(*this); - - if (rx_struct_buffer.getObjectPtr() == NULL) - { - UAVCAN_ASSERT(0); // The init method should have caught this error. - failure_count_++; - node_.getDispatcher().getTransferPerfCounter().addError(); - return; - } - - rx_struct_buffer.getObjectPtr()->setTransfer(&transfer); + ReceivedDataStructureSpec rx_struct(&transfer); /* * Decoding into the temporary storage @@ -340,7 +270,7 @@ void GenericSubscriber::handleIncomi BitStream bitstream(transfer); ScalarCodec codec(bitstream); - const int decode_res = DataStruct::decode(*rx_struct_buffer.getObjectPtr(), codec); + const int decode_res = DataStruct::decode(rx_struct, codec); // We don't need the data anymore, the memory can be reused from the callback: transfer.release(); @@ -357,7 +287,7 @@ void GenericSubscriber::handleIncomi /* * Invoking the callback */ - handleReceivedDataStruct(*rx_struct_buffer.getObjectPtr()); + handleReceivedDataStruct(rx_struct); } template diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index e90a9b396f..353233bc33 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -17,11 +17,8 @@ namespace uavcan class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer { public: - virtual uint8_t* getDataPtr() = 0; + virtual const uint8_t* getDataPtr() const = 0; virtual unsigned getMaxWritePos() const = 0; - virtual unsigned getSize() const = 0; - - const uint8_t* getDataPtr() const { return const_cast(this)->getDataPtr(); } }; /** @@ -64,12 +61,10 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider return buf_.write(offset, data, len); } - virtual uint8_t* getDataPtr() { return buf_.getRawPtr(); } + virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } - virtual unsigned getSize() const { return MaxSize_; } - public: void reset() { buf_.reset(); } }; diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index d12cec6558..9cc2773c5c 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -47,24 +47,21 @@ namespace uavcan * be allocated in the memory pool if needed. * Default value is acceptable for any use case. * - * @tparam MarshalBufferSize Size of the marshal buffer, that is used to provide short-term temporary storage for: - * 1. Serialized data for TX transfers; - * 2. De-serialized data for RX transfers. - * The buffer must be large enough to accommodate largest serialized TX transfer and - * largest de-serialized RX data structure. The former value is constant for UAVCAN, the - * latter is platform-dependent (depends on the field padding, memory alignment, pointer - * size, etc.). - * The default value should be enough for all use cases on virtually all platforms. - * If this value is not large enough, transport objects (such as Subscriber<>, - * Publisher<>, Service*<>) will be failing at run time during initialization. + * @tparam MarshalBufferSize Size of the marshal buffer that is used to provide short-term temporary storage for + * serialized data for TX transfers. The buffer must be large enough to accommodate + * largest serialized TX transfer. The default value is guaranteed to be large enough, + * but it can be reduced if long TX transfers are not used to optimize memory use. + * If UAVCAN_TINY mode is enabled, this value defaults to the maximum length of a + * response transfer of uavcan.protocol.GetNodeInfo. */ template ::Result #else unsigned OutgoingTransferRegistryStaticEntries = 10, + unsigned MarshalBufferSize = MaxPossibleTransferPayloadLen #endif - unsigned MarshalBufferSize = 512 > class UAVCAN_EXPORT Node : public INode { diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index e166c07420..35e38cd535 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -59,6 +59,7 @@ struct ServiceCallID /** * Object of this type will be returned to the application as a result of service call. * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. + * The class is made noncopyable because it keeps a reference to a stack-allocated object. */ template class UAVCAN_EXPORT ServiceCallResult : Noncopyable @@ -258,14 +259,10 @@ private: int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()), DataType::getDataTypeFullName()); - typename SubscriberType::ReceivedDataStructureBuffer rx_struct_buffer(owner); - if (rx_struct_buffer.getObjectPtr() == NULL) - { - handleFatalError("RX obj buf"); - } + typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), - *rx_struct_buffer.getObjectPtr()); // Mutable! + rx_struct); // Mutable! owner.invokeCallback(result); } From d8c096430f33e2a57448a3a6b026e01a588f38b9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 22:23:41 +0300 Subject: [PATCH 243/268] Added some comments concerning stack allocation and references --- libuavcan/include/uavcan/node/generic_subscriber.hpp | 5 ++++- libuavcan/include/uavcan/node/service_client.hpp | 7 +++++++ libuavcan/include/uavcan/node/service_server.hpp | 3 +++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index b749f8614e..bb203788c6 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -140,7 +140,10 @@ public: typedef TransferListenerTemplate Type; }; - +/** + * Please note that the reference passed to the RX callback points to a stack-allocated object, which means + * that it gets invalidated shortly after the callback returns. + */ template class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase { diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 35e38cd535..97d0a55fb3 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -93,6 +93,9 @@ public: ServiceCallID getCallID() const { return call_id_; } + /** + * Returned reference points to a stack-allocated object. + */ const ResponseFieldType& getResponse() const { return response_; } ResponseFieldType& getResponse() { return response_; } }; @@ -201,6 +204,10 @@ public: * 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. * + * Note that the reference passed to the callback points to a stack-allocated object, which means that the + * reference invalidates once the callback returns. If you want to use this object after the callback execution, + * you need to copy it somewhere. + * * @tparam DataType_ Service data type. * * @tparam Callback_ Service response will be delivered through the callback of this type. diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index 1602e6b70f..d7181fd0d0 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -64,6 +64,9 @@ public: /** * Use this class to implement UAVCAN service servers. * + * Note that the references passed to the callback may point to stack-allocated objects, which means that the + * references get invalidated once the callback returns. + * * @tparam DataType_ Service data type. * * @tparam Callback_ Service calls will be delivered through the callback of this type, and service From 3499db227b1c0fffce3068fdda481036915c2429 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 22:36:16 +0300 Subject: [PATCH 244/268] Stack-allocating the service response structure --- libuavcan/include/uavcan/node/service_server.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index d7181fd0d0..17076b4c5a 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -120,26 +120,26 @@ private: PublisherType publisher_; Callback callback_; uint32_t response_failure_count_; - ServiceResponseDataStructure response_; virtual void handleReceivedDataStruct(ReceivedDataStructure& request) { UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); + + ServiceResponseDataStructure response; + if (try_implicit_cast(callback_, true)) { - // The application needs newly initialized structure - response_ = ServiceResponseDataStructure(); - UAVCAN_ASSERT(response_.isResponseEnabled()); // Enabled by default - callback_(request, response_); + UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default + callback_(request, response); } else { handleFatalError("Srv serv clbk"); } - if (response_.isResponseEnabled()) + if (response.isResponseEnabled()) { - const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(), + const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), request.getTransferID()); if (res < 0) { From 0ce23a4f341fac3c35447e453827cb7348514cac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 23:29:15 +0300 Subject: [PATCH 245/268] Lazy initialization of TransferSender removed --- .../include/uavcan/node/generic_publisher.hpp | 17 +++------ libuavcan/include/uavcan/node/publisher.hpp | 36 +++---------------- .../protocol/global_time_sync_master.hpp | 17 +++------ libuavcan/include/uavcan/protocol/logger.hpp | 3 +- .../uavcan/transport/transfer_sender.hpp | 26 ++++++++++---- libuavcan/src/node/uc_generic_publisher.cpp | 22 +++++------- .../protocol/uc_dynamic_node_id_client.cpp | 6 +--- .../src/protocol/uc_node_status_provider.cpp | 6 +--- .../src/transport/uc_transfer_sender.cpp | 17 ++++++--- libuavcan/test/node/publisher.cpp | 17 ++------- 10 files changed, 64 insertions(+), 103 deletions(-) diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index 7dde549c00..7991cc5044 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -20,15 +19,14 @@ namespace uavcan class GenericPublisherBase : Noncopyable { - const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced + TransferSender sender_; MonotonicDuration tx_timeout_; INode& node_; - LazyConstructor sender_; protected: GenericPublisherBase(INode& node, MonotonicDuration tx_timeout, MonotonicDuration max_transfer_interval) - : max_transfer_interval_(max_transfer_interval) + : sender_(node.getDispatcher(), max_transfer_interval) , tx_timeout_(tx_timeout) , node_(node) { @@ -51,7 +49,8 @@ protected: int genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); - TransferSender* getTransferSender(); + TransferSender& getTransferSender() { return sender_; } + const TransferSender& getTransferSender() const { return sender_; } public: static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); } @@ -66,7 +65,7 @@ public: */ void allowAnonymousTransfers() { - sender_->allowAnonymousTransfers(); + sender_.allowAnonymousTransfers(); } INode& getNode() const { return node_; } @@ -124,12 +123,6 @@ public: { return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline); } - - TransferSender* getTransferSender() - { - (void)checkInit(); - return GenericPublisherBase::getTransferSender(); - } }; // ---------------------------------------------------------------------------- diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index 4b95313d24..2b9222d1b5 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -81,46 +81,20 @@ public: /** * Returns priority of outgoing transfers. - * TODO: Make const. */ - TransferPriority getPriority() + TransferPriority getPriority() const { - // TODO probably TransferSender must be transformed into regular field? - TransferSender* const ts = getTransferSender(); - if (ts != NULL) - { - return ts->getPriority(); - } - else - { - return TransferPriorityNormal; // This is default - } + return BaseType::getTransferSender().getPriority(); } /** * Allows to change the priority of outgoing transfers. * Note that only High, Normal and Low priorities can be used; Service priority is not available for messages. - * Returns negative error code if priority cannot be set, non-negative on success. + * If the priority value is invalid, an assertion failure will be generated, and the value will not be updated. */ - int setPriority(const TransferPriority prio) + void setPriority(const TransferPriority prio) { - if (prio < NumTransferPriorities && prio != TransferPriorityService) - { - TransferSender* const ts = getTransferSender(); // TODO: Static TransferSender? - if (ts != NULL) - { - ts->setPriority(prio); - return 0; - } - else - { - return -ErrLogic; - } - } - else - { - return -ErrInvalidParam; - } + BaseType::getTransferSender().setPriority(prio); } static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(10); } diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp index 4b69451a0a..4cab0c4abf 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -51,16 +51,9 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase const int res = pub_.init(); if (res >= 0) { - TransferSender* const ts = pub_.getTransferSender(); - UAVCAN_ASSERT(ts != NULL); - ts->setIfaceMask(uint8_t(1 << iface_index_)); - ts->setCanIOFlags(CanIOFlagLoopback); - - const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority - if (prio_res < 0) - { - return prio_res; - } + pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_)); + pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback); + pub_.setPriority(TransferPriorityHigh); // Fixed priority } return res; } @@ -84,8 +77,8 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase int publish(TransferID tid, MonotonicTime current_time) { - UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); - UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); + UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback); + UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_)); const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; iface_prev_pub_mono_ = current_time; diff --git a/libuavcan/include/uavcan/protocol/logger.hpp b/libuavcan/include/uavcan/protocol/logger.hpp index 5856e07355..e6e3358152 100644 --- a/libuavcan/include/uavcan/protocol/logger.hpp +++ b/libuavcan/include/uavcan/protocol/logger.hpp @@ -101,7 +101,8 @@ public: { return res; } - return logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority + logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority + return 0; } /** diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 26d6e0e628..6c45e524bb 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -20,10 +20,10 @@ namespace uavcan class UAVCAN_EXPORT TransferSender { const MonotonicDuration max_transfer_interval_; - const DataTypeDescriptor& data_type_; + DataTypeID data_type_id_; TransferPriority priority_; - const CanTxQueue::Qos qos_; - const TransferCRC crc_base_; + CanTxQueue::Qos qos_; + TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; bool allow_anonymous_transfers_; @@ -43,16 +43,30 @@ public: TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) : max_transfer_interval_(max_transfer_interval) - , data_type_(data_type) , priority_(TransferPriorityNormal) - , qos_(qos) - , crc_base_(data_type.getSignature().toTransferCRC()) + , qos_(CanTxQueue::Qos()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) + , dispatcher_(dispatcher) + { + init(data_type, qos); + } + + TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) + : max_transfer_interval_(max_transfer_interval) + , priority_(TransferPriorityNormal) + , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) , dispatcher_(dispatcher) { } + void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos); + + bool isInitialized() const { return data_type_id_ != DataTypeID(); } + CanIOFlags getCanIOFlags() const { return flags_; } void setCanIOFlags(CanIOFlags flags) { flags_ = flags; } diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp index 09f361f5b0..73ab834d96 100644 --- a/libuavcan/src/node/uc_generic_publisher.cpp +++ b/libuavcan/src/node/uc_generic_publisher.cpp @@ -9,7 +9,7 @@ namespace uavcan bool GenericPublisherBase::isInited() const { - return bool(sender_); + return sender_.isInitialized(); } int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos) @@ -22,13 +22,14 @@ int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQ GlobalDataTypeRegistry::instance().freeze(); const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname); - if (!descr) + if (descr == NULL) { UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname); return -ErrUnknownDataType; } - sender_.construct - (node_.getDispatcher(), *descr, qos, max_transfer_interval_); + + sender_.init(*descr, qos); + return 0; } @@ -47,21 +48,16 @@ int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferT { if (tid) { - return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id, *tid); + return sender_.send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id, *tid); } else { - return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id); + return sender_.send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id); } } -TransferSender* GenericPublisherBase::getTransferSender() -{ - return sender_.isConstructed() ? static_cast(sender_) : NULL; -} - void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout) { tx_timeout = max(tx_timeout, getMinTxTimeout()); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp index f6150ed3ac..173314ed81 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp @@ -160,11 +160,7 @@ int DynamicNodeIDClient::start(const protocol::HardwareVersion& hardware_version return res; } dnida_pub_.allowAnonymousTransfers(); - res = dnida_pub_.setPriority(transfer_priority); - if (res < 0) - { - return res; - } + dnida_pub_.setPriority(transfer_priority); res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation)); if (res < 0) diff --git a/libuavcan/src/protocol/uc_node_status_provider.cpp b/libuavcan/src/protocol/uc_node_status_provider.cpp index bd090adb10..736050628f 100644 --- a/libuavcan/src/protocol/uc_node_status_provider.cpp +++ b/libuavcan/src/protocol/uc_node_status_provider.cpp @@ -71,11 +71,7 @@ int NodeStatusProvider::startAndPublish(TransferPriority priority) int res = -1; - res = node_status_pub_.setPriority(priority); - if (res < 0) - { - goto fail; - } + node_status_pub_.setPriority(priority); if (!getNode().isPassiveMode()) { diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index 6f49f7a635..232ce82235 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -15,6 +15,15 @@ void TransferSender::registerError() const dispatcher_.getTransferPerfCounter().addError(); } +void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos) +{ + UAVCAN_ASSERT(!isInitialized()); + + qos_ = qos; + data_type_id_ = dtid.getID(); + crc_base_ = dtid.getSignature().toTransferCRC(); +} + int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, TransferID tid) const @@ -24,7 +33,7 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic return -ErrTransferTooLong; } - Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); + Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); if (transfer_type == TransferTypeMessageBroadcast || transfer_type == TransferTypeMessageUnicast) { @@ -143,7 +152,7 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic /* * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped? */ - const OutgoingTransferRegistryKey otr_key(data_type_.getID(), transfer_type, dst_node_id); + const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id); UAVCAN_ASSERT(!tx_deadline.isZero()); const MonotonicTime otr_deadline = tx_deadline + max_transfer_interval_; @@ -151,8 +160,8 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); if (tid == NULL) { - UAVCAN_TRACE("TransferSender", "OTR access failure, dtd=%s tt=%i", - data_type_.toString().c_str(), int(transfer_type)); + UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i", + int(data_type_id_.get()), int(transfer_type)); return -ErrMemory; } diff --git a/libuavcan/test/node/publisher.cpp b/libuavcan/test/node/publisher.cpp index 61a57ea1c2..231a25b934 100644 --- a/libuavcan/test/node/publisher.cpp +++ b/libuavcan/test/node/publisher.cpp @@ -18,6 +18,8 @@ TEST(Publisher, Basic) uavcan::Publisher publisher(node); + ASSERT_FALSE(publisher.getTransferSender().isInitialized()); + std::cout << "sizeof(uavcan::Publisher): " << sizeof(uavcan::Publisher) << std::endl; @@ -110,18 +112,5 @@ TEST(Publisher, Basic) * Misc */ ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen()); - ASSERT_TRUE(publisher.getTransferSender()); -} - - -TEST(Publisher, ImplicitInitialization) -{ - SystemClockMock clock_mock(100); - CanDriverMock can_driver(2, clock_mock); - TestNode node(can_driver, clock_mock, 1); - - uavcan::Publisher publisher(node); - - // Will be initialized ad-hoc - ASSERT_TRUE(publisher.getTransferSender()); + ASSERT_TRUE(publisher.getTransferSender().isInitialized()); } From 941981066c0d4acf9ec93f405cc2f029d4b9c073 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 23:56:41 +0300 Subject: [PATCH 246/268] CRC64 removed, file messages refactored --- dsdl/uavcan/protocol/file/215.GetInfo.uavcan | 3 +-- dsdl/uavcan/protocol/file/218.Read.uavcan | 2 +- dsdl/uavcan/protocol/file/219.Write.uavcan | 4 ++-- libuavcan/include/uavcan/protocol/file_server.hpp | 13 ++++--------- libuavcan/test/protocol/file_server.cpp | 10 ++-------- 5 files changed, 10 insertions(+), 22 deletions(-) diff --git a/dsdl/uavcan/protocol/file/215.GetInfo.uavcan b/dsdl/uavcan/protocol/file/215.GetInfo.uavcan index d26d085ac0..f018a2a835 100644 --- a/dsdl/uavcan/protocol/file/215.GetInfo.uavcan +++ b/dsdl/uavcan/protocol/file/215.GetInfo.uavcan @@ -11,7 +11,6 @@ Path path --- -uint64 crc64 # Undefined for directories -uint32 size # Undefined for directories +uint40 size # Undefined for directories Error error EntryType entry_type diff --git a/dsdl/uavcan/protocol/file/218.Read.uavcan b/dsdl/uavcan/protocol/file/218.Read.uavcan index b120a86301..ebc1c6d4a1 100644 --- a/dsdl/uavcan/protocol/file/218.Read.uavcan +++ b/dsdl/uavcan/protocol/file/218.Read.uavcan @@ -12,7 +12,7 @@ # will be returned, and data array will be empty. # -uint32 offset +uint40 offset Path path --- diff --git a/dsdl/uavcan/protocol/file/219.Write.uavcan b/dsdl/uavcan/protocol/file/219.Write.uavcan index 1cbec0666d..a661803863 100644 --- a/dsdl/uavcan/protocol/file/219.Write.uavcan +++ b/dsdl/uavcan/protocol/file/219.Write.uavcan @@ -15,9 +15,9 @@ # example by means of creating a staging area for uncompleted writes (like FTP servers do). # -uint32 offset +uint40 offset Path path -uint8[<=200] data +uint8[<=192] data --- diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index 1e5fed04a2..f1ca877cd8 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -30,11 +30,6 @@ public: 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. */ @@ -51,7 +46,7 @@ public: * 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; + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) = 0; /** * Backend for uavcan.protocol.file.Read. @@ -60,7 +55,7 @@ public: * 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; + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0; // Methods below are optional. @@ -69,7 +64,7 @@ public: * 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) + virtual int16_t write(const Path& path, const uint64_t offset, const uint8_t* buffer, const uint16_t size) { (void)path; (void)offset; @@ -129,7 +124,7 @@ class BasicFileServer 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); + resp.error.value = backend_.getInfo(req.path.path, resp.size, resp.entry_type); } void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp) diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp index f3591e79e3..be970b4d04 100644 --- a/libuavcan/test/protocol/file_server.cpp +++ b/libuavcan/test/protocol/file_server.cpp @@ -13,15 +13,10 @@ 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) + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) { if (path == file_name) { - { - FileCRC crc; - crc.add(reinterpret_cast(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; @@ -33,7 +28,7 @@ public: } } - virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size) + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) { if (path == file_name) { @@ -90,7 +85,6 @@ TEST(BasicFileServer, Basic) 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, From b73dbd3f41f714e96bf3168ff81a1f688b872a24 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 30 May 2015 01:34:05 +0300 Subject: [PATCH 247/268] Padding optimization in TransferSender --- .../include/uavcan/transport/transfer_sender.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 6c45e524bb..2b95c20b42 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -20,16 +20,17 @@ namespace uavcan class UAVCAN_EXPORT TransferSender { const MonotonicDuration max_transfer_interval_; - DataTypeID data_type_id_; + + Dispatcher& dispatcher_; + TransferPriority priority_; CanTxQueue::Qos qos_; TransferCRC crc_base_; + DataTypeID data_type_id_; CanIOFlags flags_; uint8_t iface_mask_; bool allow_anonymous_transfers_; - Dispatcher& dispatcher_; - void registerError() const; public: @@ -43,24 +44,24 @@ public: TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) , priority_(TransferPriorityNormal) , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) - , dispatcher_(dispatcher) { init(data_type, qos); } TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) , priority_(TransferPriorityNormal) , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) - , dispatcher_(dispatcher) { } void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos); From 4733a0d1a0376962dd9ee758ee78810fb03a60ab Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 29 May 2015 17:59:31 -1000 Subject: [PATCH 248/268] File Open Cache --- .../basic_file_server_backend.hpp | 317 +++++++++++++++++- 1 file changed, 309 insertions(+), 8 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 32226c3b38..0663105d4f 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -34,7 +34,287 @@ class BasicFileSeverBackend : public uavcan::IFileServerBackend { enum { FilePermissions = 438 }; ///< 0o666 + protected: + + class FDCacheBase { + + public: + FDCacheBase() { } + virtual ~FDCacheBase() { } + + virtual int open(const char *path, int oflags) + { + using namespace std; + + return ::open(path, oflags); + } + + virtual int close(int fd, bool done = true) + { + using namespace std; + + return ::close(fd); + } + + }; + + FDCacheBase fallback_; + + class FDCache : public FDCacheBase { + + enum { MaxAgeSeconds = 3 }; + + class FDCacheItem { + + friend FDCache; + FDCacheItem *next_; + time_t last_access_; + int fd_; + int oflags_; + const char *path_; + + public: + + enum { InvalidFD = -1 }; + + FDCacheItem() : + next_(0), + last_access_(0), + fd_(InvalidFD), + oflags_(0), + path_(0) + { } + + FDCacheItem(int fd, const char * path, int oflags) : + next_(0), + last_access_(0), + fd_(fd), + oflags_(oflags), + path_(strdup(path)) + { + + } + + ~FDCacheItem() + { + if (valid()) + { + delete path_; + } + } + + inline bool valid() + { + return path_ != 0; + } + + inline int getFD() + { + return fd_; + } + + inline time_t getAccess() + { + return last_access_; + } + + time_t acessed() + { + last_access_ = time(0); + return getAccess(); + } + + void expire() + { + last_access_ = 0; + } + + bool expired() + { + return 0 == last_access_ || (time(0) - last_access_) > MaxAgeSeconds; + } + + int compare(const char * path, int oflags) + { + return (oflags_ == oflags && 0 == ::strcmp(path, path_)) ? 0 : 1; + } + + int compare(int fd) + { + return fd_ == fd ? 0 : 1; + } + + }; + + FDCacheItem* head_; + + FDCacheItem* find(const char *path, int oflags) + { + for(FDCacheItem* pi = head_; pi; pi = pi->next_) + { + if (0 == pi->compare(path, oflags)) + { + return pi; + } + } + return 0; + } + + FDCacheItem* find(int fd) + { + for(FDCacheItem* pi = head_; pi; pi = pi->next_) + { + if(0 == pi->compare(fd)) + { + return pi; + } + } + return 0; + } + + + FDCacheItem* add(FDCacheItem* pi) + { + pi->next_ = head_; + head_ = pi; + pi->acessed(); + return pi; + } + + void removeExpired(FDCacheItem** pi) + { + while (*pi) + { + if ((*pi)->expired()) + { + FDCacheItem* next = (*pi)->next_; + (void)FDCacheBase::close((*pi)->fd_); + delete(*pi); + *pi = next; + continue; + } + pi = &(*pi)->next_; + } + } + + void remove(FDCacheItem* pi, bool done) + { + if (done) + { + pi->expire(); + } + removeExpired(&head_); + } + + + void clear() + { + FDCacheItem* tmp; + for(FDCacheItem* pi = head_; pi; pi = tmp) + { + tmp = pi->next_; + (void)FDCacheBase::close(pi->fd_); + delete pi; + } + } + + + public: + + FDCache() : + head_(0) + { } + + virtual ~FDCache() + { + clear(); + } + + virtual int open(const char *path, int oflags) + { + int fd = FDCacheItem::InvalidFD; + + FDCacheItem *pi = find(path, oflags); + + if (pi != 0) + { + pi->acessed(); + } + else + { + fd = FDCacheBase::open(path, oflags); + if (fd < 0) + { + return fd; + } + + /* Allocate and clone path */ + + pi = new FDCacheItem(fd, path, oflags); + + /* Allocation worked but check clone */ + + if (pi && !pi->valid()) + { + + /* Allocation worked but clone or path failed */ + + delete pi; + pi = 0; + } + + if (pi == 0) + { + /* + * If allocation fails no harm just can not cache it + * return open fd + */ + + return fd; + } + /* add new */ + add(pi); + } + return pi->getFD(); + } + + + virtual int close(int fd, bool done) + { + FDCacheItem *pi = find(fd); + if (pi == 0) + { + /* + * If not found just close it + */ + + return FDCacheBase::close(fd); + } + remove(pi, done); + return 0; + } + + }; + + + FDCacheBase *fdcache_; + + FDCacheBase& getFDCache() + { + if (fdcache_ == 0) + { + fdcache_ = new FDCache(); + + if (fdcache_ == 0) + { + fdcache_ = &fallback_; + + } + } + return *fdcache_; + } + /** * Back-end for uavcan.protocol.file.GetInfo. * Implementation of this method is required. @@ -42,11 +322,13 @@ protected: */ 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; @@ -93,7 +375,7 @@ protected: rv = 0; out_close: - close(fd); + (void)::close(fd); } } return rv; @@ -106,14 +388,14 @@ protected: * 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); + FDCacheBase& cache = getFDCache(); + int fd = cache.open(path.c_str(), O_RDONLY); if (fd < 0) { @@ -121,14 +403,18 @@ protected: } else { - if (::lseek(fd, offset, SEEK_SET) < 0) + rv = ::lseek(fd, offset, SEEK_SET); + + ssize_t len = 0; + + if (rv < 0) { rv = errno; } else { // TODO use a read at offset to fill on EAGAIN - ssize_t len = ::read(fd, out_buffer, inout_size); + len = ::read(fd, out_buffer, inout_size); if (len < 0) { @@ -136,17 +422,32 @@ protected: } else { - - inout_size = len; rv = 0; } } - (void)close(fd); + + (void)cache.close(fd, rv != 0 || len != inout_size); + inout_size = len; } } return rv; } +public: + + BasicFileSeverBackend() : + fdcache_(0) + { + } + + ~BasicFileSeverBackend() + { + if (fdcache_ != &fallback_) + { + delete fdcache_; + fdcache_ = 0; + } + } }; } From cf32ca057324c5423fc63b5fdcee2e6506c76cfc Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Sat, 30 May 2015 08:00:02 -1000 Subject: [PATCH 249/268] Uodated to 20150527 Read GetInfo changes --- .../basic_file_server_backend.hpp | 65 ++++++------------- 1 file changed, 21 insertions(+), 44 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 0663105d4f..81570d69c7 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -320,62 +320,39 @@ protected: * 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) + virtual int16_t getInfo(const Path& path, uint64_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]; + struct stat sb; - int fd = ::open(path.c_str(), O_RDONLY); + rv = stat(path.c_str(), &sb); - if (fd >= 0) + if (rv < 0) + { + rv = errno; + } + else { - 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: - (void)::close(fd); + out_size = sb.st_size; + out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE; + if (S_ISDIR(sb.st_mode)) + { + out_type.flags |= uavcan::protocol::file::EntryType::FLAG_DIRECTORY; + } + else if (S_ISREG(sb.st_mode)) + { + out_type.flags |= uavcan::protocol::file::EntryType::FLAG_FILE; + } + // TODO Using fixed flag FLAG_READABLE until we add file permission checks to return actual value. } } return rv; @@ -388,7 +365,7 @@ protected: * 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) + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) { int rv = uavcan::protocol::file::Error::INVALID_VALUE; From 05099181e5bde0299df28a2622be0775b7d1fb11 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 31 May 2015 07:18:56 +0300 Subject: [PATCH 250/268] Marshal buffer removed --- .../include/uavcan/node/abstract_node.hpp | 2 - .../include/uavcan/node/generic_publisher.hpp | 33 ++++--- .../include/uavcan/node/marshal_buffer.hpp | 90 ------------------- libuavcan/include/uavcan/node/node.hpp | 25 ++---- libuavcan/src/node/uc_generic_publisher.cpp | 13 +-- libuavcan/test/node/test_node.hpp | 2 - 6 files changed, 30 insertions(+), 135 deletions(-) delete mode 100644 libuavcan/include/uavcan/node/marshal_buffer.hpp diff --git a/libuavcan/include/uavcan/node/abstract_node.hpp b/libuavcan/include/uavcan/node/abstract_node.hpp index 7fddbe32b6..026cb65c57 100644 --- a/libuavcan/include/uavcan/node/abstract_node.hpp +++ b/libuavcan/include/uavcan/node/abstract_node.hpp @@ -7,7 +7,6 @@ #include #include -#include namespace uavcan { @@ -23,7 +22,6 @@ public: virtual IPoolAllocator& getAllocator() = 0; virtual Scheduler& getScheduler() = 0; virtual const Scheduler& getScheduler() const = 0; - virtual IMarshalBufferProvider& getMarshalBufferProvider() = 0; virtual void registerInternalFailure(const char* msg) = 0; Dispatcher& getDispatcher() { return getScheduler().getDispatcher(); } diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index 7991cc5044..d1c04e56e0 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -10,6 +10,7 @@ #include #include #include +#include #include #include #include @@ -44,10 +45,8 @@ protected: MonotonicTime getTxDeadline() const; - IMarshalBuffer* getBuffer(unsigned byte_len); - - int genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id, - TransferID* tid, MonotonicTime blocking_deadline); + int genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); TransferSender& getTransferSender() { return sender_; } const TransferSender& getTransferSender() const { return sender_; } @@ -79,6 +78,15 @@ public: template class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase { + struct ZeroTransferBuffer : public StaticTransferBufferImpl + { + ZeroTransferBuffer() : StaticTransferBufferImpl(NULL, 0) { } + }; + + typedef typename Select::Result> >::Result Buffer; + enum { Qos = (DataTypeKind(DataSpec::DataTypeKind) == DataTypeKindMessage) ? @@ -87,7 +95,7 @@ class UAVCAN_EXPORT GenericPublisher : public GenericPublisherBase int checkInit(); - int doEncode(const DataStruct& message, IMarshalBuffer& buffer) const; + int doEncode(const DataStruct& message, ITransferBuffer& buffer) const; int genericPublish(const DataStruct& message, TransferType transfer_type, NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); @@ -138,7 +146,7 @@ int GenericPublisher::checkInit() } template -int GenericPublisher::doEncode(const DataStruct& message, IMarshalBuffer& buffer) const +int GenericPublisher::doEncode(const DataStruct& message, ITransferBuffer& buffer) const { BitStream bitstream(buffer); ScalarCodec codec(bitstream); @@ -161,17 +169,16 @@ int GenericPublisher::genericPublish(const DataStruct& mes { return res; } - IMarshalBuffer* const buf = getBuffer(BitLenToByteLen::Result); - if (!buf) - { - return -ErrMemory; - } - const int encode_res = doEncode(message, *buf); + + Buffer buffer; + + const int encode_res = doEncode(message, buffer); if (encode_res < 0) { return encode_res; } - return GenericPublisherBase::genericPublish(*buf, transfer_type, dst_node_id, tid, blocking_deadline); + + return GenericPublisherBase::genericPublish(buffer, transfer_type, dst_node_id, tid, blocking_deadline); } } diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp deleted file mode 100644 index 353233bc33..0000000000 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ /dev/null @@ -1,90 +0,0 @@ -/* - * Copyright (C) 2014 Pavel Kirienko - */ - -#ifndef UAVCAN_NODE_MARSHAL_BUFFER_HPP_INCLUDED -#define UAVCAN_NODE_MARSHAL_BUFFER_HPP_INCLUDED - -#include -#include -#include - -namespace uavcan -{ -/** - * Abstract temporary buffer for data marshalling. - */ -class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer -{ -public: - virtual const uint8_t* getDataPtr() const = 0; - virtual unsigned getMaxWritePos() const = 0; -}; - -/** - * Abstract provider of abstract buffer for data marshalling. - */ -class UAVCAN_EXPORT IMarshalBufferProvider -{ -public: - virtual ~IMarshalBufferProvider() { } - - /** - * Returns pointer to abstract buffer for data marshalling, - * but does not transfer ownership. - * If the requested buffer size is larger than available, - * null pointer will be returned. - * @param size Maximum buffer size needed for marshaling. - * @return Pointer to the buffer, or null pointer if - * the requested size is too large. - */ - virtual IMarshalBuffer* getBuffer(unsigned size) = 0; -}; - -/** - * Default implementation of marshal buffer provider. - */ -template -class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider -{ - class Buffer : public IMarshalBuffer - { - StaticTransferBuffer buf_; - - virtual int read(unsigned offset, uint8_t* data, unsigned len) const - { - return buf_.read(offset, data, len); - } - - virtual int write(unsigned offset, const uint8_t* data, unsigned len) - { - return buf_.write(offset, data, len); - } - - virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } - - virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } - - public: - void reset() { buf_.reset(); } - }; - - Buffer buffer_; - -public: - enum { MaxSize = MaxSize_ }; - - virtual IMarshalBuffer* getBuffer(unsigned size) - { - if (size > MaxSize) - { - return NULL; - } - buffer_.reset(); - return &buffer_; - } -}; - -} - -#endif // UAVCAN_NODE_MARSHAL_BUFFER_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 9cc2773c5c..fc8b43e9f9 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -10,7 +10,6 @@ #include #include #include -#include // High-level functionality available by default #include @@ -46,21 +45,12 @@ namespace uavcan * Additional objects for Transfer ID tracking will * be allocated in the memory pool if needed. * Default value is acceptable for any use case. - * - * @tparam MarshalBufferSize Size of the marshal buffer that is used to provide short-term temporary storage for - * serialized data for TX transfers. The buffer must be large enough to accommodate - * largest serialized TX transfer. The default value is guaranteed to be large enough, - * but it can be reduced if long TX transfers are not used to optimize memory use. - * If UAVCAN_TINY mode is enabled, this value defaults to the maximum length of a - * response transfer of uavcan.protocol.GetNodeInfo. */ template ::Result + unsigned OutgoingTransferRegistryStaticEntries = 0 #else - unsigned OutgoingTransferRegistryStaticEntries = 10, - unsigned MarshalBufferSize = MaxPossibleTransferPayloadLen + unsigned OutgoingTransferRegistryStaticEntries = 10 #endif > class UAVCAN_EXPORT Node : public INode @@ -73,7 +63,6 @@ class UAVCAN_EXPORT Node : public INode typedef PoolAllocator Allocator; Allocator pool_allocator_; - MarshalBufferProvider marsh_buf_; OutgoingTransferRegistry outgoing_trans_reg_; Scheduler scheduler_; @@ -100,8 +89,6 @@ protected: #endif } - virtual IMarshalBufferProvider& getMarshalBufferProvider() { return marsh_buf_; } - public: Node(ICanDriver& can_driver, ISystemClock& system_clock) : outgoing_trans_reg_(pool_allocator_) @@ -265,8 +252,8 @@ public: // ---------------------------------------------------------------------------- -template -int Node::start( +template +int Node::start( const TransferPriority priority) { if (started_) @@ -312,8 +299,8 @@ fail: #if !UAVCAN_TINY -template -int Node:: +template +int Node:: checkNetworkCompatibility(NetworkCompatibilityCheckResult& result) { if (!started_) diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp index 73ab834d96..28e9b36acd 100644 --- a/libuavcan/src/node/uc_generic_publisher.cpp +++ b/libuavcan/src/node/uc_generic_publisher.cpp @@ -38,22 +38,17 @@ MonotonicTime GenericPublisherBase::getTxDeadline() const return node_.getMonotonicTime() + tx_timeout_; } -IMarshalBuffer* GenericPublisherBase::getBuffer(unsigned byte_len) -{ - return node_.getMarshalBufferProvider().getBuffer(byte_len); -} - -int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id, - TransferID* tid, MonotonicTime blocking_deadline) +int GenericPublisherBase::genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline) { if (tid) { - return sender_.send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), blocking_deadline, transfer_type, dst_node_id, *tid); } else { - return sender_.send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), blocking_deadline, transfer_type, dst_node_id); } } diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 5a943ae6da..f101f00991 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -20,7 +20,6 @@ struct TestNode : public uavcan::INode { uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; - uavcan::MarshalBufferProvider<512> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; uavcan::Scheduler scheduler; uint64_t internal_failure_count; @@ -43,7 +42,6 @@ struct TestNode : public uavcan::INode virtual uavcan::PoolManager<1>& getAllocator() { return poolmgr; } virtual uavcan::Scheduler& getScheduler() { return scheduler; } virtual const uavcan::Scheduler& getScheduler() const { return scheduler; } - virtual uavcan::IMarshalBufferProvider& getMarshalBufferProvider() { return buffer_provider; } }; From 28e82797c24f8421dfbd8e953711d20ea641c397 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sun, 31 May 2015 08:12:46 +0300 Subject: [PATCH 251/268] Optimized padding, added more sizeof() outputs --- libuavcan/include/uavcan/transport/transfer_listener.hpp | 4 ++-- libuavcan/test/node/service_server.cpp | 3 +++ .../dynamic_node_id_server/distributed/server.cpp | 8 ++++++++ libuavcan/test/transport/transfer_listener.cpp | 7 +++++++ libuavcan/test/transport/transfer_receiver.cpp | 2 ++ 5 files changed, 22 insertions(+), 2 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 6b57c25e99..66bfe9f6f7 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -99,10 +99,10 @@ public: class UAVCAN_EXPORT TransferListenerBase : public LinkedListNode, Noncopyable { const DataTypeDescriptor& data_type_; - const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant MapBase& receivers_; ITransferBufferManager& bufmgr_; TransferPerfCounter& perf_; + const TransferCRC crc_base_; ///< Pre-initialized with data type hash, thus constant bool allow_anonymous_transfers_; class TimedOutReceiverPredicate @@ -126,10 +126,10 @@ protected: MapBase& receivers, ITransferBufferManager& bufmgr) : data_type_(data_type) - , crc_base_(data_type.getSignature().toTransferCRC()) , receivers_(receivers) , bufmgr_(bufmgr) , perf_(perf) + , crc_base_(data_type.getSignature().toTransferCRC()) , allow_anonymous_transfers_(false) { } diff --git a/libuavcan/test/node/service_server.cpp b/libuavcan/test/node/service_server.cpp index 4137bb1203..ec49b27772 100644 --- a/libuavcan/test/node/service_server.cpp +++ b/libuavcan/test/node/service_server.cpp @@ -144,6 +144,9 @@ TEST(ServiceServer, Empty) uavcan::ServiceServer server(node); + std::cout << "sizeof(ServiceServer): " + << sizeof(uavcan::ServiceServer) << std::endl; + ASSERT_EQ(0, node.getDispatcher().getNumServiceRequestListeners()); ASSERT_GE(0, server.start(impl.bind())); ASSERT_EQ(1, node.getDispatcher().getNumServiceRequestListeners()); diff --git a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp index 59a4178ebd..d6992c02c8 100644 --- a/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -176,6 +176,8 @@ TEST(dynamic_node_id_server_Server, Basic) TEST(dynamic_node_id_server, ObjectSizes) { + using namespace uavcan; + using namespace uavcan::protocol::dynamic_node_id::server; using namespace uavcan::dynamic_node_id_server; std::cout << "distributed::Log: " << sizeof(distributed::Log) << std::endl; @@ -184,4 +186,10 @@ TEST(dynamic_node_id_server, ObjectSizes) std::cout << "distributed::RaftCore: " << sizeof(distributed::RaftCore) << std::endl; std::cout << "distributed::Server: " << sizeof(distributed::Server) << std::endl; std::cout << "AllocationRequestManager: " << sizeof(AllocationRequestManager) << std::endl; + + std::cout << "ServiceServer: " << sizeof(ServiceServer) << std::endl; + std::cout << "ServiceClient: " << sizeof(ServiceClient) << std::endl; + std::cout << "ServiceServer: " << sizeof(ServiceServer) << std::endl; + std::cout << "ServiceClient:" + << sizeof(ServiceClient&), 5>) << std::endl; } diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index 59d20e3374..ee3474394c 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -289,3 +289,10 @@ TEST(TransferListener, AnonymousTransfers) ASSERT_TRUE(subscriber.isEmpty()); } + +TEST(TransferListener, Sizes) +{ + using namespace uavcan; + + std::cout << "sizeof(TransferListener<64, 1, 2>): " << sizeof(TransferListener<64, 1, 2>) << std::endl; +} diff --git a/libuavcan/test/transport/transfer_receiver.cpp b/libuavcan/test/transport/transfer_receiver.cpp index 755d193c0f..6be7d99ce9 100644 --- a/libuavcan/test/transport/transfer_receiver.cpp +++ b/libuavcan/test/transport/transfer_receiver.cpp @@ -102,6 +102,8 @@ TEST(TransferReceiver, Basic) uavcan::ITransferBufferManager& bufmgr = context.bufmgr; uavcan::TransferBufferAccessor bk(context.bufmgr, RxFrameGenerator::DEFAULT_KEY); + std::cout << "sizeof(TransferReceiver): " << sizeof(TransferReceiver) << std::endl; + /* * Empty */ From 22787651e699c58b18258a8cb01249b0f5f329cb Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 13:58:20 +0300 Subject: [PATCH 252/268] STM32 NuttX driver: Edge-triggered poll(), sort of fixes #35 --- .../stm32/driver/include/uavcan_stm32/can.hpp | 3 +- .../driver/include/uavcan_stm32/thread.hpp | 14 ++++----- .../stm32/driver/src/uc_stm32_thread.cpp | 30 +++++-------------- 3 files changed, 14 insertions(+), 33 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 45e022bf38..170d69b820 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -194,8 +194,7 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable public: template CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) - : update_event_(*this) - , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) + : if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) #if UAVCAN_STM32_NUM_IFACES > 1 , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) #endif diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index d2c6869ddc..5d084d6a94 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -34,11 +34,9 @@ class BusEvent chibios_rt::CounterSemaphore sem_; public: - BusEvent(CanDriver& can_driver) + BusEvent() : sem_(0) - { - (void)can_driver; - } + { } bool wait(uavcan::MonotonicDuration duration); @@ -58,13 +56,15 @@ public: #elif UAVCAN_STM32_NUTTX +/** + * All bus events are reported as POLLIN. + */ class BusEvent : uavcan::Noncopyable { static const unsigned MaxPollWaiters = 8; ::file_operations file_ops_; ::pollfd* pollset_[MaxPollWaiters]; - CanDriver& can_driver_; bool signal_; static int openTrampoline(::file* filp); @@ -75,15 +75,13 @@ class BusEvent : uavcan::Noncopyable int close(::file* filp); int poll(::file* filp, ::pollfd* fds, bool setup); - unsigned makePollMask() const; - int addPollWaiter(::pollfd* fds); int removePollWaiter(::pollfd* fds); public: static const char* const DevName; - BusEvent(CanDriver& can_driver); + BusEvent(); ~BusEvent(); bool wait(uavcan::MonotonicDuration duration); diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index c24885968d..4c7c16927e 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -99,7 +99,7 @@ int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) ret = addPollWaiter(fds); if (ret == 0) { - fds->revents |= fds->events & makePollMask(); + fds->revents |= fds->events & POLLIN; if (fds->revents != 0) { (void)sem_post(fds->sem); @@ -114,26 +114,11 @@ int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) return ret; } -unsigned BusEvent::makePollMask() const -{ - const uavcan::CanSelectMasks select_masks = can_driver_.makeSelectMasks(); - unsigned poll_mask = 0; - if (select_masks.read != 0) - { - poll_mask |= POLLIN; - } - if (select_masks.write != 0) - { - poll_mask |= POLLOUT; - } - return poll_mask; -} - int BusEvent::addPollWaiter(::pollfd* fds) { for (unsigned i = 0; i < MaxPollWaiters; i++) { - if (pollset_[i] == nullptr) + if (pollset_[i] == NULL) { pollset_[i] = fds; return 0; @@ -148,16 +133,15 @@ int BusEvent::removePollWaiter(::pollfd* fds) { if (fds == pollset_[i]) { - pollset_[i] = nullptr; + pollset_[i] = NULL; return 0; } } return -EINVAL; } -BusEvent::BusEvent(CanDriver& can_driver) - : can_driver_(can_driver) - , signal_(false) +BusEvent::BusEvent() + : signal_(false) { std::memset(&file_ops_, 0, sizeof(file_ops_)); std::memset(pollset_, 0, sizeof(pollset_)); @@ -201,9 +185,9 @@ void BusEvent::signalFromInterrupt() for (unsigned i = 0; i < MaxPollWaiters; i++) { ::pollfd* const fd = pollset_[i]; - if (fd != nullptr) + if (fd != NULL) { - fd->revents = fd->events & makePollMask(); + fd->revents |= fd->events & POLLIN; if ((fd->revents != 0) && (fd->sem->semcount <= 0)) { (void)sem_post(fd->sem); From 52f809a620858b6430d9f386766b305406fe9e14 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 14:22:32 +0300 Subject: [PATCH 253/268] firmware_version_checker.hpp formatting --- .../uavcan_posix/firmware_version_checker.hpp | 15 +++------------ 1 file changed, 3 insertions(+), 12 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index d38bf1a1f2..8de2c21585 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -18,6 +18,7 @@ #include +// TODO Get rid of the macro #if !defined(DIRENT_ISFILE) && defined(DT_REG) # define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) #endif @@ -49,7 +50,6 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker */ typedef uavcan::MakeString::Type PathString; - BasePathString base_path_; BasePathString cache_path_; @@ -121,13 +121,10 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { if (size < 0) { - rv = -errno; - } else { - if (size != write(dfd, buffer, size)) { rv = -errno; @@ -156,8 +153,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker uint8_t reserved[6]; }; - - static int getFileInfo(const char* path, AppDescriptor & descriptor) + static int getFileInfo(const char* path, AppDescriptor& descriptor) { using namespace std; @@ -166,7 +162,6 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker uint64_t signature = 0; std::memcpy(&signature, "APDesc00", 8); - int rv = -ENOENT; uint64_t chunk[MaxChunk]; int fd = open(path, O_RDONLY); @@ -196,7 +191,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker { if (*p == signature) { - pdescriptor = (AppDescriptor*) p; + pdescriptor = reinterpret_cast(p); // FIXME TODO This breaks strict aliasing descriptor = *pdescriptor; rv = 0; break; @@ -212,7 +207,6 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker } public: - const BasePathString& getFirmwareBasePath() const { return base_path_; } const BasePathString& getFirmwareCachePath() const { return cache_path_; } @@ -281,8 +275,6 @@ public: return rv; } - - protected: /** * This method will be invoked when the class obtains a response to GetNodeInfo request. @@ -341,7 +333,6 @@ protected: 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. From cc74cf46aff84252d9d4aed81b981558966b7374 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 14:57:44 +0300 Subject: [PATCH 254/268] Basic file server backend formatting --- .../basic_file_server_backend.hpp | 88 +++++++------------ 1 file changed, 33 insertions(+), 55 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 81570d69c7..f8a23e9431 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -34,16 +34,14 @@ class BasicFileSeverBackend : public uavcan::IFileServerBackend { enum { FilePermissions = 438 }; ///< 0o666 - protected: - - class FDCacheBase { - + class FDCacheBase + { public: FDCacheBase() { } virtual ~FDCacheBase() { } - virtual int open(const char *path, int oflags) + virtual int open(const char* path, int oflags) { using namespace std; @@ -56,45 +54,41 @@ protected: return ::close(fd); } - }; FDCacheBase fallback_; - class FDCache : public FDCacheBase { - + class FDCache : public FDCacheBase + { enum { MaxAgeSeconds = 3 }; - class FDCacheItem { - + class FDCacheItem + { friend FDCache; - FDCacheItem *next_; + FDCacheItem* next_; time_t last_access_; int fd_; int oflags_; - const char *path_; + const char* path_; public: - enum { InvalidFD = -1 }; - FDCacheItem() : - next_(0), - last_access_(0), - fd_(InvalidFD), - oflags_(0), - path_(0) + FDCacheItem() + : next_(0) + , last_access_(0) + , fd_(InvalidFD) + , oflags_(0) + , path_(0) { } - FDCacheItem(int fd, const char * path, int oflags) : - next_(0), - last_access_(0), - fd_(fd), - oflags_(oflags), - path_(strdup(path)) - { - - } + FDCacheItem(int fd, const char* path, int oflags) + : next_(0) + , last_access_(0) + , fd_(fd) + , oflags_(oflags) + , path_(strdup(path)) + { } ~FDCacheItem() { @@ -135,7 +129,7 @@ protected: return 0 == last_access_ || (time(0) - last_access_) > MaxAgeSeconds; } - int compare(const char * path, int oflags) + int compare(const char* path, int oflags) { return (oflags_ == oflags && 0 == ::strcmp(path, path_)) ? 0 : 1; } @@ -144,12 +138,11 @@ protected: { return fd_ == fd ? 0 : 1; } - }; FDCacheItem* head_; - FDCacheItem* find(const char *path, int oflags) + FDCacheItem* find(const char* path, int oflags) { for(FDCacheItem* pi = head_; pi; pi = pi->next_) { @@ -173,7 +166,6 @@ protected: return 0; } - FDCacheItem* add(FDCacheItem* pi) { pi->next_ = head_; @@ -207,7 +199,6 @@ protected: removeExpired(&head_); } - void clear() { FDCacheItem* tmp; @@ -219,11 +210,9 @@ protected: } } - public: - - FDCache() : - head_(0) + FDCache() + : head_(NULL) { } virtual ~FDCache() @@ -231,11 +220,11 @@ protected: clear(); } - virtual int open(const char *path, int oflags) + virtual int open(const char* path, int oflags) { int fd = FDCacheItem::InvalidFD; - FDCacheItem *pi = find(path, oflags); + FDCacheItem* pi = find(path, oflags); if (pi != 0) { @@ -257,7 +246,6 @@ protected: if (pi && !pi->valid()) { - /* Allocation worked but clone or path failed */ delete pi; @@ -279,10 +267,9 @@ protected: return pi->getFD(); } - virtual int close(int fd, bool done) { - FDCacheItem *pi = find(fd); + FDCacheItem* pi = find(fd); if (pi == 0) { /* @@ -294,11 +281,9 @@ protected: remove(pi, done); return 0; } - }; - - FDCacheBase *fdcache_; + FDCacheBase* fdcache_; FDCacheBase& getFDCache() { @@ -309,7 +294,6 @@ protected: if (fdcache_ == 0) { fdcache_ = &fallback_; - } } return *fdcache_; @@ -322,14 +306,12 @@ protected: */ virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) { - int rv = uavcan::protocol::file::Error::INVALID_VALUE; if (path.size() > 0) { using namespace std; - struct stat sb; rv = stat(path.c_str(), &sb); @@ -340,7 +322,6 @@ protected: } else { - rv = 0; out_size = sb.st_size; out_type.flags = uavcan::protocol::file::EntryType::FLAG_READABLE; @@ -411,11 +392,9 @@ protected: } public: - - BasicFileSeverBackend() : - fdcache_(0) - { - } + BasicFileSeverBackend() + : fdcache_(NULL) + { } ~BasicFileSeverBackend() { @@ -426,7 +405,6 @@ public: } } }; - } #endif // Include guard From 49019bfcffa26b014a1a38f19090959fc00086bf Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 14:58:21 +0300 Subject: [PATCH 255/268] firmware_version_checker.hpp formatting --- .../posix/include/uavcan_posix/firmware_version_checker.hpp | 3 --- 1 file changed, 3 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 8de2c21585..43d7911c20 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -290,7 +290,6 @@ protected: * @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) @@ -403,13 +402,11 @@ protected: } public: - const char* getFirmwarePath() const { return getFirmwareCachePath().c_str(); } }; - } #endif // Include guard From 52529408d302afcb3cc637b1e5ded17b63b2dda3 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 15:03:27 +0300 Subject: [PATCH 256/268] POSIX tools - visibility fix --- .../uavcan_posix/firmware_version_checker.hpp | 137 +++++++++--------- 1 file changed, 68 insertions(+), 69 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index 43d7911c20..ff09260b8b 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -206,75 +206,6 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker return rv; } -public: - const BasePathString& getFirmwareBasePath() const { return base_path_; } - - const BasePathString& getFirmwareCachePath() const { return cache_path_; } - - static char getPathSeparator() - { - return static_cast(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. @@ -402,6 +333,74 @@ protected: } public: + const BasePathString& getFirmwareBasePath() const { return base_path_; } + + const BasePathString& getFirmwareCachePath() const { return cache_path_; } + + static char getPathSeparator() + { + return static_cast(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; + } + const char* getFirmwarePath() const { return getFirmwareCachePath().c_str(); From 2615fda63e51cde026f2e4ea1b3ec67423113a51 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 15:04:46 +0300 Subject: [PATCH 257/268] POSIX event tracer formatting --- .../uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp index 45a6d11c12..d07f6d5624 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -63,7 +63,7 @@ public: /** * Initializes the file based event tracer. */ - int init(const PathString & path) + int init(const PathString& path) { using namespace std; From 1c88bd7183963f2c51fd0a07c666395e2a2d3cc2 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 15:09:34 +0300 Subject: [PATCH 258/268] Using proper NULL in POSIX tools --- .../basic_file_server_backend.hpp | 27 +++++++++---------- 1 file changed, 13 insertions(+), 14 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index f8a23e9431..83a7295bb7 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -75,7 +75,7 @@ protected: enum { InvalidFD = -1 }; FDCacheItem() - : next_(0) + : next_(NULL) , last_access_(0) , fd_(InvalidFD) , oflags_(0) @@ -83,7 +83,7 @@ protected: { } FDCacheItem(int fd, const char* path, int oflags) - : next_(0) + : next_(NULL) , last_access_(0) , fd_(fd) , oflags_(oflags) @@ -100,7 +100,7 @@ protected: inline bool valid() { - return path_ != 0; + return path_ != NULL; } inline int getFD() @@ -115,7 +115,7 @@ protected: time_t acessed() { - last_access_ = time(0); + last_access_ = time(NULL); return getAccess(); } @@ -126,7 +126,7 @@ protected: bool expired() { - return 0 == last_access_ || (time(0) - last_access_) > MaxAgeSeconds; + return 0 == last_access_ || (time(NULL) - last_access_) > MaxAgeSeconds; } int compare(const char* path, int oflags) @@ -151,7 +151,7 @@ protected: return pi; } } - return 0; + return NULL; } FDCacheItem* find(int fd) @@ -226,7 +226,7 @@ protected: FDCacheItem* pi = find(path, oflags); - if (pi != 0) + if (pi != NULL) { pi->acessed(); } @@ -249,10 +249,10 @@ protected: /* Allocation worked but clone or path failed */ delete pi; - pi = 0; + pi = NULL; } - if (pi == 0) + if (pi == NULL) { /* * If allocation fails no harm just can not cache it @@ -270,12 +270,11 @@ protected: virtual int close(int fd, bool done) { FDCacheItem* pi = find(fd); - if (pi == 0) + if (pi == NULL) { /* * If not found just close it */ - return FDCacheBase::close(fd); } remove(pi, done); @@ -287,11 +286,11 @@ protected: FDCacheBase& getFDCache() { - if (fdcache_ == 0) + if (fdcache_ == NULL) { fdcache_ = new FDCache(); - if (fdcache_ == 0) + if (fdcache_ == NULL) { fdcache_ = &fallback_; } @@ -401,7 +400,7 @@ public: if (fdcache_ != &fallback_) { delete fdcache_; - fdcache_ = 0; + fdcache_ = NULL; } } }; From e0ea51aa2152836429190a0e50194bf740cb0000 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 15:17:58 +0300 Subject: [PATCH 259/268] Fixed naming and CV-correctness --- .../basic_file_server_backend.hpp | 22 ++++++++++--------- 1 file changed, 12 insertions(+), 10 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 83a7295bb7..48030b85cc 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -65,6 +65,7 @@ protected: class FDCacheItem { friend FDCache; + FDCacheItem* next_; time_t last_access_; int fd_; @@ -98,17 +99,17 @@ protected: } } - inline bool valid() + bool valid() const { return path_ != NULL; } - inline int getFD() + int getFD() const { return fd_; } - inline time_t getAccess() + time_t getAccess() const { return last_access_; } @@ -124,19 +125,20 @@ protected: last_access_ = 0; } - bool expired() + bool expired() const { return 0 == last_access_ || (time(NULL) - last_access_) > MaxAgeSeconds; } - int compare(const char* path, int oflags) + bool equals(const char* path, int oflags) const { - return (oflags_ == oflags && 0 == ::strcmp(path, path_)) ? 0 : 1; + return oflags_ == oflags && + 0 == ::strcmp(path, path_); } - int compare(int fd) + bool equals(int fd) const { - return fd_ == fd ? 0 : 1; + return fd_ == fd; } }; @@ -146,7 +148,7 @@ protected: { for(FDCacheItem* pi = head_; pi; pi = pi->next_) { - if (0 == pi->compare(path, oflags)) + if (pi->equals(path, oflags)) { return pi; } @@ -158,7 +160,7 @@ protected: { for(FDCacheItem* pi = head_; pi; pi = pi->next_) { - if(0 == pi->compare(fd)) + if(pi->equals(fd)) { return pi; } From 7518172f6e2282137920a8277be83eb77acc7ad4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 15:22:06 +0300 Subject: [PATCH 260/268] POSIX tools - more autoformatting --- .../basic_file_server_backend.hpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 48030b85cc..85f9058ed0 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -50,6 +50,7 @@ protected: virtual int close(int fd, bool done = true) { + (void)done; using namespace std; return ::close(fd); @@ -132,8 +133,7 @@ protected: bool equals(const char* path, int oflags) const { - return oflags_ == oflags && - 0 == ::strcmp(path, path_); + return oflags_ == oflags && 0 == ::strcmp(path, path_); } bool equals(int fd) const @@ -146,7 +146,7 @@ protected: FDCacheItem* find(const char* path, int oflags) { - for(FDCacheItem* pi = head_; pi; pi = pi->next_) + for (FDCacheItem* pi = head_; pi; pi = pi->next_) { if (pi->equals(path, oflags)) { @@ -158,9 +158,9 @@ protected: FDCacheItem* find(int fd) { - for(FDCacheItem* pi = head_; pi; pi = pi->next_) + for (FDCacheItem* pi = head_; pi; pi = pi->next_) { - if(pi->equals(fd)) + if (pi->equals(fd)) { return pi; } @@ -183,8 +183,8 @@ protected: if ((*pi)->expired()) { FDCacheItem* next = (*pi)->next_; - (void)FDCacheBase::close((*pi)->fd_); - delete(*pi); + (void) FDCacheBase::close((*pi)->fd_); + delete (*pi); *pi = next; continue; } @@ -204,10 +204,10 @@ protected: void clear() { FDCacheItem* tmp; - for(FDCacheItem* pi = head_; pi; pi = tmp) + for (FDCacheItem* pi = head_; pi; pi = tmp) { tmp = pi->next_; - (void)FDCacheBase::close(pi->fd_); + (void) FDCacheBase::close(pi->fd_); delete pi; } } @@ -286,7 +286,7 @@ protected: FDCacheBase* fdcache_; - FDCacheBase& getFDCache() + FDCacheBase& getFDCache() { if (fdcache_ == NULL) { From 5442c0ac04700dc8ade4774ebb793592758bd67d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 16:36:02 +0300 Subject: [PATCH 261/268] Revert "STM32 NuttX driver: Edge-triggered poll(), sort of fixes #35" This reverts commit 22787651e699c58b18258a8cb01249b0f5f329cb. --- .../stm32/driver/include/uavcan_stm32/can.hpp | 3 +- .../driver/include/uavcan_stm32/thread.hpp | 14 +++++---- .../stm32/driver/src/uc_stm32_thread.cpp | 30 ++++++++++++++----- 3 files changed, 33 insertions(+), 14 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index 170d69b820..45e022bf38 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -194,7 +194,8 @@ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable public: template CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) - : if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) + : update_event_(*this) + , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) #if UAVCAN_STM32_NUM_IFACES > 1 , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) #endif diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index 5d084d6a94..d2c6869ddc 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -34,9 +34,11 @@ class BusEvent chibios_rt::CounterSemaphore sem_; public: - BusEvent() + BusEvent(CanDriver& can_driver) : sem_(0) - { } + { + (void)can_driver; + } bool wait(uavcan::MonotonicDuration duration); @@ -56,15 +58,13 @@ public: #elif UAVCAN_STM32_NUTTX -/** - * All bus events are reported as POLLIN. - */ class BusEvent : uavcan::Noncopyable { static const unsigned MaxPollWaiters = 8; ::file_operations file_ops_; ::pollfd* pollset_[MaxPollWaiters]; + CanDriver& can_driver_; bool signal_; static int openTrampoline(::file* filp); @@ -75,13 +75,15 @@ class BusEvent : uavcan::Noncopyable int close(::file* filp); int poll(::file* filp, ::pollfd* fds, bool setup); + unsigned makePollMask() const; + int addPollWaiter(::pollfd* fds); int removePollWaiter(::pollfd* fds); public: static const char* const DevName; - BusEvent(); + BusEvent(CanDriver& can_driver); ~BusEvent(); bool wait(uavcan::MonotonicDuration duration); diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index 4c7c16927e..c24885968d 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -99,7 +99,7 @@ int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) ret = addPollWaiter(fds); if (ret == 0) { - fds->revents |= fds->events & POLLIN; + fds->revents |= fds->events & makePollMask(); if (fds->revents != 0) { (void)sem_post(fds->sem); @@ -114,11 +114,26 @@ int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) return ret; } +unsigned BusEvent::makePollMask() const +{ + const uavcan::CanSelectMasks select_masks = can_driver_.makeSelectMasks(); + unsigned poll_mask = 0; + if (select_masks.read != 0) + { + poll_mask |= POLLIN; + } + if (select_masks.write != 0) + { + poll_mask |= POLLOUT; + } + return poll_mask; +} + int BusEvent::addPollWaiter(::pollfd* fds) { for (unsigned i = 0; i < MaxPollWaiters; i++) { - if (pollset_[i] == NULL) + if (pollset_[i] == nullptr) { pollset_[i] = fds; return 0; @@ -133,15 +148,16 @@ int BusEvent::removePollWaiter(::pollfd* fds) { if (fds == pollset_[i]) { - pollset_[i] = NULL; + pollset_[i] = nullptr; return 0; } } return -EINVAL; } -BusEvent::BusEvent() - : signal_(false) +BusEvent::BusEvent(CanDriver& can_driver) + : can_driver_(can_driver) + , signal_(false) { std::memset(&file_ops_, 0, sizeof(file_ops_)); std::memset(pollset_, 0, sizeof(pollset_)); @@ -185,9 +201,9 @@ void BusEvent::signalFromInterrupt() for (unsigned i = 0; i < MaxPollWaiters; i++) { ::pollfd* const fd = pollset_[i]; - if (fd != NULL) + if (fd != nullptr) { - fd->revents |= fd->events & POLLIN; + fd->revents = fd->events & makePollMask(); if ((fd->revents != 0) && (fd->sem->semcount <= 0)) { (void)sem_post(fd->sem); From 0dc18f9623ea722352bcc276265a5049f16b7e09 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 1 Jun 2015 16:45:12 +0300 Subject: [PATCH 262/268] Fixed STM32 NuttX driver --- .../driver/include/uavcan_stm32/thread.hpp | 5 +-- .../stm32/driver/src/uc_stm32_thread.cpp | 31 +++++++------------ 2 files changed, 14 insertions(+), 22 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index d2c6869ddc..1ad2d30716 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -58,6 +58,9 @@ public: #elif UAVCAN_STM32_NUTTX +/** + * All bus events are reported as POLLIN. + */ class BusEvent : uavcan::Noncopyable { static const unsigned MaxPollWaiters = 8; @@ -75,8 +78,6 @@ class BusEvent : uavcan::Noncopyable int close(::file* filp); int poll(::file* filp, ::pollfd* fds, bool setup); - unsigned makePollMask() const; - int addPollWaiter(::pollfd* fds); int removePollWaiter(::pollfd* fds); diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index c24885968d..88fd0b37ed 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -99,7 +99,13 @@ int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) ret = addPollWaiter(fds); if (ret == 0) { - fds->revents |= fds->events & makePollMask(); + /* + * Two events can be reported via POLLIN: + * - The RX queue is not empty. This event is level-triggered. + * - Transmission complete. This event is edge-triggered. + * FIXME Since TX event is edge-triggered, it can be lost between poll() calls. + */ + fds->revents |= fds->events & ((can_driver_.makeSelectMasks().read == 0) ? 0 : POLLIN); if (fds->revents != 0) { (void)sem_post(fds->sem); @@ -114,26 +120,11 @@ int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) return ret; } -unsigned BusEvent::makePollMask() const -{ - const uavcan::CanSelectMasks select_masks = can_driver_.makeSelectMasks(); - unsigned poll_mask = 0; - if (select_masks.read != 0) - { - poll_mask |= POLLIN; - } - if (select_masks.write != 0) - { - poll_mask |= POLLOUT; - } - return poll_mask; -} - int BusEvent::addPollWaiter(::pollfd* fds) { for (unsigned i = 0; i < MaxPollWaiters; i++) { - if (pollset_[i] == nullptr) + if (pollset_[i] == NULL) { pollset_[i] = fds; return 0; @@ -148,7 +139,7 @@ int BusEvent::removePollWaiter(::pollfd* fds) { if (fds == pollset_[i]) { - pollset_[i] = nullptr; + pollset_[i] = NULL; return 0; } } @@ -201,9 +192,9 @@ void BusEvent::signalFromInterrupt() for (unsigned i = 0; i < MaxPollWaiters; i++) { ::pollfd* const fd = pollset_[i]; - if (fd != nullptr) + if (fd != NULL) { - fd->revents = fd->events & makePollMask(); + fd->revents |= fd->events & POLLIN; if ((fd->revents != 0) && (fd->sem->semcount <= 0)) { (void)sem_post(fd->sem); From a39c8fef3a806104c47c4cc79ef486bb40fabe3c Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 1 Jun 2015 10:38:44 -1000 Subject: [PATCH 263/268] Consistant use of NULL --- .../posix/include/uavcan_posix/basic_file_server_backend.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 85f9058ed0..6c52d1769a 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -165,7 +165,7 @@ protected: return pi; } } - return 0; + return NULL; } FDCacheItem* add(FDCacheItem* pi) From 82d9bf29be9c14cb813881bf407a2892f89e7295 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Tue, 2 Jun 2015 11:03:11 -1000 Subject: [PATCH 264/268] Added Garbage collection closes #36 --- .../basic_file_server_backend.hpp | 70 ++++++++++++++----- 1 file changed, 51 insertions(+), 19 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 6c52d1769a..764a5a4e42 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -55,13 +56,21 @@ protected: return ::close(fd); } + + virtual void init() { } }; FDCacheBase fallback_; - class FDCache : public FDCacheBase + class FDCache : public FDCacheBase, protected uavcan::TimerBase { - enum { MaxAgeSeconds = 3 }; + /* Age in Seconds an entry will stay in the cache if not accessed. */ + + enum { MaxAgeSeconds = 7 }; + + /* Rate in Seconds that the cache will be flushed of stale entries. */ + + enum { GarbageCollectionSeconds = 60 }; class FDCacheItem { @@ -77,19 +86,19 @@ protected: enum { InvalidFD = -1 }; FDCacheItem() - : next_(NULL) - , last_access_(0) - , fd_(InvalidFD) - , oflags_(0) - , path_(0) + : next_(NULL), + last_access_(0), + fd_(InvalidFD), + oflags_(0), + path_(0) { } FDCacheItem(int fd, const char* path, int oflags) - : next_(NULL) - , last_access_(0) - , fd_(fd) - , oflags_(oflags) - , path_(strdup(path)) + : next_(NULL), + last_access_(0), + fd_(fd), + oflags_(oflags), + path_(strdup(path)) { } ~FDCacheItem() @@ -183,7 +192,7 @@ protected: if ((*pi)->expired()) { FDCacheItem* next = (*pi)->next_; - (void) FDCacheBase::close((*pi)->fd_); + (void)FDCacheBase::close((*pi)->fd_); delete (*pi); *pi = next; continue; @@ -207,21 +216,40 @@ protected: for (FDCacheItem* pi = head_; pi; pi = tmp) { tmp = pi->next_; - (void) FDCacheBase::close(pi->fd_); + (void)FDCacheBase::close(pi->fd_); delete pi; } } + /* Removed stale entries. In the normal case a node will read the + * complete contents of a file and the read of the last block will + * cause the method remove() to be invoked with done true. Thereby + * flushing the entry from the cache. But if the node does not + * stay the course of the read, it may leave a dangling entry. + * This call back handles the garbage collection. + */ + virtual void handleTimerEvent(const uavcan::TimerEvent& event) + { + removeExpired(&head_); + } + public: - FDCache() - : head_(NULL) + FDCache(uavcan::INode& node) : + TimerBase(node), + head_(NULL) { } virtual ~FDCache() { + stop(); clear(); } + virtual void init() + { + startPeriodic(uavcan::MonotonicDuration::fromMSec(GarbageCollectionSeconds * 1000)); + } + virtual int open(const char* path, int oflags) { int fd = FDCacheItem::InvalidFD; @@ -285,17 +313,20 @@ protected: }; FDCacheBase* fdcache_; + uavcan::INode& node_; FDCacheBase& getFDCache() { if (fdcache_ == NULL) { - fdcache_ = new FDCache(); + fdcache_ = new FDCache(node_); if (fdcache_ == NULL) { fdcache_ = &fallback_; } + + fdcache_->init(); } return *fdcache_; } @@ -393,8 +424,9 @@ protected: } public: - BasicFileSeverBackend() - : fdcache_(NULL) + BasicFileSeverBackend(uavcan::INode& node) : + fdcache_(NULL), + node_(node) { } ~BasicFileSeverBackend() From ac3e70f6ba1336e6a195907b6adbe23eb267c62f Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Jun 2015 01:32:10 +0300 Subject: [PATCH 265/268] Insignificant formatting fixes (uncrustify is not smart enough) --- .../uavcan_posix/basic_file_server_backend.hpp | 14 ++++++-------- 1 file changed, 6 insertions(+), 8 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 764a5a4e42..1615b06a6a 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -64,12 +64,10 @@ protected: class FDCache : public FDCacheBase, protected uavcan::TimerBase { - /* Age in Seconds an entry will stay in the cache if not accessed. */ - + /// Age in Seconds an entry will stay in the cache if not accessed. enum { MaxAgeSeconds = 7 }; - /* Rate in Seconds that the cache will be flushed of stale entries. */ - + /// Rate in Seconds that the cache will be flushed of stale entries. enum { GarbageCollectionSeconds = 60 }; class FDCacheItem @@ -85,16 +83,16 @@ protected: public: enum { InvalidFD = -1 }; - FDCacheItem() - : next_(NULL), + FDCacheItem() : + next_(NULL), last_access_(0), fd_(InvalidFD), oflags_(0), path_(0) { } - FDCacheItem(int fd, const char* path, int oflags) - : next_(NULL), + FDCacheItem(int fd, const char* path, int oflags) : + next_(NULL), last_access_(0), fd_(fd), oflags_(oflags), From 250837965eeb2395b930952e9d8b72fd2368ff08 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Jun 2015 01:49:54 +0300 Subject: [PATCH 266/268] Minor fixes to BasicFileSeverBackend --- .../uavcan_posix/basic_file_server_backend.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index 1615b06a6a..bec6d91fd7 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -70,15 +70,15 @@ protected: /// Rate in Seconds that the cache will be flushed of stale entries. enum { GarbageCollectionSeconds = 60 }; - class FDCacheItem + class FDCacheItem : uavcan::Noncopyable { friend FDCache; FDCacheItem* next_; time_t last_access_; - int fd_; - int oflags_; - const char* path_; + const int fd_; + const int oflags_; + const char* const path_; public: enum { InvalidFD = -1 }; @@ -88,7 +88,7 @@ protected: last_access_(0), fd_(InvalidFD), oflags_(0), - path_(0) + path_(NULL) { } FDCacheItem(int fd, const char* path, int oflags) : @@ -96,14 +96,14 @@ protected: last_access_(0), fd_(fd), oflags_(oflags), - path_(strdup(path)) + path_(::strndup(path, uavcan::protocol::file::Path::FieldTypes::path::MaxSize)) { } ~FDCacheItem() { if (valid()) { - delete path_; + ::free(const_cast(path_)); } } From e54a1ed75eb10a9eff90aa8ab129125751b7a0ba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Jun 2015 01:58:23 +0300 Subject: [PATCH 267/268] Typo --- libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp index 39634e313a..388e493881 100644 --- a/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -558,7 +558,7 @@ Options parseOptions(int argc, const char** argv) std::cerr << error_text << "\n" << "Usage:\n\t" << executable_name - << " [can-iface-name-N...] [-c ] -s ]" + << " [can-iface-name-N...] [-c ] -s " << std::endl; std::exit(1); } From 3d5f6f5d4035d52e62f750109af4db079cebef6d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 3 Jun 2015 02:20:36 +0300 Subject: [PATCH 268/268] test_file_server --- libuavcan_drivers/linux/CMakeLists.txt | 3 + .../linux/apps/test_file_server.cpp | 156 ++++++++++++++++++ .../basic_file_server_backend.hpp | 2 +- .../uavcan_posix/firmware_version_checker.hpp | 2 +- 4 files changed, 161 insertions(+), 2 deletions(-) create mode 100644 libuavcan_drivers/linux/apps/test_file_server.cpp diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index b33af01b8e..d4be18302c 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -63,6 +63,9 @@ 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}) +add_executable(test_file_server apps/test_file_server.cpp) +target_link_libraries(test_file_server ${UAVCAN_LIB} rt ${CMAKE_THREAD_LIBS_INIT}) + # # Tools # diff --git a/libuavcan_drivers/linux/apps/test_file_server.cpp b/libuavcan_drivers/linux/apps/test_file_server.cpp new file mode 100644 index 0000000000..9ea9851c73 --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_file_server.cpp @@ -0,0 +1,156 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include +#include +#include +#include "debug.hpp" +// UAVCAN +#include +// UAVCAN Linux drivers +#include +// UAVCAN POSIX drivers +#include +#include // Compilability test + +namespace +{ + +uavcan_linux::NodePtr initNode(const std::vector& 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; +} + +void runForever(const uavcan_linux::NodePtr& node) +{ + uavcan_posix::BasicFileSeverBackend backend(*node); + + uavcan::FileServer server(*node, backend); + + const int server_init_res = server.start(); + if (server_init_res < 0) + { + throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res)); + } + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(100)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + } +} + +struct Options +{ + uavcan::NodeID node_id; + std::vector ifaces; +}; + +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 + << " [can-iface-name-N...]" + << std::endl; + std::exit(1); + } + }; + + enforce(argc >= 2, "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 + { + enforce(false, "Unexpected argument"); + } + } + + return out; +} + +} + +int main(int argc, const char** argv) +{ + try + { + auto options = parseOptions(argc, argv); + + std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" + "Num ifaces: " << options.ifaces.size() << "\n" +#ifdef NDEBUG + "Build mode: Release" +#else + "Build mode: Debug" +#endif + << std::endl; + + auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_test_file_server"); + runForever(node); + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +} diff --git a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp index bec6d91fd7..11e291a955 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -226,7 +226,7 @@ protected: * stay the course of the read, it may leave a dangling entry. * This call back handles the garbage collection. */ - virtual void handleTimerEvent(const uavcan::TimerEvent& event) + virtual void handleTimerEvent(const uavcan::TimerEvent&) { removeExpired(&head_); } diff --git a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp index ff09260b8b..58143e1056 100644 --- a/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -221,7 +221,7 @@ protected: * @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, + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID, const uavcan::protocol::GetNodeInfo::Response& node_info, FirmwareFilePath& out_firmware_file_path) {