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/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/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..fb34c2757c --- /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[<=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 new file mode 100644 index 0000000000..30a966eb78 --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/server/220.AppendEntries.uavcan @@ -0,0 +1,33 @@ +# +# This type is a part of the Raft consensus algorithm. +# Please refer to the specification for details. +# + +# +# Given min election timeout and cluster size, the maximum recommended request interval can be derived as follows: +# max recommended request interval = (min election timeout) / 2 requests / (cluster size - 1) +# The equation assumes that the Leader requests one Follower at a time, so that there's at most one pending call +# at any moment. Such behavior is optimal as it creates uniform bus load, but it is actually implementation-specific. +# Obviously, request interval can be lower than that if needed, but higher values are not recommended as they may +# cause Followers to initiate premature elections in case of intensive frame losses or delays. +# +# Real timeout is randomized in the range (MIN, MAX], according to the Raft paper. +# +uint16 DEFAULT_MIN_ELECTION_TIMEOUT_MS = 4000 +uint16 DEFAULT_MAX_ELECTION_TIMEOUT_MS = 6000 + +uint32 term +uint32 prev_log_term +uint8 prev_log_index +uint8 leader_commit + +# +# 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 + +--- + +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..113c6ee3d2 --- /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 +uint32 last_log_term +uint8 last_log_index + +--- + +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 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/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/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/marshal/array.hpp b/libuavcan/include/uavcan/marshal/array.hpp index fb17b68dea..c7263d106e 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()); @@ -1073,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/node/abstract_node.hpp b/libuavcan/include/uavcan/node/abstract_node.hpp index cebdcec7fc..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(); } @@ -76,6 +74,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/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index 7dde549c00..d1c04e56e0 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -9,8 +9,8 @@ #include #include #include -#include #include +#include #include #include #include @@ -20,15 +20,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) { @@ -46,12 +45,11 @@ protected: MonotonicTime getTxDeadline() const; - IMarshalBuffer* getBuffer(unsigned byte_len); + int genericPublish(const StaticTransferBufferImpl& buffer, TransferType transfer_type, + NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); - 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 +64,7 @@ public: */ void allowAnonymousTransfers() { - sender_->allowAnonymousTransfers(); + sender_.allowAnonymousTransfers(); } INode& getNode() const { return node_; } @@ -80,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) ? @@ -88,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); @@ -124,12 +131,6 @@ public: { return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline); } - - TransferSender* getTransferSender() - { - (void)checkInit(); - return GenericPublisherBase::getTransferSender(); - } }; // ---------------------------------------------------------------------------- @@ -145,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); @@ -168,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/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index e3d51c9440..bb203788c6 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -33,26 +33,27 @@ 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 { if (_transfer_ == NULL) { - UAVCAN_ASSERT(0); return Ret(); } return (_transfer_->*Fun)(); } 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: @@ -139,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 { @@ -162,23 +166,24 @@ 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*)); protected: + struct ReceivedDataStructureSpec : public ReceivedDataStructure + { + ReceivedDataStructureSpec() { } + + ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) + : ReceivedDataStructure(arg_transfer) + { } + }; + explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) { } @@ -189,16 +194,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,19 +227,11 @@ protected: */ void stop() { + UAVCAN_TRACE("GenericSubscriber", "Stop; dtname=%s", DataSpec::getDataTypeFullName()); GenericSubscriberBase::stop(forwarder_); } 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_; } }; // ---------------------------------------------------------------------------- @@ -244,48 +246,51 @@ int GenericSubscriber::checkInit() { return 0; } + 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) { + ReceivedDataStructureSpec rx_struct(&transfer); + + /* + * Decoding into the temporary storage + */ BitStream bitstream(transfer); ScalarCodec codec(bitstream); - message_.setTransfer(&transfer); + const int decode_res = DataStruct::decode(rx_struct, 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); } template diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp deleted file mode 100644 index 35a7e0885a..0000000000 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ /dev/null @@ -1,92 +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 getDataLength() 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. - * This implementation provides the buffer large enough to - * serialize any UAVCAN data structure. - */ -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 getDataLength() 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 930ecf419d..fc8b43e9f9 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -10,13 +10,12 @@ #include #include #include -#include // High-level functionality available by default -#include #include #if !UAVCAN_TINY +# include # include # include # 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 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). */ template class UAVCAN_EXPORT Node : public INode @@ -73,23 +63,24 @@ class UAVCAN_EXPORT Node : public INode typedef PoolAllocator Allocator; Allocator pool_allocator_; - MarshalBufferProvider marsh_buf_; OutgoingTransferRegistry outgoing_trans_reg_; Scheduler scheduler_; - DataTypeInfoProvider proto_dtp_; NodeStatusProvider proto_nsp_; #if !UAVCAN_TINY + DataTypeInfoProvider proto_dtp_; Logger proto_logger_; RestartRequestServer proto_rrs_; 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; @@ -98,19 +89,18 @@ protected: #endif } - virtual IMarshalBufferProvider& getMarshalBufferProvider() { return marsh_buf_; } - 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) , proto_logger_(*this) , proto_rrs_(*this) , proto_tsp_(*this) #endif + , internal_failure_cnt_(0) , started_(false) { } @@ -139,6 +129,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. @@ -260,9 +252,8 @@ public: // ---------------------------------------------------------------------------- -template -int Node::start( +template +int Node::start( const TransferPriority priority) { if (started_) @@ -272,17 +263,17 @@ int Node -int Node:: +template +int Node:: checkNetworkCompatibility(NetworkCompatibilityCheckResult& result) { if (!started_) 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/node/scheduler.hpp b/libuavcan/include/uavcan/node/scheduler.hpp index 847f76b9d9..01e58eb877 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(); @@ -76,6 +77,17 @@ class UAVCAN_EXPORT Scheduler : Noncopyable MonotonicDuration cleanup_period_; bool inside_spin_; + struct InsideSpinSetter + { + Scheduler& owner; + InsideSpinSetter(Scheduler& o) + : owner(o) + { + owner.inside_spin_ = true; + } + ~InsideSpinSetter() { owner.inside_spin_ = false; } + }; + MonotonicTime computeDispatcherSpinDeadline(MonotonicTime spin_deadline) const; void pollCleanup(MonotonicTime mono_ts, uint32_t num_frames_processed_with_last_spin); @@ -90,10 +102,18 @@ public: /** * Spin until the deadline, or until some error occurs. + * This function will return strictly when the deadline is reached, even if there are unprocessed frames. * Returns negative error code. */ int spin(MonotonicTime deadline); + /** + * Non-blocking version of @ref spin() - spins until all pending frames and events are processed, + * or until some error occurs. If there's nothing to do, returns immediately. + * Returns negative error code. + */ + int spinOnce(); + DeadlineScheduler& getDeadlineScheduler() { return deadline_scheduler_; } Dispatcher& getDispatcher() { return dispatcher_; } diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b4c82774ba..97d0a55fb3 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,42 +21,83 @@ 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(); } }; /** * 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 -struct UAVCAN_EXPORT ServiceCallResult +class UAVCAN_EXPORT ServiceCallResult : Noncopyable { +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_; } + + /** + * Returned reference points to a stack-allocated object. + */ + const ResponseFieldType& getResponse() const { return response_; } + ResponseFieldType& getResponse() { return response_; } }; /** @@ -67,10 +109,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,64 +125,113 @@ static Stream& operator<<(Stream& s, const ServiceCallResult& scr) /** * Do not use directly. */ -class ServiceClientBase : protected DeadlineHandler +class ServiceClientBase : protected ITransferAcceptanceFilter + , protected DeadlineHandler { const DataTypeDescriptor* data_type_descriptor_; ///< This will be initialized at the time of first call protected: - MonotonicDuration request_timeout_; - bool pending_; + class CallState : DeadlineHandler + { + ServiceClientBase& owner_; + const ServiceCallID id_; + bool timed_out_; - explicit ServiceClientBase(INode& node) + virtual void handleDeadline(MonotonicTime); + + public: + CallState(INode& node, ServiceClientBase& owner, ServiceCallID call_id) + : DeadlineHandler(node.getScheduler()) + , owner_(owner) + , id_(call_id) + , timed_out_(false) + { + UAVCAN_ASSERT(id_.isValid()); + DeadlineHandler::startWithDelay(owner_.request_timeout_); + } + + ServiceCallID getCallID() const { return id_; } + + bool hasTimedOut() const { return timed_out_; } + + static bool hasTimedOutPredicate(const CallState& cs) { return cs.hasTimedOut(); } + + bool operator==(const CallState& rhs) const + { + return (&owner_ == &rhs.owner_) && (id_ == rhs.id_); + } + }; + + struct CallStateMatchingPredicate + { + const ServiceCallID id; + CallStateMatchingPredicate(ServiceCallID reference) : id(reference) { } + bool operator()(const CallState& state) const { return (state.getCallID() == id) && !state.hasTimedOut(); } + }; + + struct ServerSearchPredicate + { + const NodeID server_node_id; + ServerSearchPredicate(NodeID nid) : server_node_id(nid) { } + bool operator()(const CallState& state) const { return state.getCallID().server_node_id == server_node_id; } + }; + + MonotonicDuration request_timeout_; + + ServiceClientBase(INode& node) : DeadlineHandler(node.getScheduler()) , data_type_descriptor_(NULL) , request_timeout_(getDefaultRequestTimeout()) - , pending_(false) { } virtual ~ServiceClientBase() { } - int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, TransferID& out_transfer_id); + int prepareToCall(INode& node, const char* dtname, NodeID server_node_id, ServiceCallID& out_call_id); public: - /** - * Returns true if the service call is currently in progress. - */ - bool isPending() const { return pending_; } - /** * It's not recommended to override default timeouts. + * Change of this value will not affect pending calls. */ - static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(1000); } + static MonotonicDuration getDefaultRequestTimeout() { return MonotonicDuration::fromMSec(500); } static MonotonicDuration getMinRequestTimeout() { return MonotonicDuration::fromMSec(10); } static MonotonicDuration getMaxRequestTimeout() { return MonotonicDuration::fromMSec(60000); } - - /** - * Returns the service response waiting deadline, if pending. - */ - using DeadlineHandler::getDeadline; }; /** * Use this class to invoke services on remote nodes. * + * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent + * calls is limited only by amount of available pool memory. + * + * 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. * 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 - 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,16 +241,47 @@ 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; + 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()); + + typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized + + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), + rx_struct); // Mutable! + + owner.invokeCallback(result); + } + } + }; + + CallRegistry call_registry_; + 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); @@ -166,6 +289,8 @@ private: virtual void handleDeadline(MonotonicTime); + int addCallState(ServiceCallID call_id); + public: /** * @param node Node instance this client will be registered with. @@ -174,6 +299,7 @@ public: explicit ServiceClient(INode& node, const Callback& callback = Callback()) : SubscriberType(node) , ServiceClientBase(node) + , call_registry_(node.getAllocator()) , publisher_(node, getDefaultRequestTimeout()) , callback_(callback) { @@ -183,7 +309,7 @@ public: #endif } - virtual ~ServiceClient() { cancel(); } + virtual ~ServiceClient() { cancelAllCalls(); } /** * Shall be called before first use. @@ -199,21 +325,39 @@ public: * This method transmits the service request and returns immediately. * * Service response will be delivered into the application via the callback. - * Note that the callback will ALWAYS be called even if the service call has timed out; the + * Note that the callback will ALWAYS be called even if the service call times out; the * actual result of the call (success/failure) will be passed to the callback as well. * - * If this client instance is already pending service response, it will be cancelled and the new - * call will be performed. - * * Returns negative error code. */ int call(NodeID server_node_id, const RequestType& request); /** - * Cancel the pending service call. - * Does nothing if it is not pending. + * Same as plain @ref call() above, but this overload also returns the call ID of the new call. + * The call ID structure can be used to cancel this request later if needed. */ - void cancel(); + int call(NodeID server_node_id, const RequestType& request, ServiceCallID& out_call_id); + + /** + * Cancels certain call referred via call ID structure. + */ + void cancelCall(ServiceCallID call_id); + + /** + * Cancels all pending calls. + */ + void cancelAllCalls(); + + /** + * Checks whether there's currently a pending call addressed to the specified node ID. + */ + bool hasPendingCallToServer(NodeID server_node_id) const; + + /** + * This method allows to traverse pending calls. If the index is out of range, an invalid call ID will be returned. + * Warning: average complexity is O(index); worst case complexity is O(size). + */ + ServiceCallID getCallIDByIndex(unsigned index) const; /** * Service response callback must be set prior service call. @@ -221,6 +365,18 @@ public: const Callback& getCallback() const { return callback_; } void setCallback(const Callback& cb) { callback_ = cb; } + /** + * Complexity is O(N) of number of pending calls. + * Note that the number of pending calls will not be updated until the callback is executed. + */ + unsigned getNumPendingCalls() const { return call_registry_.getSize(); } + + /** + * Complexity is O(1). + * Note that the number of pending calls will not be updated until the callback is executed. + */ + bool hasPendingCalls() const { return !call_registry_.isEmpty(); } + /** * Returns the number of failed attempts to decode received response. Generally, a failed attempt means either: * - Transient failure in the transport layer. @@ -229,7 +385,7 @@ public: uint32_t getResponseFailureCount() const { return SubscriberType::getFailureCount(); } /** - * Request timeouts. + * Request timeouts. Note that changing the request timeout will not affect calls that are already pending. * There is no such config as TX timeout - TX timeouts are configured automagically according to request timeouts. * Not recommended to change. */ @@ -246,10 +402,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,118 +415,172 @@ 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 + + return NULL != call_registry_.find(CallStateMatchingPredicate(ServiceCallID(frame.getSrcNodeID(), + frame.getTransferID()))); + +} + +template +void ServiceClient:: +handleReceivedDataStruct(ReceivedDataStructure& response) { UAVCAN_ASSERT(response.getTransferType() == TransferTypeServiceResponse); - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) + + ServiceCallID call_id(response.getSrcNodeID(), response.getTransferID()); + cancelCall(call_id); + ServiceCallResultType result(ServiceCallResultType::Success, call_id, response); // Mutable! + invokeCallback(result); +} + + +template +void ServiceClient::handleDeadline(MonotonicTime) +{ + UAVCAN_TRACE("ServiceClient", "Shared deadline event received"); + /* + * Invoking callbacks for timed out call state objects. + */ + TimeoutCallbackCaller callback_caller(*this); + call_registry_.template forEach(callback_caller); + /* + * Removing timed out objects. + * This operation cannot be merged with the previous one because that will not work with recursive calls. + */ + call_registry_.removeAllWhere(&CallState::hasTimedOutPredicate); + /* + * Subscriber does not need to be registered if we don't have any pending calls. + * Removing it makes processing of incoming frames a bit faster. + */ + if (call_registry_.isEmpty()) { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ServiceCallResultType result(ServiceCallResultType::Success, erp.src_node_id, response); - cancel(); - invokeCallback(result); - } - else - { - UAVCAN_ASSERT(0); - cancel(); + SubscriberType::stop(); } } -template -void ServiceClient::handleDeadline(MonotonicTime) +template +int ServiceClient::addCallState(ServiceCallID call_id) { - const TransferListenerType* const listener = SubscriberType::getTransferListener(); - if (listener) + if (call_registry_.isEmpty()) { - const typename TransferListenerType::ExpectedResponseParams erp = listener->getExpectedResponseParams(); - ReceivedDataStructure& ref = SubscriberType::getReceivedStructStorage(); - ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, erp.src_node_id, ref); + const int subscriber_res = SubscriberType::startAsServiceResponseListener(); + if (subscriber_res < 0) + { + UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res); + return subscriber_res; + } + } - UAVCAN_TRACE("ServiceClient", "Timeout from nid=%i, dtname=%s", - erp.src_node_id.get(), DataType::getDataTypeFullName()); - cancel(); - invokeCallback(result); - } - else + if (NULL == call_registry_.template emplace(SubscriberType::getNode(), + *this, call_id)) { - UAVCAN_ASSERT(0); - cancel(); + SubscriberType::stop(); + return -ErrMemory; } + + return 0; } -template -int ServiceClient::call(NodeID server_node_id, const RequestType& request) +template +int ServiceClient::call(NodeID server_node_id, const RequestType& request) { - cancel(); - if (!isCallbackValid()) + 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; } /* * Common procedures that don't depend on the struct data type */ - TransferID transfer_id; const int prep_res = - prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, transfer_id); + prepareToCall(SubscriberType::getNode(), DataType::getDataTypeFullName(), server_node_id, out_call_id); if (prep_res < 0) { UAVCAN_TRACE("ServiceClient", "Failed to prepare the call, error: %i", prep_res); - cancel(); return prep_res; } /* - * Starting the subscriber + * Initializing the call state - this will start the subscriber ad-hoc */ - const int subscriber_res = SubscriberType::startAsServiceResponseListener(); - if (subscriber_res < 0) + const int call_state_res = addCallState(out_call_id); + if (call_state_res < 0) { - UAVCAN_TRACE("ServiceClient", "Failed to start the subscriber, error: %i", subscriber_res); - cancel(); - return subscriber_res; + UAVCAN_TRACE("ServiceClient", "Failed to add call state, error: %i", call_state_res); + return call_state_res; } /* - * Configuring the listener so it will accept only the matching response + * Configuring the listener so it will accept only the matching responses + * TODO move to init(), but this requires to somewhat refactor GenericSubscriber<> (remove TransferForwarder) */ TransferListenerType* const tl = SubscriberType::getTransferListener(); if (tl == NULL) { UAVCAN_ASSERT(0); // Must have been created - cancel(); + cancelCall(out_call_id); return -ErrLogic; } - const typename TransferListenerType::ExpectedResponseParams erp(server_node_id, transfer_id); - tl->setExpectedResponseParams(erp); + tl->installAcceptanceFilter(this); /* * Publishing the request */ - const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, transfer_id); + const int publisher_res = publisher_.publish(request, TransferTypeServiceRequest, server_node_id, + out_call_id.transfer_id); if (publisher_res < 0) { - cancel(); + cancelCall(out_call_id); + return publisher_res; } + + UAVCAN_ASSERT(server_node_id == out_call_id.server_node_id); return publisher_res; } -template -void ServiceClient::cancel() +template +void ServiceClient::cancelCall(ServiceCallID call_id) { - pending_ = false; - SubscriberType::stop(); - DeadlineHandler::stop(); - TransferListenerType* const tl = SubscriberType::getTransferListener(); - if (tl) + call_registry_.removeFirstWhere(CallStateMatchingPredicate(call_id)); + if (call_registry_.isEmpty()) { - tl->stopAcceptingAnything(); + SubscriberType::stop(); } } +template +void ServiceClient::cancelAllCalls() +{ + call_registry_.clear(); + SubscriberType::stop(); +} + +template +bool ServiceClient::hasPendingCallToServer(NodeID server_node_id) const +{ + 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/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index 6d5695451c..17076b4c5a 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -19,16 +19,61 @@ 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. * + * 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 * 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 +87,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,28 +120,37 @@ private: PublisherType publisher_; Callback callback_; uint32_t response_failure_count_; - ResponseType response_; virtual void handleReceivedDataStruct(ReceivedDataStructure& request) { UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); + + ServiceResponseDataStructure response; + if (try_implicit_cast(callback_, true)) { - response_ = ResponseType(); // The application needs newly initialized structure - callback_(request, response_); + UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default + callback_(request, response); } else { 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/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. 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/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..ad5238ffc8 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 UAVCAN_EXPORT 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/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..2c0aa98670 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/allocation_request_manager.hpp @@ -0,0 +1,278 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#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 +#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: + /** + * 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() { } +}; + +/** + * 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_; + + MonotonicTime last_message_timestamp_; + Allocation::FieldTypes::unique_id current_unique_id_; + + IAllocationRequestHandler& handler_; + IEventTracer& tracer_; + + Subscriber allocation_sub_; + Publisher allocation_pub_; + + enum { InvalidStage = 0 }; + + 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 = 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() == Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + if (msg.unique_id.size() < 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() >= (Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST * 2)) + { + return 3; + } + if (current_unique_id_.size() >= Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST) + { + return 2; + } + return InvalidStage; + } + + void publishFollowupAllocationResponse() + { + 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) + { + trace(TraceAllocationActivity, msg.getSrcNodeID().get()); + + if (!msg.isAnonymousTransfer()) + { + return; // This is a response from another allocator, 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(); + trace(TraceAllocationFollowupTimeout, (msg.getMonotonicTimestamp() - last_message_timestamp_).toUSec()); + } + + /* + * Checking if request stage matches the expected stage + */ + 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()); + UAVCAN_ASSERT(max_expected_bytes > 0); + if (msg.unique_id.size() > max_expected_bytes) + { + trace(TraceAllocationBadRequest, msg.unique_id.size()); + 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]); + } + + 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. + */ + 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(); + + { + uint64_t event_agrument = 0; + for (uint8_t i = 0; i < 8; i++) + { + event_agrument |= static_cast(unique_id[i]) << (56U - i * 8U); + } + trace(TraceAllocationExchangeComplete, static_cast(event_agrument)); + } + + handler_.handleAllocationRequest(unique_id, msg.node_id); + } + else + { + if (handler_.canPublishFollowupAllocationResponse()) + { + publishFollowupAllocationResponse(); + } + else + { + trace(TraceAllocationFollowupDenied, 0); + current_unique_id_.clear(); + } + } + + /* + * It is super important to update timestamp only if the request has been processed successfully. + */ + last_message_timestamp_ = msg.getMonotonicTimestamp(); + } + +public: + 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) + { } + + int init() + { + int res = allocation_pub_.init(); + if (res < 0) + { + 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) + { + return res; + } + allocation_sub_.allowAnonymousTransfers(); + + return 0; + } + + int broadcastAllocationResponse(const UniqueID& unique_id, NodeID allocated_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(); + + trace(TraceAllocationResponse, msg.node_id); + + return allocation_pub_.broadcast(msg); + } +}; + +} +} + +#endif // Include guard 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/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..c03957d14f --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/centralized/storage.hpp @@ -0,0 +1,253 @@ +/* + * 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; + + Entry() { } + + Entry(const NodeID nid, const UniqueID& uid) + : unique_id(uid) + , node_id(nid) + { } + + bool operator==(const Entry& rhs) const + { + return unique_id == rhs.unique_id && + node_id == rhs.node_id; + } + }; + +private: + IStorageBackend& storage_; + Entry entries_[Capacity]; + Size size_; + + static IStorageBackend::String getSizeKey() { return "size"; } + + static IStorageBackend::String makeEntryKey(Size index, const char* postfix) + { + IStorageBackend::String str; + // "0_foobar" + str.appendFormatted("%d", int(index)); + str += "_"; + str += postfix; + return str; + } + + int readEntryFromStorage(Size index, Entry& out_entry) + { + const StorageMarshaller io(storage_); + + // Unique ID + if (io.get(makeEntryKey(index, "unique_id"), out_entry.unique_id) < 0) + { + return -ErrFailure; + } + + // Node ID + uint32_t node_id = 0; + if (io.get(makeEntryKey(index, "node_id"), node_id) < 0) + { + return -ErrFailure; + } + if (node_id > NodeID::Max || node_id == 0) + { + return -ErrFailure; + } + out_entry.node_id = NodeID(static_cast(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; + } + + if (!node_id.isUnicast()) + { + return -ErrInvalidParam; + } + + Entry entry; + entry.node_id = node_id; + entry.unique_id = unique_id; + + // If next operations fail, we'll get a dangling entry, but it's absolutely OK. + int res = writeEntryToStorage(size_, entry); + if (res < 0) + { + return res; + } + + // Updating the size + StorageMarshaller io(storage_); + uint32_t new_size_index = size_ + 1U; + res = io.setAndGetBack(getSizeKey(), new_size_index); + if (res < 0) + { + return res; + } + if (new_size_index != size_ + 1U) + { + return -ErrFailure; + } + + entries_[size_] = entry; + size_++; + + return 0; + } + + /** + * Returns nullptr if there's no such entry. + */ + const Entry* findByNodeID(const NodeID node_id) const + { + for (Size i = 0; i < size_; i++) + { + if (entries_[i].node_id == node_id) + { + return &entries_[i]; + } + } + return NULL; + } + + /** + * Returns nullptr if there's no such entry. + */ + const Entry* findByUniqueID(const UniqueID& unique_id) const + { + for (Size i = 0; i < size_; i++) + { + if (entries_[i].unique_id == unique_id) + { + return &entries_[i]; + } + } + return NULL; + } + + Size getSize() const { return size_; } +}; + +} +} +} + +#endif // Include guard 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..822f757522 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed.hpp @@ -0,0 +1,20 @@ +/* + * 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 + +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/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..0fa18fc17d --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/cluster_manager.hpp @@ -0,0 +1,434 @@ +/* + * 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 +#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_; + + 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; + } + + virtual void handleTimerEvent(const TimerEvent&) + { + UAVCAN_ASSERT(num_known_servers_ < cluster_size_); + + tracer_.onEvent(TraceRaftDiscoveryBroadcast, 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::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::distributed::ClusterManager", "Discovery broadcst failed: %d", res); + getNode().registerInternalFailure("Raft discovery broadcast"); + } + + /* + * Termination condition + */ + if (isClusterDiscovered()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::ClusterManager", + "Discovery broadcasting timer stopped"); + stop(); + } + } + + void handleDiscovery(const ReceivedDataStructure& msg) + { + tracer_.onEvent(TraceRaftDiscoveryReceived, 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(TraceRaftBadClusterSizeReceived, msg.configured_cluster_size); + getNode().registerInternalFailure("Bad Raft cluster size"); + return; + } + + /* + * 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) + { } + + /** + * 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::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::distributed::ClusterManager", "Cluster size is invalid"); + return -ErrInvalidConfiguration; + } + 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::distributed::ClusterManager", "Failed to store cluster size"); + return -ErrFailure; + } + } + + tracer_.onEvent(TraceRaftClusterSizeInited, 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; + } + + /** + * 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. + */ + 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_); + } + } + + /** + * 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/log.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp new file mode 100644 index 0000000000..20f957b3e7 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/log.hpp @@ -0,0 +1,310 @@ +/* + * 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) + { } + + 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::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::distributed::Log", "Failed to read last index"); + return -ErrFailure; + } + } + if (value >= Capacity) + { + return -ErrFailure; + } + last_index_ = Index(value); + } + + tracer_.onEvent(TraceRaftLogLastIndexRestored, 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::distributed::Log", "Failed to read entry at index %u: %d", + unsigned(index), result); + return result; + } + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::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(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); + 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::distributed::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(TraceRaftLogRemove, 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::distributed::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..0152b1851e --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/persistent_state.hpp @@ -0,0 +1,237 @@ +/* + * 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) + { } + + /** + * 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() + { + /* + * Reading log + */ + int res = log_.init(); + if (res < 0) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::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::distributed::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::distributed::PersistentState", + "Failed to read current term: %d", res); + return res; + } + } + + tracer_.onEvent(TraceRaftCurrentTermRestored, current_term_); + + if (current_term_ < last_entry->term) + { + 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; + } + + /* + * 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::distributed::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::distributed::PersistentState", + "Failed to read votedFor: %d", res); + return res; + } + if (stored_voted_for > NodeID::Max) + { + return -ErrInvalidConfiguration; + } + voted_for_ = NodeID(uint8_t(stored_voted_for)); + } + + tracer_.onEvent(TraceRaftVotedForRestored, 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(TraceRaftCurrentTermUpdate, 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(TraceRaftVotedForUpdate, 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..f00fd1ec2f --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/raft_core.hpp @@ -0,0 +1,916 @@ +/* + * 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 +#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 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 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() { } +}; + +/** + * 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. + * + * 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 + */ +class RaftCore : private TimerBase +{ +public: + enum ServerState + { + ServerStateFollower, + ServerStateCandidate, + ServerStateLeader + }; + +private: + typedef MethodBinder&, + ServiceResponseDataStructure&)> + AppendEntriesCallback; + + typedef MethodBinder&)> + AppendEntriesResponseCallback; + + typedef MethodBinder&, + ServiceResponseDataStructure&)> + RequestVoteCallback; + + typedef MethodBinder&)> + RequestVoteResponseCallback; + + struct PendingAppendEntriesFields + { + Log::Index prev_log_index; + Log::Index num_entries; + + PendingAppendEntriesFields() + : prev_log_index(0) + , num_entries(0) + { } + }; + + /* + * Constants + */ + enum { MaxNumFollowers = ClusterManager::MaxClusterSize - 1 }; + + IEventTracer& tracer_; + IRaftLeaderMonitor& leader_monitor_; + + /* + * States + */ + PersistentState persistent_state_; + ClusterManager cluster_; + Log::Index commit_index_; + + MonotonicTime last_activity_timestamp_; + MonotonicDuration randomized_activity_timeout_; + 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_; + ServiceClient request_vote_client_; + + /* + * 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 checkInvariants() const + { + // Commit index + UAVCAN_ASSERT(commit_index_ <= persistent_state_.getLog().getLastIndex()); + + // Term + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex()) != NULL); + UAVCAN_ASSERT(persistent_state_.getLog().getEntryAtIndex(persistent_state_.getLog().getLastIndex())->term + <= persistent_state_.getCurrentTerm()); + + // Elections + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !request_vote_client_.hasPendingCalls() || + persistent_state_.getVotedFor() == getNode().getNodeID()); + UAVCAN_ASSERT(num_votes_received_in_this_campaign_ <= cluster_.getClusterSize()); + + // Transport + UAVCAN_ASSERT(append_entries_client_.getNumPendingCalls() <= 1); + UAVCAN_ASSERT(request_vote_client_.getNumPendingCalls() <= cluster_.getNumKnownServers()); + UAVCAN_ASSERT(server_state_ != ServerStateCandidate || !append_entries_client_.hasPendingCalls()); + UAVCAN_ASSERT(server_state_ != ServerStateLeader || !request_vote_client_.hasPendingCalls()); + UAVCAN_ASSERT(server_state_ != ServerStateFollower || + (!append_entries_client_.hasPendingCalls() && !request_vote_client_.hasPendingCalls())); + } + + void registerActivity() + { + last_activity_timestamp_ = getNode().getMonotonicTime(); + + static const int32_t randomization_range_msec = AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS - + AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS; + + const int32_t random_msec = (std::rand() % randomization_range_msec) + 1; + + randomized_activity_timeout_ = + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS + random_msec); + + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() > AppendEntries::Request::DEFAULT_MIN_ELECTION_TIMEOUT_MS); + UAVCAN_ASSERT(randomized_activity_timeout_.toMSec() <= AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS); + } + + bool isActivityTimedOut() const + { + return getNode().getMonotonicTime() > (last_activity_timestamp_ + randomized_activity_timeout_); + } + + void handlePersistentStateUpdateError(int error) + { + UAVCAN_ASSERT(error < 0); + trace(TraceRaftPersistStateUpdateError, error); + switchState(ServerStateFollower); + registerActivity(); // Deferring reelections + } + + void updateFollower() + { + if (isActivityTimedOut()) + { + switchState(ServerStateCandidate); + registerActivity(); + } + } + + void updateCandidate() + { + 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)); + + 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 < MaxNumFollowers; i++) + { + const NodeID node_id = cluster_.getRemoteServerNodeIDAtIndex(i); + if (!node_id.isUnicast()) + { + break; + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", + "Requesting vote from %d", int(node_id.get())); + trace(TraceRaftVoteRequestInitiation, node_id.get()); + + res = request_vote_client_.call(node_id, req); + if (res < 0) + { + trace(TraceError, res); + } + } + } + } + + void updateLeader() + { + if (append_entries_client_.hasPendingCalls()) + { + append_entries_client_.cancelAllCalls(); // Refer to the response callback to learn why + } + + if (cluster_.getClusterSize() > 1) + { + 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) + { + return; + } + + /* + * Logging + */ + UAVCAN_TRACE("dynamic_node_id_server::distributed::RaftCore", "State switch: %d --> %d", + int(server_state_), int(new_state)); + trace(TraceRaftStateSwitch, new_state); + + /* + * Updating the current state + */ + const ServerState old_state = server_state_; + server_state_ = new_state; + + /* + * Resetting specific states + */ + cluster_.resetAllServerIndices(); + + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; + + request_vote_client_.cancelAllCalls(); + append_entries_client_.cancelAllCalls(); + + /* + * 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); + } + } + + 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); + } + + 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()) + { + /* + * Not all local entries are committed. + * Deciding if it is safe to increment commit index. + */ + uint8_t num_nodes_with_next_log_entry_available = 1; // Local node + for (uint8_t i = 0; i < cluster_.getNumKnownServers(); i++) + { + 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 + leader_monitor_.handleLogCommitOnLeader(*persistent_state_.getLog().getEntryAtIndex(commit_index_)); + } + } + } + + void handleAppendEntriesRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + checkInvariants(); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + if (cluster_.isClusterDiscovered()) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + else + { + cluster_.addServer(request.getSrcNodeID()); + } + } + + 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) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + 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; + } + + registerActivity(); + 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 handleAppendEntriesResponse(const ServiceCallResult& result) + { + UAVCAN_ASSERT(server_state_ == ServerStateLeader); // When state switches, all requests must be cancelled + checkInvariants(); + + if (!result.isSuccessful()) + { + return; + } + + if (result.getResponse().term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.getResponse().term); + } + else + { + if (result.getResponse().success) + { + 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.getCallID().server_node_id); + trace(TraceRaftAppendEntriesRespUnsucfl, result.getCallID().server_node_id.get()); + } + } + + pending_append_entries_fields_ = PendingAppendEntriesFields(); + // Rest of the logic is implemented in periodic update handlers. + } + + void handleRequestVoteRequest(const ReceivedDataStructure& request, + ServiceResponseDataStructure& response) + { + checkInvariants(); + trace(TraceRaftVoteRequestReceived, request.getSrcNodeID().get()); + + if (!cluster_.isKnownServer(request.getSrcNodeID())) + { + trace(TraceRaftRequestIgnored, request.getSrcNodeID().get()); + response.setResponseEnabled(false); + return; + } + + 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()) + { + switchState(ServerStateFollower); // Our term is stale, so we can't serve as leader + + int res = persistent_state_.setCurrentTerm(request.term); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + return; + } + + res = persistent_state_.resetVotedFor(); + if (res < 0) + { + handlePersistentStateUpdateError(res); + response.setResponseEnabled(false); + 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) + { + switchState(ServerStateFollower); // Avoiding race condition when Candidate + 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 + checkInvariants(); + + if (!result.isSuccessful()) + { + return; + } + + trace(TraceRaftVoteRequestSucceeded, result.getCallID().server_node_id.get()); + + if (result.getResponse().term > persistent_state_.getCurrentTerm()) + { + tryIncrementCurrentTermFromResponse(result.getResponse().term); + } + else + { + if (result.getResponse().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&) + { + checkInvariants(); + + 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, + IRaftLeaderMonitor& leader_monitor) + : TimerBase(node) + , 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()) + , randomized_activity_timeout_( + MonotonicDuration::fromMSec(AppendEntries::Request::DEFAULT_MAX_ELECTION_TIMEOUT_MS)) + , server_state_(ServerStateFollower) + , next_server_index_(0) + , num_votes_received_in_this_campaign_(0) + , append_entries_srv_(node) + , append_entries_client_(node) + , request_vote_srv_(node) + , 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(uint8_t cluster_size = ClusterManager::ClusterSizeUnknown) + { + /* + * Initializing state variables + */ + server_state_ = ServerStateFollower; + next_server_index_ = 0; + num_votes_received_in_this_campaign_ = 0; + commit_index_ = 0; + + registerActivity(); + + /* + * 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)); + + res = request_vote_client_.init(); + if (res < 0) + { + return res; + } + request_vote_client_.setCallback(RequestVoteResponseCallback(this, &RaftCore::handleRequestVoteResponse)); + + /* + * Initializing timing constants + * Refer to the specification for the formula + */ + const uint8_t num_followers = static_cast(cluster_.getClusterSize() - 1); + + const MonotonicDuration update_interval = + 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())); + + 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; + } + + /** + * This function is mostly needed for testing. + */ + 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(). + */ + bool isLeader() const { return server_state_ == ServerStateLeader; } + + /** + * Inserts one entry into log. + * 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. + */ + void 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); + const int res = persistent_state_.getLog().append(entry); + if (res < 0) + { + handlePersistentStateUpdateError(res); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * 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 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 + 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(); + } + + Log::Index getNumAllocations() const + { + // Remember that index zero contains a special-purpose entry that doesn't count as allocation + return persistent_state_.getLog().getLastIndex(); + } + + /** + * These accessors are needed for debugging, visualization and testing. + */ + const PersistentState& getPersistentState() const { return persistent_state_; } + const ClusterManager& getClusterManager() const { return cluster_; } + MonotonicTime getLastActivityTimestamp() const { return last_activity_timestamp_; } + ServerState getServerState() const { return server_state_; } + MonotonicDuration getUpdateInterval() const { return getPeriod(); } + MonotonicDuration getRandomizedTimeout() const { return randomized_activity_timeout_; } +}; + +} +} +} + +#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..a8affdb8a2 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/distributed/server.hpp @@ -0,0 +1,375 @@ +/* + * 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 +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +namespace distributed +{ +/** + * This class implements the top-level allocation logic and server API. + */ +class UAVCAN_EXPORT Server : IAllocationRequestHandler + , INodeDiscoveryHandler + , IRaftLeaderMonitor +{ + 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(); + } + }; + + /* + * Constants + */ + UniqueID own_unique_id_; + + /* + * States + */ + INode& node_; + IEventTracer& tracer_; + RaftCore raft_core_; + AllocationRequestManager allocation_request_manager_; + NodeDiscoverer node_discoverer_; + + /* + * 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 + { + 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()) + { + raft_core_.appendLog(uid, 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); + } + + 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()) + { + raft_core_.appendLog(own_unique_id_, node_.getNodeID()); + } + } + + /* + * Private methods + */ + bool isNodeIDTaken(const NodeID node_id) const + { + 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)); + } + + void allocateNewNode(const UniqueID& unique_id, const NodeID preferred_node_id) + { + const NodeID allocated_node_id = + NodeIDSelector(this, &Server::isNodeIDTaken).findFreeNodeID(preferred_node_id); + if (!allocated_node_id.isUnicast()) + { + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "Request ignored - no free node ID left"); + return; + } + + UAVCAN_TRACE("dynamic_node_id_server::distributed::Server", "New node ID allocated: %d", + int(allocated_node_id.get())); + raft_core_.appendLog(unique_id, allocated_node_id); + } + + 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) + { + tracer_.onEvent(TraceError, res); + node_.registerInternalFailure("Dynamic allocation response"); + } + } + +public: + Server(INode& node, + 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) + { } + + 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) + { + return res; + } + + res = node_discoverer_.init(); + if (res < 0) + { + return res; + } + + return 0; + } + + Log::Index getNumAllocations() const { return raft_core_.getNumAllocations(); } + + /** + * These accessors are needed for debugging, visualization and testing. + */ + const RaftCore& getRaftCore() const { return raft_core_; } + const NodeDiscoverer& getNodeDiscoverer() const { return node_discoverer_; } +}; + +/** + * This structure represents immediate state of the server. + * It can be used for state visualization and debugging. + */ +struct StateReport +{ + uint8_t cluster_size; + + RaftCore::ServerState state; + + Log::Index last_log_index; + Log::Index commit_index; + + Term last_log_term; + Term current_term; + + NodeID voted_for; + + MonotonicTime last_activity_timestamp; + MonotonicDuration randomized_timeout; + + uint8_t num_unknown_nodes; + + struct FollowerState + { + NodeID node_id; + Log::Index next_index; + Log::Index match_index; + + FollowerState() + : next_index(0) + , match_index(0) + { } + } followers[ClusterManager::MaxClusterSize - 1]; + + StateReport(const Server& s) + : cluster_size (s.getRaftCore().getClusterManager().getClusterSize()) + , state (s.getRaftCore().getServerState()) + , last_log_index (s.getRaftCore().getPersistentState().getLog().getLastIndex()) + , commit_index (s.getRaftCore().getCommitIndex()) + , last_log_term (0) // See below + , current_term (s.getRaftCore().getPersistentState().getCurrentTerm()) + , voted_for (s.getRaftCore().getPersistentState().getVotedFor()) + , last_activity_timestamp(s.getRaftCore().getLastActivityTimestamp()) + , randomized_timeout (s.getRaftCore().getRandomizedTimeout()) + , num_unknown_nodes (s.getNodeDiscoverer().getNumUnknownNodes()) + { + const Entry* const e = s.getRaftCore().getPersistentState().getLog().getEntryAtIndex(last_log_index); + UAVCAN_ASSERT(e != NULL); + if (e != NULL) + { + last_log_term = e->term; + } + + for (uint8_t i = 0; i < (cluster_size - 1U); i++) + { + const ClusterManager& mgr = s.getRaftCore().getClusterManager(); + const NodeID node_id = mgr.getRemoteServerNodeIDAtIndex(i); + if (node_id.isUnicast()) + { + followers[i].node_id = node_id; + followers[i].next_index = mgr.getServerNextIndex(node_id); + followers[i].match_index = mgr.getServerMatchIndex(node_id); + } + } + } +}; + +} +} +} + +#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/event.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp new file mode 100644 index 0000000000..d447e01ac6 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/event.hpp @@ -0,0 +1,175 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_EVENT_HPP_INCLUDED + +#include +#include + +namespace uavcan +{ +namespace dynamic_node_id_server +{ +/** + * @ref IEventTracer. + * Event codes cannot be changed, only new ones can be added. + */ +enum UAVCAN_EXPORT TraceCode +{ + // Event name Argument + // 0 + TraceError, // error code (may be negated) + TraceRaftLogLastIndexRestored, // recovered last index value + TraceRaftLogAppend, // index of new entry + TraceRaftLogRemove, // new last index value + TraceRaftCurrentTermRestored, // current term + // 5 + TraceRaftCurrentTermUpdate, // current term + TraceRaftVotedForRestored, // value of votedFor + TraceRaftVotedForUpdate, // value of votedFor + TraceRaftDiscoveryBroadcast, // number of known servers + TraceRaftNewServerDiscovered, // node ID of the new server + // 10 + 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 + Trace0, + 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) + TraceRaftElectionComplete, // number of votes collected + TraceRaftAppendEntriesRespUnsucfl, // node ID of the client + 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 big endian + TraceAllocationResponse, // allocated node ID + TraceAllocationActivity, // source node ID of the message + 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 + TraceDiscoveryNodeRestartDetected, // node ID + TraceDiscoveryNodeRemoved, // node ID + Trace22, + // 50 + + NumTraceCodes +}; + +/** + * This interface allows the application to trace events that happen in the server. + */ +class UAVCAN_EXPORT 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;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", + "RaftLogLastIndexRestored", + "RaftLogAppend", + "RaftLogRemove", + "RaftCurrentTermRestored", + "RaftCurrentTermUpdate", + "RaftVotedForRestored", + "RaftVotedForUpdate", + "RaftDiscoveryBroadcast", + "RaftNewServerDiscovered", + "RaftDiscoveryReceived", + "RaftClusterSizeInited", + "RaftBadClusterSizeReceived", + "RaftCoreInited", + "RaftStateSwitch", + "", + "RaftNewLogEntry", + "RaftRequestIgnored", + "RaftVoteRequestReceived", + "RaftVoteRequestSucceeded", + "RaftVoteRequestInitiation", + "RaftPersistStateUpdateError", + "RaftCommitIndexUpdate", + "RaftNewerTermInResponse", + "RaftNewEntryCommitted", + "RaftAppendEntriesCallFailure", + "RaftElectionComplete", + "RaftAppendEntriesRespUnsucfl", + "", + "", + "AllocationFollowupResponse", + "AllocationFollowupDenied", + "AllocationFollowupTimeout", + "AllocationBadRequest", + "AllocationUnexpectedStage", + "AllocationRequestAccepted", + "AllocationExchangeComplete", + "AllocationResponse", + "AllocationActivity", + "", + "DiscoveryNewNodeFound", + "DiscoveryCommitCacheUpdated", + "DiscoveryNodeFinalized", + "DiscoveryGetNodeInfoFailure", + "DiscoveryTimerStart", + "DiscoveryTimerStop", + "DiscoveryGetNodeInfoRequest", + "DiscoveryNodeRestartDetected", + "DiscoveryNodeRemoved", + "" + }; + 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. + * 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/node_discoverer.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp new file mode 100644 index 0000000000..e5e4da5e1f --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_discoverer.hpp @@ -0,0 +1,350 @@ +/* + * 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 +#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 = 5 }; + + enum { TimerPollIntervalMs = 170 }; // ~ ceil(500 ms service timeout / 3) + + enum { NumNodeStatusStaticReceivers = 64 }; + + /* + * 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 + + ServiceClient get_node_info_client_; + Subscriber node_status_sub_; + + /* + * Methods + */ + void trace(TraceCode code, int64_t argument) { tracer_.onEvent(code, argument); } + + 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 + { + // 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) + { + 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) + { + removeNode(node_id); + return false; + } + else if (awareness == INodeDiscoveryHandler::NodeAwarenessKnownAndCommitted) + { + trace(TraceDiscoveryCommitCacheUpdated, node_id.get()); + committed_node_mask_[node_id.get()] = true; + removeNode(node_id); + return false; + } + else + { + UAVCAN_ASSERT(0); + return false; + } + } + + NodeID pickNextNodeToQueryAndCleanupMap() + { + NodeID node_id; + do + { + node_id = pickNextNodeToQuery(); + if (node_id.isUnicast()) + { + if (needToQuery(node_id)) + { + return node_id; + } + else + { + removeNode(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)); + 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 + * duplicate node ID in the log. + */ + if (needToQuery(node_id)) + { + 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.getCallID().server_node_id.get())); + finalizeNodeDiscovery(&result.getResponse().hardware_version.unique_id, result.getCallID().server_node_id); + } + else + { + trace(TraceDiscoveryGetNodeInfoFailure, result.getCallID().server_node_id.get()); + + NodeData* const data = node_map_.access(result.getCallID().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.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.getCallID().server_node_id); + // Warning: data pointer is invalidated now + } + } + } + + void handleTimerEvent(const TimerEvent&) + { + if (get_node_info_client_.hasPendingCalls()) + { + return; + } + + const NodeID node_id = pickNextNodeToQueryAndCleanupMap(); + if (!node_id.isUnicast()) + { + trace(TraceDiscoveryTimerStop, 0); + stop(); + 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", + 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) + { + trace(TraceDiscoveryNewNodeFound, 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) + { + trace(TraceDiscoveryNodeRestartDetected, msg.getSrcNodeID().get()); + data->num_get_node_info_attempts = 0; + } + data->last_seen_uptime = msg.uptime_sec; + + if (!isRunning()) + { + startPeriodic(MonotonicDuration::fromMSec(TimerPollIntervalMs)); + trace(TraceDiscoveryTimerStart, getPeriod().toUSec()); + } + } + +public: + 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) + { } + + 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(); } + + /** + * 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()); } +}; + +} +} + +#endif // Include guard 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..bd0b55c0f9 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_server/node_id_selector.hpp @@ -0,0 +1,71 @@ +/* + * 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) const + { + uint8_t candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; + + // Up + while (candidate <= NodeID::MaxRecommendedForRegularNodes) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate++; + } + + candidate = preferred.isUnicast() ? preferred.get() : NodeID::MaxRecommendedForRegularNodes; + + // Down + while (candidate > 0) + { + if (!(owner_->*is_node_id_taken_)(candidate)) + { + return candidate; + } + candidate--; + } + + return NodeID(); + } +}; + +} +} + +#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..9e2395c92a --- /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 UAVCAN_EXPORT 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 MakeString::Type 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..09870d9192 --- /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_STORAGE_MARSHALLER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_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..360a93aee6 --- /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_TYPES_HPP_INCLUDED +#define UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_SERVER_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/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp new file mode 100644 index 0000000000..f1ca877cd8 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -0,0 +1,268 @@ +/* + * 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. + * Note that error codes returned by these methods are defined in uavcan.protocol.file.Error; these are + * not the same as libuavcan-internal error codes defined in uavcan.error.hpp. + */ +class UAVCAN_EXPORT IFileServerBackend +{ +public: + typedef protocol::file::Path::FieldTypes::path Path; + typedef protocol::file::EntryType EntryType; + typedef protocol::file::Error Error; + + /** + * 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. + * Refer to uavcan.protocol.file.EntryType for the list of available bit flags. + * Implementation of this method is required. + * On success the method must return zero. + */ + virtual int16_t getInfo(const Path& path, uint64_t& out_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 uint64_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 uint64_t offset, const uint8_t* buffer, const uint16_t size) + { + (void)path; + (void)offset; + (void)buffer; + (void)size; + return Error::NOT_IMPLEMENTED; + } + + /** + * Backend for uavcan.protocol.file.Delete. ('delete' is a C++ keyword, so 'remove' is used instead) + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t remove(const Path& path) + { + (void)path; + return Error::NOT_IMPLEMENTED; + } + + /** + * Backend for uavcan.protocol.file.GetDirectoryEntryInfo. + * Refer to uavcan.protocol.file.EntryType for the list of available bit flags. + * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. + * On success the method must return zero. + */ + virtual int16_t getDirectoryEntryInfo(const Path& directory_path, const uint32_t entry_index, + EntryType& out_type, Path& out_entry_full_path) + { + (void)directory_path; + (void)entry_index; + (void)out_type; + (void)out_entry_full_path; + return Error::NOT_IMPLEMENTED; + } + + virtual ~IFileServerBackend() { } +}; + +/** + * Basic file server implements only the following services: + * uavcan.protocol.file.GetInfo + * uavcan.protocol.file.Read + * Also see @ref IFileServerBackend. + */ +class BasicFileServer +{ + typedef MethodBinder + 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.size, resp.entry_type); + } + + void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp) + { + uint16_t inout_size = resp.data.capacity(); + + resp.data.resize(inout_size); + + resp.error.value = backend_.read(req.path.path, req.offset, resp.data.begin(), inout_size); + + if (resp.error.value != protocol::file::Error::OK) + { + inout_size = 0; + } + + if (inout_size > resp.data.capacity()) + { + UAVCAN_ASSERT(0); + resp.error.value = protocol::file::Error::UNKNOWN_ERROR; + } + else + { + resp.data.resize(inout_size); + } + } + +protected: + IFileServerBackend& backend_; ///< Derived types can use it + +public: + BasicFileServer(INode& node, IFileServerBackend& backend) + : get_info_srv_(node) + , read_srv_(node) + , backend_(backend) + { } + + int start() + { + int res = get_info_srv_.start(GetInfoCallback(this, &BasicFileServer::handleGetInfo)); + if (res < 0) + { + return res; + } + + res = read_srv_.start(ReadCallback(this, &BasicFileServer::handleRead)); + if (res < 0) + { + return res; + } + + return 0; + } +}; + +/** + * Full file server implements all file services: + * uavcan.protocol.file.GetInfo + * uavcan.protocol.file.Read + * uavcan.protocol.file.Write + * uavcan.protocol.file.Delete + * uavcan.protocol.file.GetDirectoryEntryInfo + * Also see @ref IFileServerBackend. + */ +class FileServer : protected BasicFileServer +{ + typedef MethodBinder + 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/include/uavcan/protocol/firmware_update_trigger.hpp b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp new file mode 100644 index 0000000000..788536c07c --- /dev/null +++ b/libuavcan/include/uavcan/protocol/firmware_update_trigger.hpp @@ -0,0 +1,476 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#ifndef UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_HPP_INCLUDED +#define UAVCAN_PROTOCOL_FIRMWARE_UPDATE_TRIGGER_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 shouldRequestFirmwareUpdate(NodeID node_id, const protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) = 0; + + /** + * This method will be invoked when a node responds to the update request with an error. If the request simply + * times out, this method will not be invoked. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting + * is not needed anymore. This method will not be invoked. + * + * @param node_id Node ID that returned this error. + * + * @param error_response Contents of the error response. It contains error code and text. + * + * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be + * initialized with old path, so if the same path needs to be reused, this + * argument should be left unchanged. + * + * @return True - the class will continue sending update requests with new firmware path. + * False - the node will be forgotten, new requests will not be sent. + */ + virtual bool shouldRetryFirmwareUpdate(NodeID node_id, + const protocol::file::BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) = 0; + + /** + * This node is invoked when the node responds to the update request with confirmation. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * Implementation is optional; default one does nothing. + * + * @param node_id Node ID that confirmed the request. + * + * @param response Actual response. + */ + virtual void handleFirmwareUpdateConfirmation(NodeID node_id, + const protocol::file::BeginFirmwareUpdate::Response& response) + { + (void)node_id; + (void)response; + } + + virtual ~IFirmwareVersionChecker() { } +}; + +/** + * This class subscribes to updates from @ref NodeInfoRetriever in order to detect nodes that need firmware + * updates. The decision process of whether a firmware update is needed is relayed to the application via + * @ref IFirmwareVersionChecker. If the application confirms that the update is needed, this class will begin + * sending uavcan.protocol.file.BeginFirmwareUpdate periodically (period is configurable) to every node that + * needs an update in a round-robin fashion. There are the following termination conditions for the periodical + * sending process: + * + * - The node responds with confirmation. In this case the class will forget about the node on the assumption + * that its job is done here. Confirmation will be reported to the application via the interface. + * + * - The node responds with an error, and the error code is not ERROR_IN_PROGRESS. In this case the class will + * request the application via the interface mentioned above about its further actions - either give up or + * retry, possibly with a different firmware. + * + * - The node responds with error ERROR_IN_PROGRESS. In this case the class behaves exactly in the same way as if + * response was successful (because the firmware is alredy being updated, so the goal is fulfilled). + * Confirmation will be reported to the application via the interface. + * + * - The node goes offline or restarts. In this case the node will be immediately forgotten, and the process + * will repeat again later because the node info retriever re-queries GetNodeInfo every time when a node restarts. + * + * Since the target node (i.e. node that is being updated) will try to retrieve the specified firmware file using + * the file services (uavcan.protocol.file.*), the provided firmware path must be reachable for the file server + * (@ref FileServer, @ref BasicFileServer). Normally, an application that serves as UAVCAN firmware update server + * will include at least the following components: + * - this firmware update trigger; + * - dynamic node ID allocation server; + * - file server. + * + * Implementation details: the class uses memory pool to keep the list of nodes that have not responded yet, which + * limits the maximum length of the path to the firmware file, which is covered in @ref IFirmwareVersionChecker. + * To somewhat relieve the maximum path length limitation, the class can be supplied with a common prefix that + * will be prepended to firmware pathes before sending requests. + * Interval at which requests are being sent is configurable, but the default value should cover the needs of + * virtually all use cases (as always). + */ +class FirmwareUpdateTrigger : public INodeInfoListener, + private TimerBase +{ + typedef MethodBinder&)> + BeginFirmwareUpdateResponseCallback; + + typedef IFirmwareVersionChecker::FirmwareFilePath FirmwareFilePath; + + enum { DefaultRequestIntervalMs = 1000 }; ///< Shall not be less than default service response timeout. + + struct NextNodeIDSearchPredicate : ::uavcan::Noncopyable + { + enum { DefaultOutput = 0xFFU }; + + const FirmwareUpdateTrigger& owner; + uint8_t output; + + NextNodeIDSearchPredicate(const FirmwareUpdateTrigger& arg_owner) + : owner(arg_owner) + , output(DefaultOutput) + { } + + bool operator()(const NodeID node_id, const FirmwareFilePath&) + { + if (node_id.get() > owner.last_queried_node_id_ && + !owner.begin_fw_update_client_.hasPendingCallToServer(node_id)) + { + output = min(output, node_id.get()); + } + return false; + } + }; + + /* + * State + */ + ServiceClient 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 node_id) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d could not provide GetNodeInfo response", int(node_id.get())); + pending_nodes_.remove(node_id); // For extra paranoia + } + + virtual void handleNodeInfoRetrieved(const NodeID node_id, const protocol::GetNodeInfo::Response& node_info) + { + FirmwareFilePath firmware_file_path; + const bool update_needed = checker_.shouldRequestFirmwareUpdate(node_id, node_info, firmware_file_path); + if (update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d requires update; file path: %s", + int(node_id.get()), firmware_file_path.c_str()); + trySetPendingNode(node_id, firmware_file_path); + } + else + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d does not need update", int(node_id.get())); + pending_nodes_.remove(node_id); + } + } + + virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event) + { + if (event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE || + !event.status.known) + { + pending_nodes_.remove(event.node_id); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node ID %d is offline hence forgotten", int(event.node_id.get())); + } + } + + /* + * Own methods + */ + INode& getNode() { return begin_fw_update_client_.getNode(); } + + void trySetPendingNode(const NodeID node_id, const FirmwareFilePath& path) + { + if (NULL != pending_nodes_.insert(node_id, path)) + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer started"); + } + } + else + { + getNode().registerInternalFailure("FirmwareUpdateTrigger OOM"); + } + } + + NodeID pickNextNodeID() const + { + // We can't do index search because indices are unstable in Map<> + // First try - from the current node up + NextNodeIDSearchPredicate s1(*this); + (void)pending_nodes_.find(s1); + + if (s1.output != NextNodeIDSearchPredicate::DefaultOutput) + { + last_queried_node_id_ = s1.output; + } + else if (last_queried_node_id_ != 0) + { + // Nothing was found, resetting the selector and trying again + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node selector reset, last value: %d", int(last_queried_node_id_)); + last_queried_node_id_ = 0; + + NextNodeIDSearchPredicate s2(*this); + (void)pending_nodes_.find(s2); + + if (s2.output != NextNodeIDSearchPredicate::DefaultOutput) + { + last_queried_node_id_ = s2.output; + } + } + else + { + ; // Hopeless + } + UAVCAN_TRACE("FirmwareUpdateTrigger", "Next node ID to query: %d, pending nodes: %u, pending calls: %u", + int(last_queried_node_id_), pending_nodes_.getSize(), + begin_fw_update_client_.getNumPendingCalls()); + return last_queried_node_id_; + } + + void handleBeginFirmwareUpdateResponse(const ServiceCallResult& result) + { + if (!result.isSuccessful()) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d has timed out, will retry", + int(result.getCallID().server_node_id.get())); + return; + } + + FirmwareFilePath* const old_path = pending_nodes_.access(result.getCallID().server_node_id); + if (old_path == NULL) + { + // The entry has been removed, assuming that it's not needed anymore + return; + } + + const bool confirmed = + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_OK || + result.getResponse().error == protocol::file::BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + + if (confirmed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d confirmed the update request", + int(result.getCallID().server_node_id.get())); + pending_nodes_.remove(result.getCallID().server_node_id); + checker_.handleFirmwareUpdateConfirmation(result.getCallID().server_node_id, result.getResponse()); + } + else + { + UAVCAN_ASSERT(old_path != NULL); + UAVCAN_ASSERT(TimerBase::isRunning()); + // We won't have to call trySetPendingNode(), because we'll directly update the old path via the pointer + + const bool update_needed = + checker_.shouldRetryFirmwareUpdate(result.getCallID().server_node_id, result.getResponse(), *old_path); + + if (!update_needed) + { + UAVCAN_TRACE("FirmwareUpdateTrigger", "Node %d does not need retry", + int(result.getCallID().server_node_id.get())); + pending_nodes_.remove(result.getCallID().server_node_id); + } + } + } + + virtual void handleTimerEvent(const TimerEvent&) + { + if (pending_nodes_.isEmpty()) + { + TimerBase::stop(); + UAVCAN_TRACE("FirmwareUpdateTrigger", "Timer stopped"); + return; + } + + const NodeID node_id = pickNextNodeID(); + if (!node_id.isUnicast()) + { + return; + } + + FirmwareFilePath* const path = pending_nodes_.access(node_id); + if (path == NULL) + { + UAVCAN_ASSERT(0); // pickNextNodeID() returned a node ID that is not present in the map + return; + } + + protocol::file::BeginFirmwareUpdate::Request req; + + req.source_node_id = getNode().getNodeID().get(); + if (!common_path_prefix_.empty()) + { + req.image_file_remote_path.path += common_path_prefix_.c_str(); + req.image_file_remote_path.path.push_back(protocol::file::Path::SEPARATOR); + } + req.image_file_remote_path.path += path->c_str(); + + UAVCAN_TRACE("FirmwareUpdateTrigger", "Request to %d with path: %s", + int(node_id.get()), req.image_file_remote_path.path.c_str()); + + const int call_res = begin_fw_update_client_.call(node_id, req); + if (call_res < 0) + { + getNode().registerInternalFailure("FirmwareUpdateTrigger call"); + } + } + +public: + FirmwareUpdateTrigger(INode& node, IFirmwareVersionChecker& checker) + : TimerBase(node) + , begin_fw_update_client_(node) + , checker_(checker) + , node_info_retriever_(NULL) + , pending_nodes_(node.getAllocator()) + , request_interval_(MonotonicDuration::fromMSec(DefaultRequestIntervalMs)) + , last_queried_node_id_(0) + { } + + ~FirmwareUpdateTrigger() + { + if (node_info_retriever_ != NULL) + { + node_info_retriever_->removeListener(this); + } + } + + /** + * Starts the object. Once started, it can't be stopped unless destroyed. + * + * @param node_info_retriever The object will register itself against this retriever. + * When the destructor is called, the object will unregister itself. + * + * @param common_path_prefix If set, this path will be prefixed to all firmware pathes provided by the + * application interface. The prefix does not need to end with path separator; + * the last trailing one will be removed (so use '//' if you need '/'). + * By default the prefix is empty. + * + * @return Negative error code. + */ + int start(NodeInfoRetriever& node_info_retriever, + const FirmwareFilePath& arg_common_path_prefix = FirmwareFilePath()) + { + /* + * Configuring the node info retriever + */ + node_info_retriever_ = &node_info_retriever; + + int res = node_info_retriever_->addListener(this); + if (res < 0) + { + return res; + } + + /* + * Initializing the prefix, removing only the last trailing path separator. + */ + common_path_prefix_ = arg_common_path_prefix; + + if (!common_path_prefix_.empty() && + *(common_path_prefix_.end() - 1) == protocol::file::Path::SEPARATOR) + { + common_path_prefix_.resize(uint8_t(common_path_prefix_.size() - 1U)); + } + + /* + * Initializing the client + */ + res = begin_fw_update_client_.init(); + if (res < 0) + { + return res; + } + begin_fw_update_client_.setCallback( + BeginFirmwareUpdateResponseCallback(this, &FirmwareUpdateTrigger::handleBeginFirmwareUpdateResponse)); + + // The timer will be started ad-hoc + return 0; + } + + /** + * Interval at which uavcan.protocol.file.BeginFirmwareUpdate requests are being sent. + * Note that default value should be OK for any use case. + */ + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) + { + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) // Restarting with new interval + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * This method is mostly needed for testing. + * When triggering is not in progress, the class consumes zero CPU time. + */ + bool isTimerRunning() const { return TimerBase::isRunning(); } + + unsigned getNumPendingNodes() const + { + const unsigned ret = pending_nodes_.getSize(); + UAVCAN_ASSERT((ret > 0) ? isTimerRunning() : true); + return ret; + } + +}; + +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp index e658b33730..4cab0c4abf 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,54 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase UAVCAN_ASSERT(iface_index < MaxCanIfaces); } - int init(); + int init() + { + const int res = pub_.init(); + if (res >= 0) + { + pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_)); + pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback); + pub_.setPriority(TransferPriorityHigh); // Fixed priority + } + return res; + } - void setTxTimestamp(UtcTime ts); + 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); + 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 +102,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 +152,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 +208,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/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/include/uavcan/protocol/logger.hpp b/libuavcan/include/uavcan/protocol/logger.hpp index fcacfc4801..e6e3358152 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,16 @@ 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; + } + logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority + return 0; + } /** * Logs one message. Please consider using helper methods instead of this one. @@ -102,7 +116,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 +203,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/include/uavcan/protocol/network_compat_checker.hpp b/libuavcan/include/uavcan/protocol/network_compat_checker.hpp index 5a39b5c6fc..461085eb05 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_.hasPendingCalls()) + { + 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.getResponse().mutually_known_ids); + + UAVCAN_TRACE("NodeInitializer", "CATS response from nid=%i; local=%llu remote=%llu", + int(resp.getCallID().server_node_id.get()), static_cast(sign.get()), + static_cast(resp.getResponse().aggregate_signature)); + + if (sign.get() != resp.getResponse().aggregate_signature) + { + result_.conflicting_node = resp.getCallID().server_node_id; + } + } + } + + int checkOneNodeOneDataTypeKind(NodeID nid, DataTypeKind kind) + { + StaticAssert::check(); + StaticAssert::check(); + + UAVCAN_ASSERT(nid.isUnicast()); + UAVCAN_ASSERT(!cats_cln_.hasPendingCalls()); + + 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_.cancelAllCalls(); + 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/include/uavcan/protocol/node_info_retriever.hpp b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp new file mode 100644 index 0000000000..017102fa40 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/node_info_retriever.hpp @@ -0,0 +1,450 @@ +/* + * 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 +#include + +namespace uavcan +{ +/** + * Classes that need to receive GetNodeInfo responses should implement this interface. + */ +class UAVCAN_EXPORT INodeInfoListener +{ +public: + /** + * Called when a response to GetNodeInfo request is received. This happens shortly after the node restarts or + * becomes online for the first time. + * @param node_id Node ID of the node + * @param response Node info struct + */ + virtual void handleNodeInfoRetrieved(NodeID node_id, const protocol::GetNodeInfo::Response& node_info) = 0; + + /** + * Called when the retriever decides that the node does not support the GetNodeInfo service. + * This method will never be called if the number of attempts is unlimited. + */ + virtual void handleNodeInfoUnavailable(NodeID node_id) = 0; + + /** + * This call is routed directly from @ref NodeStatusMonitor. + * Default implementation does nothing. + * @param event Node status change event + */ + virtual void handleNodeStatusChange(const NodeStatusMonitor::NodeStatusChangeEvent& event) + { + (void)event; + } + + /** + * This call is routed directly from @ref NodeStatusMonitor. + * Default implementation does nothing. + * @param msg Node status message + */ + virtual void handleNodeStatusMessage(const ReceivedDataStructure& msg) + { + (void)msg; + } + + virtual ~INodeInfoListener() { } +}; + +/** + * This class automatically retrieves a response to GetNodeInfo once a node appears online or restarts. + * It does a number of attempts in case if there's a communication failure before assuming that the node does not + * implement the GetNodeInfo service. All parameters are pre-configured with sensible default values that should fit + * virtually any use case, but they can be overriden if needed - refer to the setter methods below for details. + * + * Defaults are pre-configured so that the class is able to query 123 nodes (node ID 1..125, where 1 is our local + * node and 1 is one node that implements GetNodeInfo service, hence 123) of which none implements GetNodeInfo + * service in under 5 seconds. The 5 second limitation is imposed by UAVCAN-compatible bootloaders, which are + * unlikely to wait for more than that before continuing to boot. In case if this default value is not appropriate + * for the end application, the request interval can be overriden via @ref setRequestInterval(). + * + * Following the above explained requirements, the default request interval is defined as follows: + * request interval [ms] = floor(5000 [ms] bootloader timeout / 123 nodes) + * Which yields 40 ms. + * + * Given default service timeout 500 ms and the defined above request frequency 40 ms, the maximum number of + * concurrent requests will be: + * max concurrent requests = ceil(500 [ms] timeout / 40 [ms] request interval) + * Which yields 13 requests. + * + * Keep the above equations in mind when changing the default request interval. + * + * Obviously, if all calls are completing in under (request interval), the number of concurrent requests will never + * exceed one. This is actually the most likely scenario. + * + * Note that all nodes are queried in a round-robin fashion, regardless of their uptime, number of requests made, etc. + * + * Events from this class can be routed to many listeners, @ref INodeInfoListener. + */ +class UAVCAN_EXPORT NodeInfoRetriever : NodeStatusMonitor + , TimerBase +{ +public: + enum { MaxNumRequestAttempts = 254 }; + enum { UnlimitedRequestAttempts = 0 }; + +private: + typedef MethodBinder&)> + 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 + } + }; + + struct NodeInfoRetrievedHandlerCaller + { + const NodeID node_id; + const protocol::GetNodeInfo::Response& node_info; + + NodeInfoRetrievedHandlerCaller(NodeID arg_node_id, const protocol::GetNodeInfo::Response& arg_node_info) + : node_id(arg_node_id) + , node_info(arg_node_info) + { } + + bool operator()(INodeInfoListener* key) + { + UAVCAN_ASSERT(key != NULL); + key->handleNodeInfoRetrieved(node_id, node_info); + return false; + } + }; + + template + struct GenericHandlerCaller + { + void (INodeInfoListener::* const method)(Event); + Event event; + + GenericHandlerCaller(void (INodeInfoListener::*arg_method)(Event), Event arg_event) + : method(arg_method) + , event(arg_event) + { } + + bool operator()(INodeInfoListener* key) + { + UAVCAN_ASSERT(key != NULL); + (key->*method)(event); + return false; + } + }; + + enum { NumStaticCalls = 2 }; + enum { DefaultNumRequestAttempts = 16 }; + enum { DefaultTimerIntervalMSec = 40 }; ///< Read explanation in the class documentation + + /* + * State + */ + Entry entries_[NodeID::Max]; // [1, NodeID::Max] + + Multiset listeners_; + + ServiceClient get_node_info_client_; + + MonotonicDuration request_interval_; + + mutable uint8_t last_picked_node_; + + uint8_t num_attempts_; + + /* + * Methods + */ + const Entry& getEntry(NodeID node_id) const { return const_cast(this)->getEntry(node_id); } + Entry& getEntry(NodeID node_id) + { + if (node_id.get() < 1 || node_id.get() > NodeID::Max) + { + handleFatalError("NodeInfoRetriever NodeID"); + } + return entries_[node_id.get() - 1]; + } + + void startTimerIfNotRunning() + { + if (!TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + UAVCAN_TRACE("NodeInfoRetriever", "Timer started, interval %s sec", request_interval_.toString().c_str()); + } + } + + NodeID pickNextNodeToQuery(bool& out_at_least_one_request_needed) const + { + out_at_least_one_request_needed = false; + + for (unsigned iter_cnt_ = 0; iter_cnt_ < (sizeof(entries_) / sizeof(entries_[0])); iter_cnt_++) // Round-robin + { + last_picked_node_++; + if (last_picked_node_ > NodeID::Max) + { + last_picked_node_ = 1; + } + UAVCAN_ASSERT((last_picked_node_ >= 1) && + (last_picked_node_ <= NodeID::Max)); + + const Entry& entry = getEntry(last_picked_node_); + + if (entry.request_needed) + { + out_at_least_one_request_needed = true; + + if (entry.updated_since_last_attempt && + !get_node_info_client_.hasPendingCallToServer(last_picked_node_)) + { + UAVCAN_TRACE("NodeInfoRetriever", "Next node to query: %d", int(last_picked_node_)); + return NodeID(last_picked_node_); + } + } + } + + return NodeID(); // No node could be found + } + + virtual void handleTimerEvent(const TimerEvent&) + { + bool at_least_one_request_needed = false; + const NodeID next = pickNextNodeToQuery(at_least_one_request_needed); + + if (next.isUnicast()) + { + UAVCAN_ASSERT(at_least_one_request_needed); + getEntry(next).updated_since_last_attempt = false; + const int res = get_node_info_client_.call(next, protocol::GetNodeInfo::Request()); + if (res < 0) + { + get_node_info_client_.getNode().registerInternalFailure("NodeInfoRetriever GetNodeInfo call"); + } + } + else + { + if (!at_least_one_request_needed) + { + TimerBase::stop(); + UAVCAN_TRACE("NodeInfoRetriever", "Timer stopped"); + } + } + } + + virtual void handleNodeStatusChange(const NodeStatusChangeEvent& event) + { + const bool was_offline = !event.old_status.known || + (event.old_status.status_code == protocol::NodeStatus::STATUS_OFFLINE); + + const bool offline_now = !event.status.known || + (event.status.status_code == protocol::NodeStatus::STATUS_OFFLINE); + + if (was_offline || offline_now) + { + Entry& entry = getEntry(event.node_id); + + entry.request_needed = !offline_now; + entry.num_attempts_made = 0; + + UAVCAN_TRACE("NodeInfoRetriever", "Offline status change: node ID %d, request needed: %d", + int(event.node_id.get()), int(entry.request_needed)); + + if (entry.request_needed) + { + startTimerIfNotRunning(); + } + } + + listeners_.forEach( + GenericHandlerCaller(&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; + + startTimerIfNotRunning(); + } + entry.uptime_sec = msg.uptime_sec; + entry.updated_since_last_attempt = true; + + listeners_.forEach(GenericHandlerCaller&>( + &INodeInfoListener::handleNodeStatusMessage, msg)); + } + + void handleGetNodeInfoResponse(const ServiceCallResult& result) + { + Entry& entry = getEntry(result.getCallID().server_node_id); + + if (result.isSuccessful()) + { + /* + * Updating the uptime here allows to properly handle a corner case where the service response arrives + * after the device has restarted and published its new NodeStatus (although it's unlikely to happen). + */ + entry.uptime_sec = result.getResponse().status.uptime_sec; + entry.request_needed = false; + listeners_.forEach(NodeInfoRetrievedHandlerCaller(result.getCallID().server_node_id, + result.getResponse())); + } + else + { + if (num_attempts_ != UnlimitedRequestAttempts) + { + entry.num_attempts_made++; + if (entry.num_attempts_made >= num_attempts_) + { + entry.request_needed = false; + listeners_.forEach(GenericHandlerCaller(&INodeInfoListener::handleNodeInfoUnavailable, + result.getCallID().server_node_id)); + } + } + } + } + +public: + NodeInfoRetriever(INode& node) + : NodeStatusMonitor(node) + , TimerBase(node) + , listeners_(node.getAllocator()) + , get_node_info_client_(node) + , request_interval_(MonotonicDuration::fromMSec(DefaultTimerIntervalMSec)) + , last_picked_node_(1) + , num_attempts_(DefaultNumRequestAttempts) + { } + + /** + * Starts the retriever. + * Destroy the object to stop it. + * Returns negative error code. + */ + int start() + { + int res = NodeStatusMonitor::start(); + if (res < 0) + { + return res; + } + + res = get_node_info_client_.init(); + if (res < 0) + { + return res; + } + get_node_info_client_.setCallback(GetNodeInfoResponseCallback(this, + &NodeInfoRetriever::handleGetNodeInfoResponse)); + // Note: the timer will be started ad-hoc + return 0; + } + + /** + * Adds one listener. Does nothing if such listener already exists. + * May return -ErrMemory if there's no space to add the listener. + */ + int addListener(INodeInfoListener* listener) + { + if (listener != NULL) + { + removeListener(listener); + return (NULL == listeners_.emplace(listener)) ? -ErrMemory : 0; + } + else + { + return -ErrInvalidParam; + } + } + + /** + * Removes the listener. + * If the listener was not registered, nothing will be done. + */ + void removeListener(INodeInfoListener* listener) + { + if (listener != NULL) + { + listeners_.removeAll(listener); + } + else + { + UAVCAN_ASSERT(0); + } + } + + unsigned getNumListeners() const { return listeners_.getSize(); } + + /** + * Number of attempts to retrieve GetNodeInfo response before giving up on the assumption that the service is + * not implemented. + * Zero is a special value that can be used to set unlimited number of attempts, @ref UnlimitedRequestAttempts. + */ + uint8_t getNumRequestAttempts() const { return num_attempts_; } + void setNumRequestAttempts(const uint8_t num) + { + num_attempts_ = min(static_cast(MaxNumRequestAttempts), num); + } + + /** + * Request interval also implicitly defines the maximum number of concurrent requests. + * Read the class documentation for details. + */ + MonotonicDuration getRequestInterval() const { return request_interval_; } + void setRequestInterval(const MonotonicDuration interval) + { + if (interval.isPositive()) + { + request_interval_ = interval; + if (TimerBase::isRunning()) + { + TimerBase::startPeriodic(request_interval_); + } + } + else + { + UAVCAN_ASSERT(0); + } + } + + /** + * These methods are needed mostly for testing. + */ + bool isRetrievingInProgress() const { return TimerBase::isRunning(); } + + uint8_t getNumPendingRequests() const + { + const unsigned num = get_node_info_client_.getNumPendingCalls(); + UAVCAN_ASSERT(num <= 0xFF); + return static_cast(num); + } +}; + +} + +#endif // Include guard diff --git a/libuavcan/include/uavcan/protocol/node_status_monitor.hpp b/libuavcan/include/uavcan/protocol/node_status_monitor.hpp index e9028e37b9..06f7bb7605 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 { @@ -16,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; @@ -40,12 +43,14 @@ public: }; private: - static const unsigned 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. */ @@ -53,6 +58,8 @@ private: Subscriber sub_; + TimerEventForwarder timer_; + struct Entry { NodeStatusCode status_code; @@ -65,13 +72,72 @@ 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); + } + + 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: /** @@ -96,8 +162,8 @@ protected: public: explicit NodeStatusMonitor(INode& node) - : TimerBase(node) - , sub_(node) + : sub_(node) + , timer_(node) { } virtual ~NodeStatusMonitor() { } @@ -107,25 +173,80 @@ 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) + { + timer_.setCallback(TimerCallback(this, &NodeStatusMonitor::handleTimerEvent)); + timer_.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/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/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/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/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/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/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index a671f4ac85..d179b32193 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 until all available frames are processed. + */ + int spinOnce(); + /** * Refer to CanIOManager::send() for the parameter description */ 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.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/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index dc0bc66ad2..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) { } @@ -178,88 +178,69 @@ public: virtual ~TransferListener() { - // Map must be cleared before bufmgr is destructed - receivers_.removeAll(); + // Map must be cleared before bufmgr is destroyed + receivers_.clear(); } }; +/** + * This class is used by transfer listener to decide if the frame should be accepted or ignored. + */ +class ITransferAcceptanceFilter +{ +public: + /** + * If it returns false, the frame will be ignored, otherwise accepted. + */ + virtual bool shouldAcceptFrame(const RxFrame& frame) const = 0; + + virtual ~ITransferAcceptanceFilter() { } +}; + /** * This class should be derived by callers. */ template -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) + { + filter_ = acceptance_filter; + } }; // ---------------------------------------------------------------------------- /* - * 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/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 26d6e0e628..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_; - const DataTypeDescriptor& data_type_; + + Dispatcher& dispatcher_; + TransferPriority priority_; - const CanTxQueue::Qos qos_; - const TransferCRC crc_base_; + 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,16 +44,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) + , dispatcher_(dispatcher) , priority_(TransferPriorityNormal) - , qos_(qos) - , crc_base_(data_type.getSignature().toTransferCRC()) + , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) + { + 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) { } + 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/include/uavcan/util/lazy_constructor.hpp b/libuavcan/include/uavcan/util/lazy_constructor.hpp index 93152adc71..c7ae975663 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_; @@ -51,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) @@ -123,50 +124,53 @@ public: // for nargs in range(1, MAX_ARGS + 1): // nums = [(x + 1) for x in range(nargs)] // l1 = ['typename P%d' % x for x in nums] -// l2 = ['typename ParamType::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/map.hpp b/libuavcan/include/uavcan/util/map.hpp index 814dd517a0..ece759af0e 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 { @@ -107,7 +108,7 @@ private: const unsigned num_static_entries_; #endif - KVPair* find(const Key& key); + KVPair* findKey(const Key& key); #if !UAVCAN_TINY void optimizeStorage(); @@ -116,7 +117,7 @@ private: struct YesPredicate { - bool operator()(const Key& k, const Value& v) const { (void)k; (void)v; return true; } + bool operator()(const Key&, const Value&) const { return true; } }; protected: @@ -136,8 +137,11 @@ protected: } #endif - /// Derived class destructor must call removeAll(); - ~MapBase() { } + /// Derived class destructor must call clear(); + ~MapBase() + { + UAVCAN_ASSERT(getSize() == 0); + } public: /** @@ -158,10 +162,10 @@ public: /** * Removes entries where the predicate returns true. * Predicate prototype: - * bool (const Key& key, const Value& value) + * bool (Key& key, Value& value) */ template - void removeWhere(Predicate predicate); + void removeAllWhere(Predicate predicate); /** * Returns first entry where the predicate returns true. @@ -169,9 +173,12 @@ public: * bool (const Key& key, const Value& value) */ template - const Key* findFirstKey(Predicate predicate) const; + const Key* find(Predicate predicate) const; - void removeAll(); + /** + * Removes all items. + */ + void clear(); /** * Returns a key-value pair located at the specified position from the beginning. @@ -181,7 +188,10 @@ public: KVPair* getByIndex(unsigned index); const KVPair* getByIndex(unsigned index) const; - bool isEmpty() const; + /** + * Complexity is O(1). + */ + bool isEmpty() const { return find(YesPredicate()) == NULL; } unsigned getSize() const; @@ -207,7 +217,7 @@ public: : MapBase(static_, NumStaticEntries, allocator) { } - ~Map() { this->removeAll(); } + ~Map() { this->clear(); } #endif // !UAVCAN_TINY }; @@ -225,7 +235,7 @@ public: #endif { } - ~Map() { this->removeAll(); } + ~Map() { this->clear(); } }; // ---------------------------------------------------------------------------- @@ -234,7 +244,7 @@ public: * MapBase<> */ template -typename 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++) @@ -344,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; } @@ -354,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); @@ -375,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(); @@ -388,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; @@ -407,8 +417,10 @@ void MapBase::removeWhere(Predicate predicate) #endif KVGroup* p = list_.get(); - while (p) + while (p != NULL) { + KVGroup* const next_group = p->getNextListNode(); + for (int i = 0; i < KVGroup::NumKV; i++) { const KVPair* const kv = p->kvs + i; @@ -421,7 +433,8 @@ void MapBase::removeWhere(Predicate predicate) } } } - p = p->getNextListNode(); + + p = next_group; } if (num_removed > 0) @@ -435,7 +448,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++) @@ -451,8 +464,10 @@ const Key* MapBase::findFirstKey(Predicate predicate) const #endif KVGroup* p = list_.get(); - while (p) + while (p != NULL) { + KVGroup* const next_group = p->getNextListNode(); + for (int i = 0; i < KVGroup::NumKV; i++) { const KVPair* const kv = p->kvs + i; @@ -464,15 +479,16 @@ const Key* MapBase::findFirstKey(Predicate predicate) const } } } - p = p->getNextListNode(); + + p = next_group; } return NULL; } template -void MapBase::removeAll() +void MapBase::clear() { - removeWhere(YesPredicate()); + removeAllWhere(YesPredicate()); } template @@ -495,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; @@ -509,7 +527,8 @@ typename MapBase::KVPair* MapBase::getByIndex(unsigned i index--; } } - p = p->getNextListNode(); + + p = next_group; } return NULL; @@ -521,12 +540,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/include/uavcan/util/multiset.hpp b/libuavcan/include/uavcan/util/multiset.hpp new file mode 100644 index 0000000000..c1a99d31ea --- /dev/null +++ b/libuavcan/include/uavcan/util/multiset.hpp @@ -0,0 +1,542 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#ifndef UAVCAN_UTIL_MULTISET_HPP_INCLUDED +#define UAVCAN_UTIL_MULTISET_HPP_INCLUDED + +#include +#include +#include +#include +#include +#include +#include + +#if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) +# error UAVCAN_CPP_VERSION +#endif + +namespace uavcan +{ +/** + * Slow but memory efficient unordered multiset. Unlike Map<>, this container does not move objects, so + * they don't have to be copyable. + * + * Items can be allocated in a static buffer or in the node's memory pool if the static buffer is exhausted. + * + * Number of static entries must not be less than 1. + */ +template +class UAVCAN_EXPORT Multiset : Noncopyable +{ + struct Item : ::uavcan::Noncopyable + { + T* ptr; + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + alignas(T) unsigned char pool[sizeof(T)]; ///< Memory efficient version +#else + union + { + unsigned char pool[sizeof(T)]; + /* + * Such alignment does not guarantee safety for all types (only for libuavcan internal ones); + * however, increasing it is too memory inefficient. So it is recommended to use C++11, where + * this issue is resolved with alignas() (see above). + */ + void* _aligner1_; + long long _aligner2_; + }; +#endif + + Item() + : ptr(NULL) + { + fill_n(pool, sizeof(pool), static_cast(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)); + } + } + }; + +private: + struct Chunk : LinkedListNode, ::uavcan::Noncopyable + { + enum { NumItems = (MemPoolBlockSize - sizeof(LinkedListNode)) / sizeof(Item) }; + Item items[NumItems]; + + Chunk() + { + StaticAssert<(static_cast(NumItems) > 0)>::check(); + IsDynamicallyAllocatable::check(); + UAVCAN_ASSERT(!items[0].isConstructed()); + } + + static Chunk* instantiate(IPoolAllocator& allocator) + { + void* const praw = allocator.allocate(sizeof(Chunk)); + if (praw == NULL) + { + return NULL; + } + return new (praw) Chunk(); + } + + static void destroy(Chunk*& obj, IPoolAllocator& allocator) + { + if (obj != NULL) + { + obj->~Chunk(); + allocator.deallocate(obj); + obj = NULL; + } + } + + Item* findFreeSlot() + { + for (unsigned i = 0; i < static_cast(NumItems); i++) + { + if (!items[i].isConstructed()) + { + return items + i; + } + } + return NULL; + } + }; + + /* + * Data + */ + LinkedListRoot list_; + IPoolAllocator& allocator_; + Item static_[NumStaticEntries]; + + /* + * Methods + */ + Item* findOrCreateFreeSlot(); + + void compact(); + + enum RemoveStrategy { RemoveOne, RemoveAll }; + + template + void removeWhere(Predicate predicate, RemoveStrategy strategy); + + struct YesPredicate + { + bool operator()(const T&) const { return true; } + }; + + struct IndexPredicate : ::uavcan::Noncopyable + { + unsigned index; + IndexPredicate(unsigned target_index) + : index(target_index) + { } + + bool operator()(const T&) + { + return index-- == 0; + } + }; + + struct ComparingPredicate + { + const T& reference; + + ComparingPredicate(const T& ref) + : reference(ref) + { } + + bool operator()(const T& sample) + { + return reference == sample; + } + }; + + template + struct OperatorToFalsePredicateAdapter : ::uavcan::Noncopyable + { + Operator oper; + + OperatorToFalsePredicateAdapter(Operator o) + : oper(o) + { } + + bool operator()(T& item) + { + oper(item); + return false; + } + + bool operator()(const T& item) const + { + oper(item); + return false; + } + }; + +public: + Multiset(IPoolAllocator& allocator) + : allocator_(allocator) + { } + + ~Multiset() + { + clear(); + } + + /** + * Creates one item in-place and returns a pointer to it. + * If creation fails due to lack of memory, NULL will be returned. + * Complexity is O(N). + */ + T* emplace() + { + Item* const item = findOrCreateFreeSlot(); + if (item == NULL) + { + return NULL; + } + UAVCAN_ASSERT(item->ptr == NULL); + item->ptr = new (item->pool) T(); + return item->ptr; + } + + template + T* emplace(P1 p1) + { + Item* const item = findOrCreateFreeSlot(); + if (item == NULL) + { + return NULL; + } + UAVCAN_ASSERT(item->ptr == NULL); + item->ptr = new (item->pool) T(p1); + return item->ptr; + } + + template + T* emplace(P1 p1, P2 p2) + { + Item* const item = findOrCreateFreeSlot(); + if (item == NULL) + { + return NULL; + } + UAVCAN_ASSERT(item->ptr == NULL); + item->ptr = new (item->pool) T(p1, p2); + return item->ptr; + } + + template + T* emplace(P1 p1, P2 p2, P3 p3) + { + Item* const item = findOrCreateFreeSlot(); + if (item == NULL) + { + return NULL; + } + UAVCAN_ASSERT(item->ptr == NULL); + item->ptr = new (item->pool) T(p1, p2, p3); + return item->ptr; + } + + /** + * Removes entries where the predicate returns true. + * Predicate prototype: + * bool (T& item) + */ + template + void removeAllWhere(Predicate predicate) { removeWhere(predicate, RemoveAll); } + + template + void removeFirstWhere(Predicate predicate) { removeWhere(predicate, RemoveOne); } + + void removeFirst(const T& ref) { removeFirstWhere(ComparingPredicate(ref)); } + + void removeAll(const T& ref) { removeAllWhere(ComparingPredicate(ref)); } + + void clear() { removeAllWhere(YesPredicate()); } + + /** + * Returns first entry where the predicate returns true. + * Predicate prototype: + * bool (const T& item) + */ + template + T* find(Predicate predicate); + + template + const T* find(Predicate predicate) const + { + 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. + * If index is greater than or equal the number of items, null pointer will be returned. + * Complexity is O(N). + */ + T* getByIndex(unsigned index) + { + IndexPredicate predicate(index); + return find(predicate); + } + + const T* getByIndex(unsigned index) const + { + return const_cast(this)->getByIndex(index); + } + + /** + * Complexity is O(1). + */ + bool isEmpty() const { return find(YesPredicate()) == NULL; } + + /** + * Counts number of items stored. + * Best case complexity is O(N). + */ + unsigned getSize() const { return getNumStaticItems() + getNumDynamicItems(); } + + /** + * For testing, do not use directly. + */ + unsigned getNumStaticItems() const; + unsigned getNumDynamicItems() const; +}; + +// ---------------------------------------------------------------------------- + +/* + * Multiset<> + */ +template +typename Multiset::Item* Multiset::findOrCreateFreeSlot() +{ +#if !UAVCAN_TINY + // Search in static pool + for (unsigned i = 0; i < NumStaticEntries; i++) + { + if (!static_[i].isConstructed()) + { + return &static_[i]; + } + } +#endif + + // Search in dynamic pool + { + Chunk* p = list_.get(); + while (p) + { + Item* const dyn = p->findFreeSlot(); + if (dyn != NULL) + { + return dyn; + } + p = p->getNextListNode(); + } + } + + // Create new dynamic chunk + Chunk* const chunk = Chunk::instantiate(allocator_); + if (chunk == NULL) + { + return NULL; + } + list_.insert(chunk); + return &chunk->items[0]; +} + +template +void Multiset::compact() +{ + Chunk* p = list_.get(); + while (p) + { + Chunk* const next = p->getNextListNode(); + bool remove_this = true; + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i].isConstructed()) + { + remove_this = false; + break; + } + } + if (remove_this) + { + list_.remove(p); + Chunk::destroy(p, allocator_); + } + p = next; + } +} + +template +template +void Multiset::removeWhere(Predicate predicate, const RemoveStrategy strategy) +{ + unsigned num_removed = 0; + +#if !UAVCAN_TINY + for (unsigned i = 0; i < NumStaticEntries; i++) + { + if (static_[i].isConstructed()) + { + if (predicate(*static_[i].ptr)) + { + num_removed++; + static_[i].destroy(); + if (strategy == RemoveOne) + { + break; + } + } + } + } +#endif + + Chunk* p = list_.get(); + while (p != NULL) + { + Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified + + if ((num_removed > 0) && (strategy == RemoveOne)) + { + break; + } + + for (int i = 0; i < Chunk::NumItems; i++) + { + Item& item = p->items[i]; + if (item.isConstructed()) + { + if (predicate(*item.ptr)) + { + num_removed++; + item.destroy(); + if (strategy == RemoveOne) + { + break; + } + } + } + } + + p = next_chunk; + } + + if (num_removed > 0) + { + compact(); + } +} + +template +template +T* Multiset::find(Predicate predicate) +{ +#if !UAVCAN_TINY + for (unsigned i = 0; i < NumStaticEntries; i++) + { + if (static_[i].isConstructed()) + { + if (predicate(*static_[i].ptr)) + { + return static_[i].ptr; + } + } + } +#endif + + Chunk* p = list_.get(); + while (p != NULL) + { + Chunk* const next_chunk = p->getNextListNode(); // For the case if the current entry gets modified + + for (int i = 0; i < Chunk::NumItems; i++) + { + if (p->items[i].isConstructed()) + { + if (predicate(*p->items[i].ptr)) + { + return p->items[i].ptr; + } + } + } + + p = next_chunk; + } + return NULL; +} + +template +unsigned Multiset::getNumStaticItems() const +{ + unsigned num = 0; +#if !UAVCAN_TINY + for (unsigned i = 0; i < NumStaticEntries; i++) + { + num += static_[i].isConstructed() ? 1U : 0U; + } +#endif + return num; +} + +template +unsigned Multiset::getNumDynamicItems() const +{ + unsigned num = 0; + Chunk* p = list_.get(); + while (p) + { + for (int i = 0; i < Chunk::NumItems; i++) + { + num += p->items[i].isConstructed() ? 1U : 0U; + } + p = p->getNextListNode(); + } + return num; +} + +} + +#endif // Include guard 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 */ diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp index 3ebc7bd699..28e9b36acd 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; } @@ -37,31 +38,21 @@ 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.getDataLength(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id, *tid); + return sender_.send(buffer.getRawPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id, *tid); } else { - return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id); + return sender_.send(buffer.getRawPtr(), 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/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/node/uc_service_client.cpp b/libuavcan/src/node/uc_service_client.cpp index a0c6cc00e8..49c8179a3a 100644 --- a/libuavcan/src/node/uc_service_client.cpp +++ b/libuavcan/src/node/uc_service_client.cpp @@ -6,12 +6,33 @@ namespace uavcan { - -int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID server_node_id, - TransferID& out_transfer_id) +/* + * ServiceClientBase::CallState + */ +void ServiceClientBase::CallState::handleDeadline(MonotonicTime) { - pending_ = true; + UAVCAN_TRACE("ServiceClient::CallState", "Timeout from nid=%d, tid=%d, dtname=%s", + int(id_.server_node_id.get()), int(id_.transfer_id.get()), + (owner_.data_type_descriptor_ == NULL) ? "???" : owner_.data_type_descriptor_->getFullName()); + /* + * What we're doing here is relaying execution from this call stack to a different one. + * We need it because call registry cannot release memory from this callback, because this will destroy the + * object method of which we're executing now. + */ + UAVCAN_ASSERT(timed_out_ == false); + timed_out_ = true; + owner_.generateDeadlineImmediately(); + UAVCAN_TRACE("ServiceClient::CallState", "Relaying execution to the owner's handler via timer callback"); +} +/* + * ServiceClientBase + */ +int ServiceClientBase::prepareToCall(INode& node, + const char* dtname, + NodeID server_node_id, + ServiceCallID& out_call_id) +{ /* * Making sure we're not going to get transport error because of invalid input data */ @@ -20,6 +41,7 @@ int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID ser UAVCAN_TRACE("ServiceClient", "Invalid Server Node ID"); return -ErrInvalidParam; } + out_call_id.server_node_id = server_node_id; /* * Determining the Data Type ID @@ -50,13 +72,9 @@ int ServiceClientBase::prepareToCall(INode& node, const char* dtname, NodeID ser UAVCAN_TRACE("ServiceClient", "OTR access failure, dtd=%s", data_type_descriptor_->toString().c_str()); return -ErrMemory; } - out_transfer_id = *otr_tid; + out_call_id.transfer_id = *otr_tid; otr_tid->increment(); - /* - * Registering the deadline handler - */ - DeadlineHandler::startWithDelay(request_timeout_); return 0; } 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; -} - -} 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 78% 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..173314ed81 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(); @@ -162,13 +160,9 @@ int DynamicNodeIDAllocationClient::start(const protocol::HardwareVersion& hardwa 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, &DynamicNodeIDAllocationClient::handleAllocation)); + res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation)); if (res < 0) { return res; 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 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(); -} - -} 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 - -} 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 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; -} - -} 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/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; -} - -} 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; -} - -} 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)); -} - -} 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 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); 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/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); /** 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/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/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()); } 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()); } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 4192bc890c..8306ea1c16 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,21 +23,31 @@ 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) { std::cout << result << std::endl; - last_status = result.status; - last_response = result.response; - last_server_node_id = result.server_node_id; + last_status = result.getStatus(); + last_response = result.getResponse(); + last_server_node_id = result.getCallID().server_node_id; + responses.push(result.getResponse()); } bool match(StatusType status, uavcan::NodeID server_node_id, const typename DataType::Response& response) const { - return - status == last_status && + if (status == last_status && server_node_id == last_server_node_id && - response == last_response; + response == last_response) + { + return true; + } + else + { + std::cout << "MISMATCH: status=" << last_status << ", last_server_node_id=" + << int(last_server_node_id.get()) << ", last response:\n" << last_response << std::endl; + return false; + } } typedef uavcan::MethodBinder& 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 } @@ -83,6 +105,12 @@ TEST(ServiceClient, Basic) ClientType client2(nodes.b); ClientType client3(nodes.b); + ASSERT_EQ(0, client1.getNumPendingCalls()); + ASSERT_EQ(0, client2.getNumPendingCalls()); + ASSERT_EQ(0, client3.getNumPendingCalls()); + + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + client1.setCallback(handler.bind()); client2.setCallback(client1.getCallback()); client3.setCallback(client1.getCallback()); @@ -94,23 +122,47 @@ TEST(ServiceClient, Basic) root_ns_a::StringService::Request request; request.string_request = "Hello world"; + std::cout << "!!! Calling!" << std::endl; + ASSERT_LT(0, client1.call(1, request)); // OK + ASSERT_LT(0, client1.call(1, request)); // OK - second request ASSERT_LT(0, client2.call(1, request)); // OK ASSERT_LT(0, client3.call(99, request)); // Will timeout! + ASSERT_LT(0, client3.call(1, request)); // OK - second request + + ASSERT_TRUE(client1.hasPendingCallToServer(1)); + ASSERT_TRUE(client2.hasPendingCallToServer(1)); + ASSERT_TRUE(client3.hasPendingCallToServer(99)); + ASSERT_TRUE(client3.hasPendingCallToServer(1)); + + std::cout << "!!! Spinning!" << std::endl; ASSERT_EQ(3, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Listening now! - ASSERT_TRUE(client1.isPending()); - ASSERT_TRUE(client2.isPending()); - ASSERT_TRUE(client3.isPending()); + ASSERT_TRUE(client1.hasPendingCalls()); + ASSERT_TRUE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); + + ASSERT_EQ(2, client1.getNumPendingCalls()); + ASSERT_EQ(1, client2.getNumPendingCalls()); + ASSERT_EQ(2, client3.getNumPendingCalls()); + + ASSERT_EQ(uavcan::NodeID(1), client2.getCallIDByIndex(0).server_node_id); + ASSERT_EQ(uavcan::NodeID(), client2.getCallIDByIndex(1).server_node_id); nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(20)); + std::cout << "!!! Spin finished!" << std::endl; + ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third is still listening! - ASSERT_FALSE(client1.isPending()); - ASSERT_FALSE(client2.isPending()); - ASSERT_TRUE(client3.isPending()); + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client2.hasPendingCalls()); + ASSERT_TRUE(client3.hasPendingCalls()); + + ASSERT_EQ(0, client1.getNumPendingCalls()); + ASSERT_EQ(0, client2.getNumPendingCalls()); + ASSERT_EQ(1, client3.getNumPendingCalls()); // The one addressed to 99 is still waiting // Validating root_ns_a::StringService::Response expected_response; @@ -119,9 +171,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 :( @@ -130,7 +182,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()); } @@ -139,6 +191,170 @@ 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(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)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(200)); + ASSERT_FALSE(client1.hasPendingCalls()); + ASSERT_FALSE(client1.hasPendingCallToServer(1)); + + ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Timed out + + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 1, root_ns_a::StringService::Response())); +} + + +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, root_ns_a::StringService::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, root_ns_a::StringService::Response())); +} + + TEST(ServiceClient, Empty) { InterlinkedTestNodesWithSysClock nodes; 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/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 6fcdc60080..f101f00991 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -4,22 +4,30 @@ #pragma once +#if __GNUC__ +// We need auto_ptr for compatibility reasons +# pragma GCC diagnostic ignored "-Wdeprecated-declarations" +#endif + #include #include +#include +#include #include "../transport/can/can.hpp" struct TestNode : public uavcan::INode { - uavcan::PoolAllocator pool; + uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; - 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,33 +36,33 @@ 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; } virtual uavcan::Scheduler& getScheduler() { return scheduler; } virtual const uavcan::Scheduler& getScheduler() const { return scheduler; } - virtual uavcan::IMarshalBufferProvider& getMarshalBufferProvider() { return buffer_provider; } }; 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) @@ -70,7 +78,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; @@ -88,8 +95,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); @@ -100,7 +110,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()) { @@ -119,6 +128,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; } @@ -178,3 +192,71 @@ 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(uint8_t(first_node_id + i))); + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + for (uavcan::uint8_t k = 0; k < NumNodes; k++) + { + nodes[i]->can_driver.linkTogether(&nodes[k]->can_driver); + } + } + + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + assert(nodes[i]->can_driver.others.size() == (NumNodes - 1)); + } + } + + int spinAll(uavcan::MonotonicDuration duration) + { + assert(!duration.isNegative()); + unsigned nspins = unsigned(duration.toMSec() / NumNodes); + nspins = nspins ? nspins : 1; + while (nspins --> 0) + { + for (uavcan::uint8_t i = 0; i < NumNodes; i++) + { + int ret = nodes[i]->node.spin(uavcan::MonotonicDuration::fromMSec(1)); + if (ret < 0) + { + return ret; + } + } + } + return 0; + } + + TestNode& operator[](unsigned index) + { + if (index >= NumNodes) + { + throw std::out_of_range("No such test node"); + } + return nodes[index]->node; + } +}; diff --git a/libuavcan/test/node/test_node_test.cpp b/libuavcan/test/node/test_node_test.cpp new file mode 100644 index 0000000000..b40357be21 --- /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 (uint8_t i = 0; i < 8; i++) + { + frame.data[i] = i; + } + frame.id = 1234U; + + ASSERT_EQ(1, nwk.nodes[0]->can_driver.send(frame, uavcan::MonotonicTime(), uavcan::CanIOFlags())); + + for (int i = 1; i < 4; i++) + { + uavcan::CanFrame rx; + uavcan::MonotonicTime ts_mono; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + ASSERT_EQ(1, nwk.nodes[i]->can_driver.receive(rx, ts_mono, ts_utc, flags)); + + ASSERT_TRUE(rx == frame); + } +} diff --git a/libuavcan/test/protocol/data_type_info_provider.cpp b/libuavcan/test/protocol/data_type_info_provider.cpp index f987b3080b..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()) { @@ -31,29 +32,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 +91,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 +106,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 +149,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 +167,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 +185,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 +204,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 +222,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/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; 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..dfecf86b6a --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/allocation_request_manager.cpp @@ -0,0 +1,117 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "event_tracer.hpp" +#include "../helpers.hpp" + + +using uavcan::dynamic_node_id_server::UniqueID; + +class AllocationRequestHandler : public uavcan::dynamic_node_id_server::IAllocationRequestHandler +{ + 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()) + { + 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 + */ + EventTracer tracer; + AllocationRequestHandler handler; + handler.can_followup = true; + + AllocationRequestManager manager(nodes.a, tracer, handler); + + ASSERT_LE(0, manager.init()); + + /* + * 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/centralized/server.cpp b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp new file mode 100644 index 0000000000..5e83b19d05 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/server.cpp @@ -0,0 +1,78 @@ +/* + * 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; + std::cout << "NodeDiscoverer: " << sizeof(NodeDiscoverer) << std::endl; +} 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..51ada6d104 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/centralized/storage.cpp @@ -0,0 +1,174 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "../../helpers.hpp" +#include "../memory_storage_backend.hpp" + + +TEST(dynamic_node_id_server_centralized_Storage, Initialization) +{ + using namespace uavcan::dynamic_node_id_server::centralized; + + // No data in the storage - initializing empty + { + MemoryStorageBackend storage; + Storage stor(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, stor.init()); + + ASSERT_EQ(1, storage.getNumKeys()); + ASSERT_EQ(0, stor.getSize()); + + ASSERT_FALSE(stor.findByNodeID(1)); + ASSERT_FALSE(stor.findByNodeID(0)); + } + // Nonempty storage, one item + { + MemoryStorageBackend storage; + Storage stor(storage); + + storage.set("size", "1"); + ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Expected one entry, none found + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("0_unique_id", "00000000000000000000000000000000"); + storage.set("0_node_id", "0"); + ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Invalid entry - zero Node ID + + storage.set("0_node_id", "1"); + ASSERT_LE(0, stor.init()); // OK now + + ASSERT_EQ(3, storage.getNumKeys()); + ASSERT_EQ(1, stor.getSize()); + + ASSERT_TRUE(stor.findByNodeID(1)); + ASSERT_FALSE(stor.findByNodeID(0)); + } + // Nonempty storage, broken data + { + MemoryStorageBackend storage; + Storage stor(storage); + + storage.set("size", "foobar"); + ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Bad value + + storage.set("size", "128"); + ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Bad value + + storage.set("size", "1"); + ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // No items + ASSERT_EQ(1, storage.getNumKeys()); + + storage.set("0_unique_id", "00000000000000000000000000000000"); + storage.set("0_node_id", "128"); // Bad value (127 max) + ASSERT_EQ(-uavcan::ErrFailure, stor.init()); // Failed + + storage.set("0_node_id", "127"); + ASSERT_LE(0, stor.init()); // OK now + ASSERT_EQ(1, stor.getSize()); + + ASSERT_TRUE(stor.findByNodeID(127)); + ASSERT_FALSE(stor.findByNodeID(0)); + + ASSERT_EQ(3, storage.getNumKeys()); + } + // Nonempty storage, many items + { + MemoryStorageBackend storage; + Storage stor(storage); + + storage.set("size", "2"); + storage.set("0_unique_id", "00000000000000000000000000000000"); + storage.set("0_node_id", "1"); + storage.set("1_unique_id", "0123456789abcdef0123456789abcdef"); + storage.set("1_node_id", "127"); + + ASSERT_LE(0, stor.init()); // OK now + ASSERT_EQ(5, storage.getNumKeys()); + ASSERT_EQ(2, stor.getSize()); + + ASSERT_TRUE(stor.findByNodeID(1)); + ASSERT_TRUE(stor.findByNodeID(127)); + ASSERT_FALSE(stor.findByNodeID(0)); + + uavcan::protocol::dynamic_node_id::server::Entry::FieldTypes::unique_id uid; + uid[0] = 0x01; + uid[1] = 0x23; + uid[2] = 0x45; + uid[3] = 0x67; + uid[4] = 0x89; + uid[5] = 0xab; + uid[6] = 0xcd; + uid[7] = 0xef; + uavcan::copy(uid.begin(), uid.begin() + 8, uid.begin() + 8); + + ASSERT_TRUE(stor.findByUniqueID(uid)); + ASSERT_EQ(127, stor.findByUniqueID(uid)->node_id.get()); + ASSERT_EQ(uid, stor.findByNodeID(127)->unique_id); + } +} + + +TEST(dynamic_node_id_server_centralized_Storage, Basic) +{ + using namespace uavcan::dynamic_node_id_server::centralized; + + MemoryStorageBackend storage; + Storage stor(storage); + + ASSERT_EQ(0, storage.getNumKeys()); + ASSERT_LE(0, stor.init()); + storage.print(); + ASSERT_EQ(1, storage.getNumKeys()); + + /* + * Adding one entry to the log, making sure it appears in the storage + */ + Storage::Entry entry; + entry.node_id = 1; + entry.unique_id[0] = 1; + ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id)); + + ASSERT_EQ("1", storage.get("size")); + ASSERT_EQ("01000000000000000000000000000000", storage.get("0_unique_id")); + ASSERT_EQ("1", storage.get("0_node_id")); + + ASSERT_EQ(3, storage.getNumKeys()); + ASSERT_EQ(1, stor.getSize()); + + /* + * Adding another entry while storage is failing + */ + storage.failOnSetCalls(true); + + ASSERT_EQ(3, storage.getNumKeys()); + + entry.node_id = 2; + entry.unique_id[0] = 2; + ASSERT_GT(0, stor.add(entry.node_id, entry.unique_id)); + + ASSERT_EQ(3, storage.getNumKeys()); // No new entries, we failed + + ASSERT_EQ(1, stor.getSize()); + + /* + * Making sure add() fails when the log is full + */ + storage.failOnSetCalls(false); + + while (stor.getSize() < stor.Capacity) + { + ASSERT_LE(0, stor.add(entry.node_id, entry.unique_id)); + + entry.node_id = uint8_t(uavcan::min(entry.node_id.get() + 1U, 127U)); + entry.unique_id[0] = uint8_t(entry.unique_id[0] + 1U); + } + + ASSERT_GT(0, stor.add(123, entry.unique_id)); // Failing because full + + storage.print(); +} 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..679386087a --- /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/log.cpp b/libuavcan/test/protocol/dynamic_node_id_server/distributed/log.cpp new file mode 100644 index 0000000000..e5d7339410 --- /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..a47fa81585 --- /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..d6992c02c8 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/distributed/server.cpp @@ -0,0 +1,195 @@ +/* + * 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::IRaftLeaderMonitor +{ + const std::string id_; + + virtual void handleLogCommitOnLeader(const uavcan::protocol::dynamic_node_id::server::Entry& entry) + { + std::cout << "ENTRY COMMITTED [" << id_ << "]\n" << entry << std::endl; + } + + virtual void handleLocalLeadershipChange(bool local_node_is_leader) + { + std::cout << "I AM LEADER [" << id_ << "]: " << (local_node_is_leader ? "YES" : "NOT ANYMORE") << std::endl; + } + +public: + CommitHandler(const std::string& id) : id_(id) { } +}; + + +TEST(dynamic_node_id_server_RaftCore, Basic) +{ + 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(9000)); + + // 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()); + + /* + * Adding some stuff + */ + Entry::FieldTypes::unique_id unique_id; + uavcan::fill_n(unique_id.begin(), 16, uint8_t(0xAA)); + + (raft_a->isLeader() ? raft_a : raft_b)->appendLog(unique_id, uavcan::NodeID(1)); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); + + /* + * Terminating the leader + */ + raft_a.reset(); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(6000)); + + /* + * 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(9000)); + + ASSERT_FALSE(raft_a->isLeader()); + ASSERT_TRUE(raft_b->isLeader()); + + ASSERT_EQ(1, raft_a->getCommitIndex()); + ASSERT_EQ(1, raft_b->getCommitIndex()); +} + + +TEST(dynamic_node_id_server_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; + uavcan::DefaultDataTypeRegistrator _reg4; + uavcan::DefaultDataTypeRegistrator _reg5; + uavcan::DefaultDataTypeRegistrator _reg6; + + 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 + */ + DistributedServer server(nodes.a, storage, tracer); + + ASSERT_LE(0, server.init(own_unique_id, 1)); + + ASSERT_EQ(0, server.getNumAllocations()); + + /* + * 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, 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; + 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; + + 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/protocol/dynamic_node_id_server/event.cpp b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp new file mode 100644 index 0000000000..7405f5d850 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/event.cpp @@ -0,0 +1,45 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "event_tracer.hpp" + + +TEST(dynamic_node_id_server_EventTracer, EventCodeToString) +{ + using namespace uavcan::dynamic_node_id_server; + + // Simply checking some error codes + ASSERT_STREQ("Error", IEventTracer::getEventName(TraceError)); + 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 new file mode 100644 index 0000000000..dc7ba09077 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/event_tracer.hpp @@ -0,0 +1,67 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + + +class EventTracer : public uavcan::dynamic_node_id_server::IEventTracer +{ + struct EventLogEntry + { + 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()); } +}; 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/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/node_discoverer.cpp b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp new file mode 100644 index 0000000000..7313bf0b0a --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_server/node_discoverer.cpp @@ -0,0 +1,290 @@ +/* + * 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 + */ + std::cout << "!!! Publishing NodeStatus, discovery is disabled" << std::endl; + 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 + */ + std::cout << "!!! Enabling discovery" << std::endl; + handler.can_discover = true; + + 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_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(650)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + 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(500)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_EQ(4, tracer.countEvents(TraceDiscoveryGetNodeInfoRequest)); + ASSERT_EQ(3, 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()); +} + + +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(1600)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + 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()); + + /* + * 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(1600)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(0, tracer.countEvents(TraceDiscoveryTimerStop)); + 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()); + + /* + * Waiting for timeout + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1600)); + + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryNewNodeFound)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStart)); + ASSERT_EQ(1, tracer.countEvents(TraceDiscoveryTimerStop)); + ASSERT_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()); + + /* + * Checking the results + */ + 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; +} 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)); +} diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp new file mode 100644 index 0000000000..be970b4d04 --- /dev/null +++ b/libuavcan/test/protocol/file_server.cpp @@ -0,0 +1,136 @@ +/* + * 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_size, EntryType& out_type) + { + if (path == file_name) + { + 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 uint64_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(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); + } +} + + +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 +} diff --git a/libuavcan/test/protocol/firmware_update_trigger.cpp b/libuavcan/test/protocol/firmware_update_trigger.cpp new file mode 100644 index 0000000000..08ef9c4fd8 --- /dev/null +++ b/libuavcan/test/protocol/firmware_update_trigger.cpp @@ -0,0 +1,336 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include +#include "helpers.hpp" + +using namespace uavcan::protocol::file; + +struct FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + unsigned should_request_cnt; + unsigned should_retry_cnt; + unsigned confirmation_cnt; + + std::string firmware_path; + + int retry_quota; + std::string expected_node_name_to_update; + + BeginFirmwareUpdate::Response last_error_response; + + FirmwareVersionChecker() + : should_request_cnt(0) + , should_retry_cnt(0) + , confirmation_cnt(0) + , retry_quota(0) + { } + + virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + should_request_cnt++; + std::cout << "REQUEST? " << int(node_id.get()) << "\n" << node_info << std::endl; + out_firmware_file_path = firmware_path.c_str(); + return node_info.name == expected_node_name_to_update; + } + + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& error_response, + FirmwareFilePath& out_firmware_file_path) + { + last_error_response = error_response; + std::cout << "RETRY? " << int(node_id.get()) << "\n" << error_response << std::endl; + should_retry_cnt++; + + EXPECT_STREQ(firmware_path.c_str(), out_firmware_file_path.c_str()); + + if (retry_quota > 0) + { + retry_quota--; + return true; + } + else + { + return false; + } + } + + virtual void handleFirmwareUpdateConfirmation(uavcan::NodeID node_id, + const BeginFirmwareUpdate::Response& response) + { + confirmation_cnt++; + std::cout << "CONFIRMED " << int(node_id.get()) << "\n" << response << std::endl; + } +}; + +struct BeginFirmwareUpdateServer +{ + uint8_t response_error_code; + + BeginFirmwareUpdateServer() : response_error_code(0) { } + + void handleRequest(const uavcan::ReceivedDataStructure& 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) +{ + 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()); + + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Updating one node + * The server that can confirm the request is not running yet + */ + checker.firmware_path = "firmware_path"; + checker.expected_node_name_to_update = "Ivan"; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(0, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2000)); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Starting the firmware update server that returns an error + * The checker will instruct the trigger to repeat + */ + uavcan::ServiceServer srv(nodes.b); + BeginFirmwareUpdateServer srv_impl; + + ASSERT_LE(0, srv.start(srv_impl.makeCallback())); + + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_UNKNOWN; + checker.retry_quota = 1000; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(0, checker.confirmation_cnt); + + // Still running! + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(1, trigger.getNumPendingNodes()); + + /* + * Trying again, this time with ERROR_IN_PROGRESS + */ + srv_impl.response_error_code = BeginFirmwareUpdate::Response::ERROR_IN_PROGRESS; + checker.retry_quota = 0; + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(1, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Restarting the node info provider + * Now it doesn't need an update + */ + provider.reset(new uavcan::NodeStatusProvider(nodes.b)); + + provider->setName("Dmitry"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(2100)); + + ASSERT_EQ(2, checker.should_request_cnt); + ASSERT_EQ(1, checker.should_retry_cnt); + ASSERT_EQ(1, checker.confirmation_cnt); + + // Stopped! + ASSERT_FALSE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); + + /* + * Final checks + */ + ASSERT_EQ(0, nodes.a.internal_failure_count); + ASSERT_EQ(0, nodes.b.internal_failure_count); +} + + +TEST(FirmwareUpdateTrigger, MultiNode) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _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(4200)); // Two will retry, one drop, one confirm + + ASSERT_TRUE(trigger.isTimerRunning()); + ASSERT_EQ(0, trigger.getNumPendingNodes()); // All removed + + EXPECT_EQ(4, checker.should_request_cnt); + EXPECT_EQ(4, checker.should_retry_cnt); + EXPECT_EQ(1, checker.confirmation_cnt); + + /* + * Waiting for the timer to stop + */ + nodes.spinAll(uavcan::MonotonicDuration::fromMSec(1100)); + + ASSERT_FALSE(trigger.isTimerRunning()); +} 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); diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index df8085566e..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); } }; @@ -103,3 +132,28 @@ struct BackgroundSpinner : uavcan::TimerBase spinning_node.spin(uavcan::MonotonicDuration::fromMSec(1)); } }; + +template +static inline void emulateSingleFrameBroadcastTransfer(CanDriver& can, uavcan::NodeID node_id, + const MessageType& message, uavcan::TransferID tid) +{ + uavcan::StaticTransferBuffer<100> buffer; + uavcan::BitStream bitstream(buffer); + uavcan::ScalarCodec codec(bitstream); + + // Manual message publication + ASSERT_LT(0, MessageType::encode(message, codec)); + ASSERT_GE(8, buffer.getMaxWritePos()); + + // DataTypeID data_type_id, TransferType transfer_type, NodeID src_node_id, NodeID dst_node_id, + // uint_fast8_t frame_index, TransferID transfer_id, bool last_frame + uavcan::Frame frame(MessageType::DefaultDataTypeID, uavcan::TransferTypeMessageBroadcast, + node_id, uavcan::NodeID::Broadcast, 0, tid, true); + + ASSERT_EQ(buffer.getMaxWritePos(), frame.setPayload(buffer.getRawPtr(), buffer.getMaxWritePos())); + + uavcan::CanFrame can_frame; + ASSERT_TRUE(frame.compile(can_frame)); + + can.pushRxToAllIfaces(can_frame); +} diff --git a/libuavcan/test/protocol/node_info_retriever.cpp b/libuavcan/test/protocol/node_info_retriever.cpp new file mode 100644 index 0000000000..88fc4aace8 --- /dev/null +++ b/libuavcan/test/protocol/node_info_retriever.cpp @@ -0,0 +1,273 @@ +/* + * 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) + { + std::cout << "NODE " << int(event.node_id.get()) << " STATUS CHANGE: " + << int(event.old_status.status_code) << " --> " << int(event.status.status_code) << std::endl; + status_change_cnt++; + } + + virtual void handleNodeStatusMessage(const uavcan::ReceivedDataStructure& 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); + retr.addListener(&listener); + retr.addListener(&listener); + ASSERT_EQ(1, retr.getNumListeners()); + + uavcan::protocol::HardwareVersion hwver; + hwver.unique_id[0] = 123; + hwver.unique_id[4] = 213; + hwver.unique_id[8] = 45; + + provider->setName("Ivan"); + provider->setHardwareVersion(hwver); + + ASSERT_LE(0, provider->startAndPublish()); + + ASSERT_FALSE(retr.isRetrievingInProgress()); + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(40, retr.getRequestInterval().toMSec()); // Default + + /* + * Waiting for discovery + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(50)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1500)); + ASSERT_FALSE(retr.isRetrievingInProgress()); + + ASSERT_EQ(2, listener.status_message_cnt); + ASSERT_EQ(1, listener.status_change_cnt); + ASSERT_EQ(0, listener.info_unavailable_cnt); + ASSERT_TRUE(listener.last_node_info.get()); + ASSERT_EQ(uavcan::NodeID(2), listener.last_node_id); + ASSERT_EQ("Ivan", listener.last_node_info->name); + ASSERT_TRUE(hwver == listener.last_node_info->hardware_version); + + provider.reset(); // Moving the provider out of the way; its entry will timeout meanwhile + + /* + * Declaring a bunch of different nodes that don't support GetNodeInfo + */ + ASSERT_FALSE(retr.isRetrievingInProgress()); + + retr.setNumRequestAttempts(3); + + uavcan::TransferID tid; + + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 10, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 11, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(10), 0, 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(11), 0, 12, tid); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 10, tid); // Reset + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_LE(2, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(3, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1000)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + EXPECT_EQ(11, listener.status_message_cnt); + EXPECT_EQ(5, listener.status_change_cnt); // node 2 online/offline + 3 test nodes above + EXPECT_EQ(2, listener.info_unavailable_cnt); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 11, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(40)); + ASSERT_EQ(1, retr.getNumPendingRequests()); // Still one because two went offline + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + tid.increment(); + publishNodeStatus(nodes.can_a, uavcan::NodeID(12), 0, 12, tid); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1200)); + ASSERT_FALSE(retr.isRetrievingInProgress()); // Out of attempts, stopping + ASSERT_EQ(0, retr.getNumPendingRequests()); + + EXPECT_EQ(13, listener.status_message_cnt); + EXPECT_EQ(7, listener.status_change_cnt); // node 2 online/offline + 2 test nodes above online/offline + 1 + EXPECT_EQ(3, listener.info_unavailable_cnt); +} + + +TEST(NodeInfoRetriever, MaxConcurrentRequests) +{ + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _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 = 14; // See class docs + const unsigned MinPendingRequestsAtFullLoad = 12; + + /* + * Sending a lot of requests, making sure that the number of concurrent calls does not exceed the specified limit. + */ + for (uint8_t node_id = 1U; node_id <= 127U; node_id++) + { + publishNodeStatus(nodes.can_a, node_id, 0, 0, uavcan::TransferID()); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(10)); + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + for (int i = 0; i < 8; i++) // Approximate + { + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(35)); + std::cout << "!!! SPIN " << i << " COMPLETE" << std::endl; + + ASSERT_GE(MaxPendingRequests, retr.getNumPendingRequests()); + ASSERT_LE(MinPendingRequestsAtFullLoad, retr.getNumPendingRequests()); + + ASSERT_TRUE(retr.isRetrievingInProgress()); + } + + ASSERT_LT(0, retr.getNumPendingRequests()); + ASSERT_TRUE(retr.isRetrievingInProgress()); + + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(5000)); + + ASSERT_EQ(0, retr.getNumPendingRequests()); + ASSERT_FALSE(retr.isRetrievingInProgress()); +} 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); } 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/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); 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); 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 */ 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; diff --git a/libuavcan/test/util/multiset.cpp b/libuavcan/test/util/multiset.cpp new file mode 100644 index 0000000000..8ff43e69b9 --- /dev/null +++ b/libuavcan/test/util/multiset.cpp @@ -0,0 +1,308 @@ +/* + * 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; } +}; + +struct NoncopyableWithCounter : uavcan::Noncopyable +{ + static int num_objects; + long long value; + + NoncopyableWithCounter() : value(0) { num_objects++; } + NoncopyableWithCounter(long long x) : value(x) { num_objects++; } + ~NoncopyableWithCounter() { num_objects--; } + + static bool isNegative(const NoncopyableWithCounter& val) + { + return val.value < 0; + } + + bool operator==(const NoncopyableWithCounter& ref) const { return ref.value == value; } +}; + +int NoncopyableWithCounter::num_objects = 0; + +template +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) +{ + 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)); + + typedef SummationOperator StringConcatenationOperator; + + // Empty + mset->removeFirst("foo"); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_FALSE(mset->getByIndex(0)); + ASSERT_FALSE(mset->getByIndex(1)); + ASSERT_FALSE(mset->getByIndex(10000)); + + // Static addion + ASSERT_EQ("1", *mset->emplace("1")); + ASSERT_EQ("2", *mset->emplace("2")); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(0, mset->getNumDynamicItems()); + + // Ordering + ASSERT_TRUE(*mset->getByIndex(0) == "1"); + ASSERT_TRUE(*mset->getByIndex(1) == "2"); + + { + StringConcatenationOperator op; + mset->forEach(op); + ASSERT_EQ("12", op.accumulator); + } + + // Dynamic addition + ASSERT_EQ("3", *mset->emplace("3")); + ASSERT_EQ("3", *mset->getByIndex(2)); + ASSERT_EQ(1, pool.getNumUsedBlocks()); + + ASSERT_EQ("4", *mset->emplace("4")); + ASSERT_LE(1, pool.getNumUsedBlocks()); // One or more + ASSERT_EQ(2, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + + // Making sure everything is here + ASSERT_EQ("1", *mset->getByIndex(0)); + ASSERT_EQ("2", *mset->getByIndex(1)); + // 2 and 3 are not tested because their placement depends on number of items per dynamic block + ASSERT_FALSE(mset->getByIndex(100)); + ASSERT_FALSE(mset->getByIndex(4)); + + const std::string data_at_pos2 = *mset->getByIndex(2); + const std::string data_at_pos3 = *mset->getByIndex(3); + + // Finding some items + ASSERT_EQ("1", *mset->find(FindPredicate("1"))); + ASSERT_EQ("2", *mset->find(FindPredicate("2"))); + ASSERT_EQ("3", *mset->find(FindPredicate("3"))); + ASSERT_EQ("4", *mset->find(FindPredicate("4"))); + ASSERT_FALSE(mset->find(FindPredicate("nonexistent"))); + + { + StringConcatenationOperator op; + mset->forEach(op); + std::cout << "Accumulator: " << op.accumulator << std::endl; + ASSERT_EQ(4, op.accumulator.size()); + } + + // Removing one static; ordering will be preserved + mset->removeFirst("1"); + mset->removeFirst("foo"); // There's no such thing anyway + ASSERT_LE(1, pool.getNumUsedBlocks()); + ASSERT_EQ(1, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); // This container does not move items + + // Ordering has not changed + ASSERT_EQ("2", *mset->getByIndex(0)); // Entry "1" was here + ASSERT_EQ(data_at_pos2, *mset->getByIndex(1)); + ASSERT_EQ(data_at_pos3, *mset->getByIndex(2)); + + // Removing another static + mset->removeFirst("2"); + ASSERT_EQ(0, mset->getNumStaticItems()); + ASSERT_EQ(2, mset->getNumDynamicItems()); + ASSERT_LE(1, pool.getNumUsedBlocks()); + + // Adding some new items + unsigned max_value_integer = 0; + for (int i = 0; i < 100; i++) + { + const std::string value = toString(i); + std::string* res = mset->emplace(value); // Will NOT override above + if (res == NULL) + { + ASSERT_LT(2, i); + break; + } + else + { + ASSERT_EQ(value, *res); + } + max_value_integer = unsigned(i); + } + std::cout << "Max value: " << max_value_integer << std::endl; + + // Making sure there is true OOM + ASSERT_EQ(0, pool.getNumFreeBlocks()); + ASSERT_FALSE(mset->emplace("nonexistent")); + + // Removing odd values - nearly half of them + mset->removeAllWhere(oddValuePredicate); + + // Making sure there's no odd values left + for (unsigned kv_int = 0; kv_int <= max_value_integer; kv_int++) + { + const std::string* val = mset->find(FindPredicate(toString(kv_int))); + if (val) + { + ASSERT_FALSE(kv_int & 1); + } + else + { + ASSERT_TRUE(kv_int & 1); + } + } + + // Clearing all strings + { + StringConcatenationOperator op; + mset->forEach(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()); +} + + +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->removeFirst(8); + ASSERT_EQ(0, pool.getNumUsedBlocks()); + ASSERT_EQ(0, mset->getSize()); + ASSERT_FALSE(mset->getByIndex(0)); + + // Insertion + ASSERT_EQ(1, *mset->emplace(1)); + ASSERT_EQ(1, mset->getSize()); + ASSERT_EQ(2, *mset->emplace(2)); + ASSERT_EQ(2, mset->getSize()); + ASSERT_EQ(3, *mset->emplace(3)); + ASSERT_EQ(4, *mset->emplace(4)); + ASSERT_EQ(4, mset->getSize()); + +#if UAVCAN_CPP_VERSION >= UAVCAN_CPP11 + // Only C++11 because C++03 uses one entry per pool block which breaks ordering + ASSERT_EQ(1, *mset->getByIndex(0)); + ASSERT_EQ(2, *mset->getByIndex(1)); + ASSERT_EQ(3, *mset->getByIndex(2)); + ASSERT_EQ(4, *mset->getByIndex(3)); + ASSERT_FALSE(mset->getByIndex(5)); + ASSERT_FALSE(mset->getByIndex(1000)); +#endif + + // Summation and clearing + { + SummationOperator 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); + } +} + + +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 +} diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index 51cbcf1c97..d4be18302c 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) @@ -52,9 +54,20 @@ 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}) + +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}) + +add_executable(test_file_server apps/test_file_server.cpp) +target_link_libraries(test_file_server ${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}) @@ -62,7 +75,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/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; + } +} 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/linux/apps/test_posix.cpp b/libuavcan_drivers/linux/apps/test_posix.cpp new file mode 100644 index 0000000000..a59aa1c3ef --- /dev/null +++ b/libuavcan_drivers/linux/apps/test_posix.cpp @@ -0,0 +1,73 @@ +/* + * 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())); + } + + /* + * 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) + { + std::cerr << "Exception: " << ex.what() << std::endl; + return 1; + } +} 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/apps/uavcan_dynamic_node_id_server.cpp b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp new file mode 100644 index 0000000000..388e493881 --- /dev/null +++ b/libuavcan_drivers/linux/apps/uavcan_dynamic_node_id_server.cpp @@ -0,0 +1,676 @@ +/* + * 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 +{ + +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); + + node->setNodeID(nid); + node->setName(name.c_str()); + node->getLogger().setLevel(uavcan::protocol::debug::LogLevel::DEBUG); + + { + const auto app_id = uavcan_linux::makeApplicationID(uavcan_linux::MachineIDReader().read(), name, nid.get()); + + uavcan::protocol::HardwareVersion hwver; + std::copy(app_id.begin(), app_id.end(), hwver.unique_id.begin()); + std::cout << hwver << std::endl; + + node->setHardwareVersion(hwver); + } + + const int start_res = node->start(); + ENFORCE(0 == start_res); + + uavcan::NetworkCompatibilityCheckResult check_result; + ENFORCE(0 == node->checkNetworkCompatibility(check_result)); + if (!check_result.isOk()) + { + throw std::runtime_error("Network conflict with node " + std::to_string(check_result.conflicting_node.get())); + } + + node->setStatusOk(); + + return node; +} + + +class EventTracer : public uavcan_posix::dynamic_node_id_server::FileEventTracer +{ +public: + struct RecentEvent + { + const uavcan::MonotonicDuration time_since_startup; + const uavcan::UtcTime utc_timestamp; + const uavcan::dynamic_node_id_server::TraceCode code; + const std::int64_t argument; + + RecentEvent(uavcan::MonotonicDuration arg_time_since_startup, + uavcan::UtcTime arg_utc_timestamp, + uavcan::dynamic_node_id_server::TraceCode arg_code, + std::int64_t arg_argument) + : time_since_startup(arg_time_since_startup) + , utc_timestamp(arg_utc_timestamp) + , code(arg_code) + , argument(arg_argument) + { } + + uavcan::MakeString<81>::Type toString() const // Heapless return + { + char timebuf[11] = { }; + { + const std::time_t rawtime = utc_timestamp.toUSec() * 1e-6; + const auto tm = localtime(&rawtime); + std::strftime(timebuf, 10, "%H:%M:%S.", tm); + std::snprintf(&timebuf[9], 3, "%02u", static_cast((utc_timestamp.toMSec() % 1000U) / 10U)); + } + + decltype(toString()) out; + out.resize(out.capacity()); + (void)std::snprintf(reinterpret_cast(out.begin()), out.size() - 1U, + "%-11s %-28s %-20lld %016llx", + timebuf, + getEventName(code), + static_cast(argument), + static_cast(argument)); + return out; + } + + static const char* getTableHeader() + { + // Matches the string format above + return "Timestamp Event name Argument (dec) Argument (hex)"; + } + }; + + struct EventStatisticsRecord + { + std::uint64_t count; + uavcan::MonotonicTime last_occurence; + + EventStatisticsRecord() + : count(0) + { } + + void hit(uavcan::MonotonicTime ts) + { + count++; + last_occurence = ts; + } + }; + +private: + struct EnumKeyHash + { + template + 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); + + had_events_ = true; + + const auto ts_m = clock_.getMonotonic(); + const auto ts_utc = clock_.getUtc(); + const auto time_since_startup = ts_m - started_at_; + + last_events_.emplace_front(time_since_startup, ts_utc, code, argument); + if (last_events_.size() > num_last_events_) + { + last_events_.pop_back(); + } + + event_counters_[code].hit(ts_m); + } + +public: + EventTracer(unsigned num_last_events_to_keep) + : num_last_events_(num_last_events_to_keep) + { } + + using uavcan_posix::dynamic_node_id_server::FileEventTracer::init; + + const RecentEvent& getEventByIndex(unsigned index) const { return last_events_.at(index); } + + unsigned getNumEvents() const { return last_events_.size(); } + + const decltype(event_counters_)& getEventCounters() const { return event_counters_; } + + bool hadEvents() + { + if (had_events_) + { + had_events_ = false; + return true; + } + return false; + } +}; + + +::winsize getTerminalSize() +{ + auto w = ::winsize(); + ENFORCE(0 >= ioctl(STDOUT_FILENO, TIOCGWINSZ, &w)); + ENFORCE(w.ws_col > 0 && w.ws_row > 0); + return w; +} + + +std::vector> +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::stable_sort(pairs.begin(), pairs.end(), [](const Pair& a, const Pair& b) { + return a.second.count > b.second.count; + }); + + return pairs; +} + +enum class CLIColor : unsigned +{ + Red = 31, + Green = 32, + Yellow = 33, + Blue = 34, + Magenta = 35, + Cyan = 36, + White = 37, + Default = 39 +}; + +CLIColor getColorHash(unsigned value) { return CLIColor(31 + value % 7); } + +class CLIColorizer +{ + const CLIColor color_; +public: + explicit CLIColorizer(CLIColor c) : color_(c) + { + std::printf("\033[%um", static_cast(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 + */ + constexpr unsigned NumRelevantEvents = 16; + constexpr unsigned NumRowsWithoutEvents = 3; + + /* + * Collecting the data + */ + const unsigned num_rows = getTerminalSize().ws_row; + + const auto relevant_events = collectRelevantEvents(event_tracer, NumRelevantEvents); + + const uavcan::dynamic_node_id_server::distributed::StateReport report(server); + + const auto time_since_last_activity = timestamp - report.last_activity_timestamp; + + /* + * Basic rendering functions + */ + unsigned next_relevant_event_index = 0; + + const auto render_next_event_counter = [&]() + { + const char* event_name = ""; + char event_count_str[10] = { }; + CLIColor event_color = CLIColor::Default; + + if (next_relevant_event_index < relevant_events.size()) + { + const auto e = relevant_events[next_relevant_event_index]; + event_name = uavcan::dynamic_node_id_server::IEventTracer::getEventName(e.first); + std::snprintf(event_count_str, sizeof(event_count_str) - 1U, "%llu", + static_cast(e.second.count)); + event_color = getColorHash(static_cast(e.first)); + } + next_relevant_event_index++; + + std::printf(" | "); + CLIColorizer izer(event_color); + std::printf("%-29s %-9s\n", event_name, event_count_str); + }; + + const auto render_top_str = [&](const char* local_state_name, const char* local_state_value, CLIColor color) + { + { + CLIColorizer izer(color); + std::printf("%-20s %-16s", local_state_name, local_state_value); + } + render_next_event_counter(); + }; + + const auto render_top_int = [&](const char* local_state_name, long long local_state_value, CLIColor color) + { + char buf[21]; + std::snprintf(buf, sizeof(buf) - 1U, "%lld", local_state_value); + render_top_str(local_state_name, buf, color); + }; + + const auto raft_state_to_string = [](uavcan::dynamic_node_id_server::distributed::RaftCore::ServerState s) + { + switch (s) + { + case RaftCore::ServerStateFollower: return "Follower"; + case RaftCore::ServerStateCandidate: return "Candidate"; + case RaftCore::ServerStateLeader: return "Leader"; + default: return "BADSTATE"; + } + }; + + const auto duration_to_string = [](uavcan::MonotonicDuration dur) + { + uavcan::MakeString<16>::Type str; // This is much faster than std::string + str.appendFormatted("%.1f", dur.toUSec() / 1e6); + return str; + }; + + const auto colorize_if = [](bool condition, CLIColor color) + { + return condition ? color : CLIColor::Default; + }; + + /* + * Rendering the data to the CLI + */ + std::printf("\x1b[1J"); // Clear screen from the current cursor position to the beginning + std::printf("\x1b[H"); // Move cursor to the coordinates 1,1 + + // Local state and relevant event counters - two columns + std::printf(" Local state | Event counters\n"); + + render_top_int("Node ID", + node->getNodeID().get(), + CLIColor::Default); + + render_top_str("State", + raft_state_to_string(report.state), + (report.state == RaftCore::ServerStateCandidate) ? CLIColor::Magenta : + (report.state == RaftCore::ServerStateLeader) ? CLIColor::Green : + CLIColor::Default); + + render_top_int("Last log index", + report.last_log_index, + CLIColor::Default); + + render_top_int("Commit index", + report.commit_index, + colorize_if(report.commit_index != report.last_log_index, CLIColor::Magenta)); + + render_top_int("Last log term", + report.last_log_term, + CLIColor::Default); + + render_top_int("Current term", + report.current_term, + CLIColor::Default); + + render_top_int("Voted for", + report.voted_for.get(), + CLIColor::Default); + + render_top_str("Since activity", + duration_to_string(time_since_last_activity).c_str(), + CLIColor::Default); + + render_top_str("Random timeout", + duration_to_string(report.randomized_timeout).c_str(), + CLIColor::Default); + + render_top_int("Unknown nodes", + report.num_unknown_nodes, + colorize_if(report.num_unknown_nodes != 0, CLIColor::Magenta)); + + render_top_int("Node failures", + node->getInternalFailureCount(), + colorize_if(node->getInternalFailureCount() != 0, CLIColor::Magenta)); + + // Empty line before the next block + std::printf(" "); + render_next_event_counter(); + + // Followers block + std::printf(" Followers "); + render_next_event_counter(); + + const auto render_followers_state = [&](const char* name, + const std::function 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) + { + std::printf("%-5d", value); + } + else + { + std::printf("N/A "); + } + } + else + { + std::printf(" "); + } + } + render_next_event_counter(); + }; + + const auto follower_color_getter = [&](std::uint8_t i) + { + if (report.state != RaftCore::ServerStateLeader) { return CLIColor::Default; } + if (!report.followers[i].node_id.isValid()) { return CLIColor::Red; } + if (report.followers[i].match_index != report.last_log_index || + report.followers[i].next_index <= report.last_log_index) + { + return CLIColor::Magenta; + } + return CLIColor::Default; + }; + + render_followers_state("Node ID", [&](std::uint8_t i) + { + const auto nid = report.followers[i].node_id; + return nid.isValid() ? nid.get() : -1; + }, + follower_color_getter); + + render_followers_state("Next index", + [&](std::uint8_t i) { return report.followers[i].next_index; }, + follower_color_getter); + + render_followers_state("Match index", + [&](std::uint8_t i) { return report.followers[i].match_index; }, + follower_color_getter); + + assert(next_relevant_event_index == NumRelevantEvents); // Ensuring that all events can be printed + + // Separator + std::printf("--------------------------------------+----------------------------------------\n"); + + // Event log + std::printf("%s\n", EventTracer::RecentEvent::getTableHeader()); + const int num_events_to_render = static_cast(num_rows) - + static_cast(next_relevant_event_index) - + 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++) + { + 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); +} + + +void runForever(const uavcan_linux::NodePtr& node, + const std::uint8_t cluster_size, + const std::string& event_log_file, + const std::string& persistent_storage_path) +{ + /* + * Event tracer + */ + EventTracer event_tracer(MaxNumLastEvents); + ENFORCE(0 <= event_tracer.init(event_log_file.c_str())); + + /* + * Storage backend + */ + uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend; + ENFORCE(0 <= storage_backend.init(persistent_storage_path.c_str())); + + /* + * Server + */ + uavcan::dynamic_node_id_server::DistributedServer server(*node, storage_backend, event_tracer); + + const int server_init_res = server.init(node->getNodeStatusProvider().getHardwareVersion().unique_id, cluster_size); + if (server_init_res < 0) + { + throw std::runtime_error("Failed to start the server; error " + std::to_string(server_init_res)); + } + + /* + * Preparing the CLI + */ + std::printf("\x1b[2J"); // Clear entire screen; this will preserve initialization output in the scrollback + + /* + * Spinning the node + */ + uavcan::MonotonicTime last_redraw_at; + + while (true) + { + const int res = node->spin(uavcan::MonotonicDuration::fromMSec(MinUpdateInterval)); + if (res < 0) + { + std::cerr << "Spin error: " << res << std::endl; + } + + const auto ts = node->getMonotonicTime(); + + if (event_tracer.hadEvents() || (ts - last_redraw_at).toMSec() > 1000) + { + last_redraw_at = ts; + redraw(node, ts, event_tracer, server); + } + } +} + +struct Options +{ + uavcan::NodeID node_id; + std::vector 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 + { + std::srand(std::time(nullptr)); + + if (isatty(STDOUT_FILENO) != 1) + { + std::cerr << "This application cannot run if stdout is not associated with a terminal" << std::endl; + std::exit(1); + } + + auto options = parseOptions(argc, argv); + + std::cout << "Self node ID: " << int(options.node_id.get()) << "\n" + "Cluster size: " << int(options.cluster_size) << "\n" + "Storage path: " << options.storage_path << "\n" + "Num ifaces: " << options.ifaces.size() << "\n" +#ifdef NDEBUG + "Build mode: Release" +#else + "Build mode: Debug" +#endif + << std::endl; + + /* + * Preparing the storage directory + */ + options.storage_path += "/node_" + std::to_string(options.node_id.get()); + + (void)std::system(("mkdir -p '" + options.storage_path + "' &>/dev/null").c_str()); + + const auto event_log_file = options.storage_path + "/events.log"; + const auto storage_path = options.storage_path + "/storage/"; + + /* + * Starting the node + */ + auto node = initNode(options.ifaces, options.node_id, "org.uavcan.linux_app.dynamic_node_id_server"); + runForever(node, options.cluster_size, event_log_file, storage_path); + return 0; + } + catch (const std::exception& ex) + { + std::cerr << "Error: " << ex.what() << std::endl; + return 1; + } +} 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++) 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) 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..b4d6ce0a5c --- /dev/null +++ b/libuavcan_drivers/linux/include/uavcan_linux/system_utils.hpp @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#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"); + } +}; + +/** + * 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; +} + +} 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 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.mk b/libuavcan_drivers/posix/include.mk new file mode 100644 index 0000000000..84ad8e1744 --- /dev/null +++ b/libuavcan_drivers/posix/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/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..11e291a955 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/basic_file_server_backend.hpp @@ -0,0 +1,441 @@ +/**************************************************************************** +* +* 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 +#include + +namespace uavcan_posix +{ +/** + * This interface implements a POSIX compliant IFileServerBackend interface + */ +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) + { + (void)done; + using namespace std; + + return ::close(fd); + } + + virtual void init() { } + }; + + FDCacheBase fallback_; + + class FDCache : public FDCacheBase, protected uavcan::TimerBase + { + /// 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 : uavcan::Noncopyable + { + friend FDCache; + + FDCacheItem* next_; + time_t last_access_; + const int fd_; + const int oflags_; + const char* const path_; + + public: + enum { InvalidFD = -1 }; + + FDCacheItem() : + next_(NULL), + last_access_(0), + fd_(InvalidFD), + oflags_(0), + path_(NULL) + { } + + FDCacheItem(int fd, const char* path, int oflags) : + next_(NULL), + last_access_(0), + fd_(fd), + oflags_(oflags), + path_(::strndup(path, uavcan::protocol::file::Path::FieldTypes::path::MaxSize)) + { } + + ~FDCacheItem() + { + if (valid()) + { + ::free(const_cast(path_)); + } + } + + bool valid() const + { + return path_ != NULL; + } + + int getFD() const + { + return fd_; + } + + time_t getAccess() const + { + return last_access_; + } + + time_t acessed() + { + last_access_ = time(NULL); + return getAccess(); + } + + void expire() + { + last_access_ = 0; + } + + bool expired() const + { + return 0 == last_access_ || (time(NULL) - last_access_) > MaxAgeSeconds; + } + + bool equals(const char* path, int oflags) const + { + return oflags_ == oflags && 0 == ::strcmp(path, path_); + } + + bool equals(int fd) const + { + return fd_ == fd; + } + }; + + FDCacheItem* head_; + + FDCacheItem* find(const char* path, int oflags) + { + for (FDCacheItem* pi = head_; pi; pi = pi->next_) + { + if (pi->equals(path, oflags)) + { + return pi; + } + } + return NULL; + } + + FDCacheItem* find(int fd) + { + for (FDCacheItem* pi = head_; pi; pi = pi->next_) + { + if (pi->equals(fd)) + { + return pi; + } + } + return NULL; + } + + 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; + } + } + + /* 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&) + { + removeExpired(&head_); + } + + public: + 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; + + FDCacheItem* pi = find(path, oflags); + + if (pi != NULL) + { + 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 = NULL; + } + + if (pi == NULL) + { + /* + * 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 == NULL) + { + /* + * If not found just close it + */ + return FDCacheBase::close(fd); + } + remove(pi, done); + return 0; + } + }; + + FDCacheBase* fdcache_; + uavcan::INode& node_; + + FDCacheBase& getFDCache() + { + if (fdcache_ == NULL) + { + fdcache_ = new FDCache(node_); + + if (fdcache_ == NULL) + { + fdcache_ = &fallback_; + } + + fdcache_->init(); + } + return *fdcache_; + } + + /** + * 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_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); + + if (rv < 0) + { + rv = errno; + } + else + { + rv = 0; + 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; + } + + /** + * Back-end for uavcan.protocol.file.Read. + * Implementation of this method is required. + * @ref inout_size is set to @ref ReadSize; read operation is required to return exactly this amount, except + * if the end of file is reached. + * On success the method must return zero. + */ + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) + { + int rv = uavcan::protocol::file::Error::INVALID_VALUE; + + if (path.size() > 0) + { + FDCacheBase& cache = getFDCache(); + int fd = cache.open(path.c_str(), O_RDONLY); + + if (fd < 0) + { + rv = errno; + } + else + { + 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 + len = ::read(fd, out_buffer, inout_size); + + if (len < 0) + { + rv = errno; + } + else + { + rv = 0; + } + } + + (void)cache.close(fd, rv != 0 || len != inout_size); + inout_size = len; + } + } + return rv; + } + +public: + BasicFileSeverBackend(uavcan::INode& node) : + fdcache_(NULL), + node_(node) + { } + + ~BasicFileSeverBackend() + { + if (fdcache_ != &fallback_) + { + delete fdcache_; + fdcache_ = NULL; + } + } +}; +} + +#endif // Include guard 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 new file mode 100644 index 0000000000..d07f6d5624 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp @@ -0,0 +1,89 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#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 +#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 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString path_; + +protected: + virtual void onEvent(uavcan::dynamic_node_id_server::TraceCode code, uavcan::int64_t argument) + { + using namespace std; + + timespec ts = timespec(); // If clock_gettime() fails, zero time will be used + (void)clock_gettime(CLOCK_REALTIME, &ts); + + int fd = open(path_.c_str(), O_WRONLY | O_CREAT | O_APPEND, FilePermissions); + if (fd >= 0) + { + const int FormatBufferLength = 63; + char buffer[FormatBufferLength + 1]; + int n = snprintf(buffer, FormatBufferLength, "%ld.%06ld\t%d\t%lld\n", + static_cast(ts.tv_sec), static_cast(ts.tv_nsec / 1000L), + static_cast(code), static_cast(argument)); + (void)write(fd, buffer, n); // TODO FIXME Write loop + (void)close(fd); + } + } + +public: + /** + * Initializes the file based event tracer. + */ + int init(const PathString& path) + { + using namespace std; + + int rv = -uavcan::ErrInvalidParam; + + if (path.size() > 0) + { + rv = 0; + path_ = path.c_str(); + int fd = open(path_.c_str(), O_RDWR | O_CREAT | O_TRUNC, FilePermissions); + if (fd >= 0) + { + (void)close(fd); + } + } + return rv; + } +}; + +} +} + +#endif // Include guard 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 new file mode 100644 index 0000000000..76c515a8cb --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp @@ -0,0 +1,135 @@ +/**************************************************************************** +* +* Copyright (c) 2015 PX4 Development Team. All rights reserved. +* Author: Pavel Kirienko +* David Sidrane +* +****************************************************************************/ + +#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 + +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 }; + + enum { FilePermissions = 438 }; ///< 0o666 + + /** + * This type is used for the path + */ + typedef uavcan::MakeString::Type PathString; + + PathString base_path; + +protected: + 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]; + (void)memset(buffer, 0, sizeof(buffer)); + int len = read(fd, buffer, MaxStringLength); + (void)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; + } + } + value = buffer; + } + } + 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, FilePermissions); + if (fd >= 0) + { + (void)write(fd, value.c_str(), value.size()); // TODO FIXME Write loop + (void)fsync(fd); + (void)close(fd); + } + } + +public: + /** + * 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; + + 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() + MaxStringLength) > 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 new file mode 100644 index 0000000000..58143e1056 --- /dev/null +++ b/libuavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp @@ -0,0 +1,411 @@ +/**************************************************************************** +* +* 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 + +// TODO Get rid of the macro +#if !defined(DIRENT_ISFILE) && defined(DT_REG) +# define DIRENT_ISFILE(dtype) ((dtype) == DT_REG) +#endif + +namespace uavcan_posix +{ +/** + * Firmware version checking logic. + * Refer to @ref FirmwareUpdateTrigger for details. + */ +class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker +{ + enum { FilePermissions = 438 }; ///< 0o666 + + enum { MaxBasePathLength = 128 }; + + /** + * This type is used for the base path + */ + typedef uavcan::MakeString::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) + { + if (path.back() != getPathSeparator()) + { + path.push_back(getPathSeparator()); + } + } + + static void removeSlash(BasePathString& path) + { + if (path.back() == getPathSeparator()) + { + path.pop_back(); + } + } + + void setFirmwareBasePath(const char* path) + { + base_path_ = path; + } + + void setFirmwareCachePath(const char* path) + { + cache_path_ = path; + } + + int copyIfNot(const char* srcpath, const char* destpath) + { + // Does the file exist + int rv = 0; + int dfd = open(destpath, O_RDONLY, 0); + + if (dfd >= 0) + { + // Close it and exit 0 + (void)close(dfd); + } + else + { + uint8_t buffer[512]; + + dfd = open(destpath, O_WRONLY | O_CREAT, FilePermissions); + if (dfd < 0) + { + rv = -errno; + } + else + { + int sfd = open(srcpath, O_RDONLY, 0); + if (sfd < 0) + { + rv = -errno; + } + else + { + ssize_t size; + do + { + size = ::read(sfd, buffer, sizeof(buffer)); + if (size != 0) + { + if (size < 0) + { + rv = -errno; + } + else + { + if (size != write(dfd, buffer, size)) + { + rv = -errno; + } + } + } + } + while (rv == 0 && size); + + (void)close(sfd); + } + (void)close(dfd); + } + } + return rv; + } + + struct AppDescriptor + { + uint8_t signature[sizeof(uavcan::uint64_t)]; + uint64_t image_crc; + uint32_t image_size; + uint32_t vcs_commit; + uint8_t major_version; + uint8_t minor_version; + uint8_t reserved[6]; + }; + + static int getFileInfo(const char* path, AppDescriptor& descriptor) + { + using namespace std; + + const unsigned MaxChunk = 512 / sizeof(uint64_t); + + uint64_t signature = 0; + std::memcpy(&signature, "APDesc00", 8); + + int rv = -ENOENT; + uint64_t chunk[MaxChunk]; + int fd = open(path, O_RDONLY); + + if (fd >= 0) + { + AppDescriptor* pdescriptor = NULL; + + while (pdescriptor == NULL) + { + int len = read(fd, chunk, sizeof(chunk)); + + if (len == 0) + { + break; + } + + if (len < 0) + { + rv = -errno; + goto out_close; + } + + uint64_t* p = &chunk[0]; + + do + { + if (*p == signature) + { + pdescriptor = reinterpret_cast(p); // FIXME TODO This breaks strict aliasing + 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. + * + * @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, + const uavcan::protocol::GetNodeInfo::Response& node_info, + FirmwareFilePath& out_firmware_file_path) + { + using namespace std; + + /* This is a work around for two issues. + * 1) FirmwareFilePath is 40 + * 2) OK using is using 32 for max file names. + * + * So for the file: + * org.pixhawk.px4cannode-v1-0.1.59efc137.uavcan.bin + * +---fw + * +-c <----------- Files are cashed here. + * +--- 59efc137.bin <---------- A unknown Firmware file + * +---org.pixhawk.px4cannode-v1 <---------- node_info.name + * +---1.0 <-------------------------------- node_info.name's hardware_version.major,minor + * + - 59efc137.bin <----------- A well known file must match the name + * in the root fw folder, so if it does not exist + * it is copied up + */ + bool rv = false; + + char fname_root[MaxBasePathLength + 1]; + int n = snprintf(fname_root, sizeof(fname_root), "%s%s/%d.%d", + getFirmwareBasePath().c_str(), + node_info.name.c_str(), + node_info.hardware_version.major, + node_info.hardware_version.minor); + + if (n > 0 && n < (int) sizeof(fname_root) - 2) + { + DIR* const fwdir = opendir(fname_root); + + fname_root[n++] = getPathSeparator(); + fname_root[n++] = '\0'; + + if (fwdir != NULL) + { + struct dirent* pfile = NULL; + while ((pfile = readdir(fwdir)) != NULL) + { + if (DIRENT_ISFILE(pfile->d_type)) + { + // Open any bin file in there. + if (strstr(pfile->d_name, ".bin") != NULL) + { + PathString full_src_path = fname_root; + full_src_path += pfile->d_name; + + PathString full_dst_path = getFirmwareCachePath().c_str(); + full_dst_path += pfile->d_name; + + // ease the burden on the user + int cr = copyIfNot(full_src_path.c_str(), full_dst_path.c_str()); + + // We have a file, is it a valid image + AppDescriptor descriptor; + + std::memset(&descriptor, 0, sizeof(descriptor)); + + if (cr == 0 && getFileInfo(full_dst_path.c_str(), descriptor) == 0) + { + volatile AppDescriptor descriptorC = descriptor; + descriptorC.reserved[1]++; + + if (node_info.software_version.image_crc == 0 || + (node_info.software_version.major == 0 && node_info.software_version.minor == 0) || + descriptor.image_crc != node_info.software_version.image_crc) + { + rv = true; + out_firmware_file_path = pfile->d_name; + } + break; + } + } + } + } + (void)closedir(fwdir); + } + } + return rv; + } + + /** + * This method will be invoked when a node responds to the update request with an error. If the request simply + * times out, this method will not be invoked. + * Note that if by the time of arrival of the response the node is already removed, this method will not be called. + * + * SPECIAL CASE: If the node responds with ERROR_IN_PROGRESS, the class will assume that further requesting + * is not needed anymore. This method will not be invoked. + * + * @param node_id Node ID that returned this error. + * + * @param error_response Contents of the error response. It contains error code and text. + * + * @param out_firmware_file_path New firmware path if a retry is needed. Note that this argument will be + * initialized with old path, so if the same path needs to be reused, this + * argument should be left unchanged. + * + * @return True - the class will continue sending update requests with new firmware path. + * False - the node will be forgotten, new requests will not be sent. + */ + virtual bool shouldRetryFirmwareUpdate(uavcan::NodeID, + const uavcan::protocol::file::BeginFirmwareUpdate::Response&, + FirmwareFilePath&) + { + // TODO: Limit the number of attempts per node + return true; + } + +public: + const 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(); + } +}; +} + +#endif // Include guard 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);