diff --git a/CMakeLists.txt b/CMakeLists.txt index b6b61bd0a1..b889336f0d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -34,4 +34,4 @@ add_subdirectory(libuavcan) if (${CMAKE_SYSTEM_NAME} MATCHES "Linux") message(STATUS "Adding Linux support library") add_subdirectory(libuavcan_drivers/linux) -endif () \ No newline at end of file +endif () diff --git a/dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan b/dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan new file mode 100644 index 0000000000..c4ecd05bf2 --- /dev/null +++ b/dsdl/uavcan/protocol/dynamic_node_id/559.Allocation.uavcan @@ -0,0 +1,64 @@ +# +# This message is used for dynamic Node ID allocation. This algorithm is an optional extension to the UAVCAN +# specification, and it is not mandatory to support it. +# +# On the client's side the protocol is defined through the following set of rules: +# +# Rule A. On initialization: +# 1. The client subscribes to this message. +# 2. The client starts the Request Timer with interval of DEFAULT_REQUEST_INTERVAL_MS. +# +# Rule B. On expiration of Request Timer: +# 1. Request Timer restarts. +# 2. The client broadcasts a first-stage Allocation request message, where the fields are assigned following values: +# node_id - preferred node ID, or zero if the client doesn't have any preference +# first_part_of_unique_id - true +# unique_id - first MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of unique ID +# +# Rule C. On any Allocation message, even if other rules also match: +# 1. Request Timer restarts. +# +# Rule D. On an Allocation message WHERE (source node ID is non-anonymous) AND (client's unique ID starts with the +# bytes available in the field unique_id) AND (unique_id is less than 16 bytes long): +# 1. The client broadcasts a second-stage Allocation request message, where the fields are assigned following values: +# node_id - same value as in the first-stage +# first_part_of_unique_id - false +# unique_id - at most MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST bytes of local unique ID with an offset +# equal to number of bytes in the received unique ID +# +# Rule E. On an Allocation message WHERE (source node ID is non-anonymous) AND (unique_id fully matches client's +# unique ID) AND (node_id in the received message is not zero): +# 1. Request Timer stops. +# 2. The client initializes its node_id with the received value. +# 3. The client terminates subscription to Allocation messages. +# 4. Exit. +# + +# +# Recommended request transmission period. +# +uint16 DEFAULT_REQUEST_PERIOD_MS = 1000 + +# +# 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. +# +uint8 MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST = 7 + +# +# If transfer is anonymous, this is the preferred ID. +# If transfer is non-anonymous, this is allocated ID. +# +uint7 node_id + +# +# If transfer is anonymous, this field indicates first-stage request. +# If transfer is non-anonymous, this field should be ignored. +# +bool first_part_of_unique_id + +# +# If transfer is anonymous, this array must not contain more than MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST items. +# Note that array is tail-optimized, i.e. it will not be prepended with length field. +# +uint8[<=16] unique_id diff --git a/dsdl/uavcan/protocol/file/589.BeginFirmwareUpdate.uavcan b/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan similarity index 87% rename from dsdl/uavcan/protocol/file/589.BeginFirmwareUpdate.uavcan rename to dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan index 0f0713a7b2..ecc6228f43 100644 --- a/dsdl/uavcan/protocol/file/589.BeginFirmwareUpdate.uavcan +++ b/dsdl/uavcan/protocol/file/580.BeginFirmwareUpdate.uavcan @@ -1,5 +1,5 @@ # -# This service initiates a firmware update on a remote node. +# This service initiates firmware update on a remote node. # # The node that is being updated (slave) will retrieve the firmware image file 'image_file_remote_path' from the node # 'source_node_id' using the file read service, then it will update the firmware and reboot. Alternatively, this @@ -23,6 +23,7 @@ Path image_file_remote_path # Empty to invoke an alternative bootloader uint8 ERROR_OK = 0 uint8 ERROR_INVALID_MODE = 1 # Cannot perform the update right now (e.g. the vehicle is operating) +uint8 ERROR_IN_PROGRESS = 2 # Firmware update is already in progess, and the slave doesn't want to restart uint8 ERROR_UNKNOWN = 255 uint8 error diff --git a/dsdl/uavcan/protocol/file/580.GetInfo.uavcan b/dsdl/uavcan/protocol/file/585.GetInfo.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/580.GetInfo.uavcan rename to dsdl/uavcan/protocol/file/585.GetInfo.uavcan diff --git a/dsdl/uavcan/protocol/file/581.GetDirectoryEntryInfo.uavcan b/dsdl/uavcan/protocol/file/586.GetDirectoryEntryInfo.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/581.GetDirectoryEntryInfo.uavcan rename to dsdl/uavcan/protocol/file/586.GetDirectoryEntryInfo.uavcan diff --git a/dsdl/uavcan/protocol/file/582.Delete.uavcan b/dsdl/uavcan/protocol/file/587.Delete.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/582.Delete.uavcan rename to dsdl/uavcan/protocol/file/587.Delete.uavcan diff --git a/dsdl/uavcan/protocol/file/583.Read.uavcan b/dsdl/uavcan/protocol/file/588.Read.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/583.Read.uavcan rename to dsdl/uavcan/protocol/file/588.Read.uavcan diff --git a/dsdl/uavcan/protocol/file/584.Write.uavcan b/dsdl/uavcan/protocol/file/589.Write.uavcan similarity index 100% rename from dsdl/uavcan/protocol/file/584.Write.uavcan rename to dsdl/uavcan/protocol/file/589.Write.uavcan diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index aece082ddb..7dde549c00 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -60,6 +60,15 @@ public: MonotonicDuration getTxTimeout() const { return tx_timeout_; } void setTxTimeout(MonotonicDuration tx_timeout); + /** + * By default, attempt to send a transfer from passive mode will result in an error @ref ErrPassive. + * This option allows to enable sending anonymous transfers from passive mode. + */ + void allowAnonymousTransfers() + { + sender_->allowAnonymousTransfers(); + } + INode& getNode() const { return node_; } }; diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 93cf7e2583..46a272fdee 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -67,6 +67,7 @@ public: TransferID getTransferID() const { return safeget(); } NodeID getSrcNodeID() const { return safeget(); } uint8_t getIfaceIndex() const { return safeget(); } + bool isAnonymousTransfer() const { return safeget(); } }; /** @@ -200,6 +201,15 @@ protected: return genericStart(&Dispatcher::registerServiceResponseListener); } + /** + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. + */ + void allowAnonymousTransfers() + { + forwarder_->allowAnonymousTransfers(); + } + /** * Terminate the subscription. * Dispatcher core will remove this instance from the subscribers list. diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index 6abb3bd533..2b720f7ee1 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -87,6 +87,7 @@ public: */ using BaseType::init; + using BaseType::allowAnonymousTransfers; using BaseType::getTransferSender; using BaseType::getMinTxTimeout; using BaseType::getMaxTxTimeout; diff --git a/libuavcan/include/uavcan/node/subscriber.hpp b/libuavcan/include/uavcan/node/subscriber.hpp index 1112a6abc0..f5c19f4c78 100644 --- a/libuavcan/include/uavcan/node/subscriber.hpp +++ b/libuavcan/include/uavcan/node/subscriber.hpp @@ -111,6 +111,7 @@ public: return BaseType::startAsMessageListener(); } + using BaseType::allowAnonymousTransfers; using BaseType::stop; using BaseType::getFailureCount; }; diff --git a/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp new file mode 100644 index 0000000000..1f11f46663 --- /dev/null +++ b/libuavcan/include/uavcan/protocol/dynamic_node_id_allocation_client.hpp @@ -0,0 +1,91 @@ +/* + * 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 + +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * This class implements client-side logic of dynamic node ID allocation procedure. + * + * Once started, the object will be publishing dynamic node ID allocation requests at the default frequency defined + * by the specification, until a Node ID is granted by the allocator. + * + * If the local node is equipped with redundant CAN interfaces, all of them will be used for publishing requests + * and listening for responses. + * + * Once dynamic allocation is complete (or not needed anymore), the object can be deleted. + */ +class DynamicNodeIDAllocationClient : private TimerBase +{ + typedef MethodBinder&)> + AllocationCallback; + + Publisher dnida_pub_; + Subscriber dnida_sub_; + + uint8_t unique_id_[protocol::HardwareVersion::FieldTypes::unique_id::MaxSize]; + NodeID preferred_node_id_; + NodeID allocated_node_id_; + NodeID allocator_node_id_; + + void terminate(); + + virtual void handleTimerEvent(const TimerEvent&); + + void handleAllocation(const ReceivedDataStructure& msg); + +public: + DynamicNodeIDAllocationClient(INode& node) + : TimerBase(node) + , dnida_pub_(node) + , dnida_sub_(node) + { } + + /** + * @param hardware_version Hardware version information, where unique_id must be set correctly. + * @param preferred_node_id Node ID that the application would like to take; set to broadcast (zero) if + * the application doesn't have any preference (this is default). + * @return Zero on success + * Negative error code on failure + * -ErrLogic if 1. the node is not in passive mode or 2. the client is already started + */ + int start(const protocol::HardwareVersion& hardware_version, const NodeID preferred_node_id = NodeID::Broadcast); + + /** + * Use this method to determine when allocation is complete. + */ + bool isAllocationComplete() const { return getAllocatedNodeID().isUnicast(); } + + /** + * This method allows to retrieve the node ID that was allocated to the local node. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, a non-unicast node ID will be returned. + */ + NodeID getAllocatedNodeID() const { return allocated_node_id_; } + + /** + * This method allows to retrieve node ID of the allocator that granted our Node ID. + * If no node ID was allocated yet, the returned node ID will be invalid (non-unicast). + * @return If allocation is complete, a valid unicast node ID will be returned. + * If allocation is not complete yet, an non-unicast node ID will be returned. + */ + NodeID getAllocatorNodeID() const { return allocator_node_id_; } +}; + +} + +#endif // UAVCAN_PROTOCOL_DYNAMIC_NODE_ID_ALLOCATION_CLIENT_HPP_INCLUDED diff --git a/libuavcan/include/uavcan/std.hpp b/libuavcan/include/uavcan/std.hpp index c04ced0c89..a1e63ea469 100644 --- a/libuavcan/include/uavcan/std.hpp +++ b/libuavcan/include/uavcan/std.hpp @@ -7,6 +7,7 @@ #include #include +#include #if !defined(UAVCAN_CPP_VERSION) || !defined(UAVCAN_CPP11) # error UAVCAN_CPP_VERSION diff --git a/libuavcan/include/uavcan/transport/dispatcher.hpp b/libuavcan/include/uavcan/transport/dispatcher.hpp index ac12bbd2b7..a671f4ac85 100644 --- a/libuavcan/include/uavcan/transport/dispatcher.hpp +++ b/libuavcan/include/uavcan/transport/dispatcher.hpp @@ -115,6 +115,7 @@ public: : canio_(driver, allocator, sysclock) , sysclock_(sysclock) , outgoing_transfer_reg_(otr) + , self_node_id_(NodeID::Broadcast) // Default , self_node_id_is_set_(false) { } diff --git a/libuavcan/include/uavcan/transport/frame.hpp b/libuavcan/include/uavcan/transport/frame.hpp index 49d1da2214..5d149ba837 100644 --- a/libuavcan/include/uavcan/transport/frame.hpp +++ b/libuavcan/include/uavcan/transport/frame.hpp @@ -50,7 +50,7 @@ public: { UAVCAN_ASSERT((transfer_type == TransferTypeMessageBroadcast) == dst_node_id.isBroadcast()); UAVCAN_ASSERT(data_type_id.isValid()); - UAVCAN_ASSERT(src_node_id != dst_node_id); + UAVCAN_ASSERT(src_node_id.isUnicast() ? (src_node_id != dst_node_id) : true); UAVCAN_ASSERT(frame_index <= MaxIndex); } diff --git a/libuavcan/include/uavcan/transport/transfer_listener.hpp b/libuavcan/include/uavcan/transport/transfer_listener.hpp index 62097459c7..9c251d276d 100644 --- a/libuavcan/include/uavcan/transport/transfer_listener.hpp +++ b/libuavcan/include/uavcan/transport/transfer_listener.hpp @@ -50,6 +50,11 @@ public: */ virtual void release() { } + /** + * Whether this is a anonymous transfer + */ + virtual bool isAnonymousTransfer() const { return false; } + MonotonicTime getMonotonicTimestamp() const { return ts_mono_; } UtcTime getUtcTimestamp() const { return ts_utc_; } TransferType getTransferType() const { return transfer_type_; } @@ -68,6 +73,7 @@ class UAVCAN_EXPORT SingleFrameIncomingTransfer : public IncomingTransfer public: explicit SingleFrameIncomingTransfer(const RxFrame& frm); virtual int read(unsigned offset, uint8_t* data, unsigned len) const; + virtual bool isAnonymousTransfer() const; }; /** @@ -93,6 +99,7 @@ class UAVCAN_EXPORT TransferListenerBase : public LinkedListNode& receivers_; ITransferBufferManager& bufmgr_; TransferPerfCounter& perf_; + bool allow_anonymous_transfers_; class TimedOutReceiverPredicate { @@ -119,17 +126,25 @@ protected: , receivers_(receivers) , bufmgr_(bufmgr) , perf_(perf) + , allow_anonymous_transfers_(false) { } virtual ~TransferListenerBase() { } void handleReception(TransferReceiver& receiver, const RxFrame& frame, TransferBufferAccessor& tba); + void handleAnonymousTransferReception(const RxFrame& frame); virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0; public: const DataTypeDescriptor& getDataTypeDescriptor() const { return data_type_; } + /** + * By default, anonymous transfers will be ignored. + * This option allows to enable reception of anonymous transfers. + */ + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } + void cleanup(MonotonicTime ts); virtual void handleFrame(const RxFrame& frame); diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 27ab8a1038..b67edf23a7 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -25,6 +25,7 @@ class UAVCAN_EXPORT TransferSender const TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; + bool allow_anonymous_transfers_; Dispatcher& dispatcher_; @@ -46,6 +47,7 @@ public: , crc_base_(data_type.getSignature().toTransferCRC()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) , dispatcher_(dispatcher) { } @@ -59,6 +61,15 @@ public: iface_mask_ = iface_mask; } + /** + * Anonymous transfers (i.e. transfers that don't carry a valid Source Node ID) can be sent if + * the local node is configured in passive mode (i.e. the node doesn't have a valid Node ID). + * By default, this class will return an error if it is asked to send a transfer while the + * node is configured in passive mode. However, if this option is enabled, it will be possible + * to send anonymous transfers from passive mode. + */ + void allowAnonymousTransfers() { allow_anonymous_transfers_ = true; } + /** * Send with explicit Transfer ID. * Should be used only for service responses, where response TID should match request TID. diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp new file mode 100644 index 0000000000..5b3b4e571c --- /dev/null +++ b/libuavcan/src/protocol/uc_dynamic_node_id_allocation_client.cpp @@ -0,0 +1,177 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +void DynamicNodeIDAllocationClient::terminate() +{ + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Client terminated"); + stop(); + dnida_sub_.stop(); +} + +void DynamicNodeIDAllocationClient::handleTimerEvent(const TimerEvent&) +{ + // This method implements Rule B + UAVCAN_ASSERT(preferred_node_id_.isValid()); + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + // Filling the message + protocol::dynamic_node_id::Allocation msg; + msg.node_id = preferred_node_id_.get(); + msg.first_part_of_unique_id = true; + + msg.unique_id.resize(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST); + copy(unique_id_, unique_id_ + msg.unique_id.size(), msg.unique_id.begin()); + UAVCAN_ASSERT(equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)); + + // Broadcasting + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "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"); + } +} + +void DynamicNodeIDAllocationClient::handleAllocation( + const ReceivedDataStructure& msg) +{ + /* + * TODO This method can blow the stack easily + */ + UAVCAN_ASSERT(preferred_node_id_.isValid()); + if (isAllocationComplete()) + { + UAVCAN_ASSERT(0); + terminate(); + return; + } + + startPeriodic(getPeriod()); // Restarting the timer - Rule C + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "Request timer reset because of Allocation message from %d", + static_cast(msg.getSrcNodeID().get())); + + // Rule D + if (!msg.isAnonymousTransfer() && + msg.unique_id.size() > 0 && + msg.unique_id.size() < msg.unique_id.capacity() && + equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_)) + { + // Filling the response message + const uint8_t size_of_unique_id_in_response = + min(protocol::dynamic_node_id::Allocation::MAX_LENGTH_OF_UNIQUE_ID_IN_REQUEST, + static_cast(msg.unique_id.capacity() - msg.unique_id.size())); + + protocol::dynamic_node_id::Allocation second_stage; + second_stage.node_id = preferred_node_id_.get(); + second_stage.first_part_of_unique_id = false; + + second_stage.unique_id.resize(size_of_unique_id_in_response); + + copy(unique_id_ + msg.unique_id.size(), + unique_id_ + msg.unique_id.size() + size_of_unique_id_in_response, + second_stage.unique_id.begin()); + + UAVCAN_ASSERT(equal(second_stage.unique_id.begin(), + second_stage.unique_id.end(), + unique_id_ + msg.unique_id.size())); + + // Broadcasting the response + UAVCAN_TRACE("DynamicNodeIDAllocationClient", + "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"); + } + } + + // Rule E + if (!msg.isAnonymousTransfer() && + msg.unique_id.size() == msg.unique_id.capacity() && + equal(msg.unique_id.begin(), msg.unique_id.end(), unique_id_) && + msg.node_id > 0) + { + allocated_node_id_ = msg.node_id; + allocator_node_id_ = msg.getSrcNodeID(); + UAVCAN_TRACE("DynamicNodeIDAllocationClient", "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) +{ + terminate(); + + // Allocation is not possible if node ID is already set + if (dnida_pub_.getNode().getNodeID().isUnicast()) + { + return -ErrLogic; + } + + // Unique ID initialization & validation + copy(hardware_version.unique_id.begin(), hardware_version.unique_id.end(), unique_id_); + bool unique_id_is_zero = true; + for (uint8_t i = 0; i < sizeof(unique_id_); i++) + { + if (unique_id_[i] != 0) + { + unique_id_is_zero = false; + break; + } + } + + if (unique_id_is_zero) + { + return -ErrInvalidParam; + } + + if (!preferred_node_id.isValid()) // Only broadcast and unicast are allowed + { + return -ErrInvalidParam; + } + + // Initializing the fields + preferred_node_id_ = preferred_node_id; + allocated_node_id_ = NodeID(); + allocator_node_id_ = NodeID(); + UAVCAN_ASSERT(preferred_node_id_.isValid()); + UAVCAN_ASSERT(!allocated_node_id_.isValid()); + UAVCAN_ASSERT(!allocator_node_id_.isValid()); + + // Initializing node objects - Rule A + int res = dnida_pub_.init(); + if (res < 0) + { + return res; + } + dnida_pub_.allowAnonymousTransfers(); + + res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDAllocationClient::handleAllocation)); + if (res < 0) + { + return res; + } + dnida_sub_.allowAnonymousTransfers(); + + startPeriodic(MonotonicDuration::fromMSec(protocol::dynamic_node_id::Allocation::DEFAULT_REQUEST_PERIOD_MS)); + + return 0; +} + +} diff --git a/libuavcan/src/transport/uc_frame.cpp b/libuavcan/src/transport/uc_frame.cpp index 3a3938a01b..3a6f6ac814 100644 --- a/libuavcan/src/transport/uc_frame.cpp +++ b/libuavcan/src/transport/uc_frame.cpp @@ -170,9 +170,12 @@ bool Frame::isValid() const const bool invalid = (frame_index_ > MaxIndex) || ((frame_index_ == MaxIndex) && !last_frame_) || - (!src_node_id_.isUnicast()) || + (!src_node_id_.isValid()) || (!dst_node_id_.isValid()) || - (src_node_id_ == dst_node_id_) || + (src_node_id_.isUnicast() ? (src_node_id_ == dst_node_id_) : false) || + (src_node_id_.isBroadcast() + ? (!last_frame_ || (frame_index_ > 0) || (transfer_type_ != TransferTypeMessageBroadcast)) + : false) || ((transfer_type_ == TransferTypeMessageBroadcast) != dst_node_id_.isBroadcast()) || (transfer_type_ >= NumTransferTypes) || (static_cast(payload_len_) > getMaxPayloadLen()) || diff --git a/libuavcan/src/transport/uc_transfer_listener.cpp b/libuavcan/src/transport/uc_transfer_listener.cpp index d193977390..6a5d38fbec 100644 --- a/libuavcan/src/transport/uc_transfer_listener.cpp +++ b/libuavcan/src/transport/uc_transfer_listener.cpp @@ -50,6 +50,11 @@ int SingleFrameIncomingTransfer::read(unsigned offset, uint8_t* data, unsigned l return int(len); } +bool SingleFrameIncomingTransfer::isAnonymousTransfer() const +{ + return (getTransferType() == TransferTypeMessageBroadcast) && getSrcNodeID().isBroadcast(); +} + /* * MultiFrameIncomingTransfer */ @@ -172,6 +177,16 @@ void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxF } } +void TransferListenerBase::handleAnonymousTransferReception(const RxFrame& frame) +{ + if (allow_anonymous_transfers_) + { + perf_.addRxTransfer(); + SingleFrameIncomingTransfer it(frame); + handleIncomingTransfer(it); + } +} + void TransferListenerBase::cleanup(MonotonicTime ts) { receivers_.removeWhere(TimedOutReceiverPredicate(ts, bufmgr_)); @@ -180,26 +195,40 @@ void TransferListenerBase::cleanup(MonotonicTime ts) void TransferListenerBase::handleFrame(const RxFrame& frame) { - const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); - - TransferReceiver* recv = receivers_.access(key); - if (recv == NULL) + if (frame.getSrcNodeID().isUnicast()) // Normal transfer { - if (!frame.isFirst()) - { - return; - } + const TransferBufferManagerKey key(frame.getSrcNodeID(), frame.getTransferType()); - TransferReceiver new_recv; - recv = receivers_.insert(key, new_recv); + TransferReceiver* recv = receivers_.access(key); if (recv == NULL) { - UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); - return; + if (!frame.isFirst()) + { + return; + } + + TransferReceiver new_recv; + recv = receivers_.insert(key, new_recv); + if (recv == NULL) + { + UAVCAN_TRACE("TransferListener", "Receiver registration failed; frame %s", frame.toString().c_str()); + return; + } } + TransferBufferAccessor tba(bufmgr_, key); + handleReception(*recv, frame, tba); + } + else if (frame.getSrcNodeID().isBroadcast() && + frame.isFirst() && + frame.isLast() && + frame.getDstNodeID().isBroadcast()) // Anonymous transfer + { + handleAnonymousTransferReception(frame); + } + else + { + UAVCAN_TRACE("TransferListenerBase", "Invalid frame: %s", frame.toString().c_str()); // Invalid frame } - TransferBufferAccessor tba(bufmgr_, key); - handleReception(*recv, frame, tba); } } diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index 1fda8c6a46..0e603a4b54 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -19,15 +19,35 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, TransferID tid) { + if (payload_len > MaxTransferPayloadLen) + { + UAVCAN_ASSERT(0); + return -ErrInvalidParam; + } + + Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); + UAVCAN_TRACE("TransferSender", "%s", frame.toString().c_str()); + + /* + * Checking if we're allowed to send. + * In passive mode we can send only anonymous transfers, if they are enabled. + */ if (dispatcher_.isPassiveMode()) { - return -ErrPassiveMode; + const bool allow = allow_anonymous_transfers_ && + (transfer_type == TransferTypeMessageBroadcast) && + (int(payload_len) <= frame.getMaxPayloadLen()); + if (!allow) + { + return -ErrPassiveMode; + } } dispatcher_.getTransferPerfCounter().addTxTransfer(); - Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); - + /* + * Sending frames + */ if (frame.getMaxPayloadLen() >= int(payload_len)) // Single Frame Transfer { const int res = frame.setPayload(payload, payload_len); diff --git a/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp new file mode 100644 index 0000000000..ecf84ec493 --- /dev/null +++ b/libuavcan/test/protocol/dynamic_node_id_allocation_client.cpp @@ -0,0 +1,182 @@ +/* + * Copyright (C) 2015 Pavel Kirienko + */ + +#include +#include +#include "helpers.hpp" + + +TEST(DynamicNodeIDAllocationClient, Basic) +{ + // Node A is Allocator, Node B is Allocatee + InterlinkedTestNodesWithSysClock nodes(uavcan::NodeID(10), uavcan::NodeID::Broadcast); + + uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + + /* + * Client initialization + */ + uavcan::protocol::HardwareVersion hwver; + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver)); // Empty hardware version is not allowed + + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrInvalidParam, dnidac.start(hwver, uavcan::NodeID())); + + const uavcan::NodeID PreferredNodeID = 42; + ASSERT_LE(0, dnidac.start(hwver, PreferredNodeID)); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Subscriber (server emulation) + */ + SubscriberWithCollector dynid_sub(nodes.a); + ASSERT_LE(0, dynid_sub.start()); + dynid_sub.subscriber.allowAnonymousTransfers(); + + /* + * Monitoring requests at 1Hz + */ + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "First-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_TRUE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + hwver.unique_id.begin())); + dynid_sub.collector.msg.reset(); + + // Rate validation + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + + // Second - rate is 1 Hz + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(500)); + ASSERT_TRUE(dynid_sub.collector.msg.get()); + dynid_sub.collector.msg.reset(); + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Publisher (server emulation) + */ + uavcan::Publisher dynid_pub(nodes.a); + ASSERT_LE(0, dynid_pub.init()); + + /* + * Sending some some Allocation messages - the timer will keep restarting + */ + for (int i = 0; i < 10; i++) + { + uavcan::protocol::dynamic_node_id::Allocation msg; // Contents of the message doesn't matter + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(210)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + /* + * Responding with partially matching unique ID - the client will respond with second-stage request immediately + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(7); + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 7, msg.unique_id.begin()); + + std::cout << "First-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Second-stage request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + hwver.unique_id.begin() + 7)); + dynid_sub.collector.msg.reset(); + } + + /* + * Responding with second-stage offer, expecting the last request back + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(14); + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.begin() + 14, msg.unique_id.begin()); + + std::cout << "Second-stage offer:\n" << msg << std::endl; + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(100)); + + ASSERT_TRUE(dynid_sub.collector.msg.get()); + std::cout << "Last request:\n" << *dynid_sub.collector.msg << std::endl; + ASSERT_EQ(PreferredNodeID.get(), dynid_sub.collector.msg->node_id); + ASSERT_FALSE(dynid_sub.collector.msg->first_part_of_unique_id); + ASSERT_TRUE(uavcan::equal(dynid_sub.collector.msg->unique_id.begin(), + dynid_sub.collector.msg->unique_id.end(), + hwver.unique_id.begin() + 14)); + dynid_sub.collector.msg.reset(); + } + + ASSERT_FALSE(dnidac.getAllocatedNodeID().isValid()); + ASSERT_FALSE(dnidac.getAllocatorNodeID().isValid()); + ASSERT_FALSE(dnidac.isAllocationComplete()); + + /* + * Now we have full unique ID for this client received, and it is possible to grant allocation + */ + { + uavcan::protocol::dynamic_node_id::Allocation msg; + msg.unique_id.resize(16); + msg.node_id = 72; + uavcan::copy(hwver.unique_id.begin(), hwver.unique_id.end(), msg.unique_id.begin()); + + ASSERT_FALSE(dynid_sub.collector.msg.get()); + ASSERT_LE(0, dynid_pub.broadcast(msg)); + nodes.spinBoth(uavcan::MonotonicDuration::fromMSec(1100)); + ASSERT_FALSE(dynid_sub.collector.msg.get()); + } + + ASSERT_EQ(uavcan::NodeID(72), dnidac.getAllocatedNodeID()); + ASSERT_EQ(uavcan::NodeID(10), dnidac.getAllocatorNodeID()); + ASSERT_TRUE(dnidac.isAllocationComplete()); +} + + +TEST(DynamicNodeIDAllocationClient, NonPassiveMode) +{ + InterlinkedTestNodesWithSysClock nodes; + + uavcan::DynamicNodeIDAllocationClient dnidac(nodes.b); + + uavcan::GlobalDataTypeRegistry::instance().reset(); + uavcan::DefaultDataTypeRegistrator _reg1; + (void)_reg1; + + uavcan::protocol::HardwareVersion hwver; + for (uavcan::uint8_t i = 0; i < hwver.unique_id.size(); i++) + { + hwver.unique_id[i] = i; + } + + ASSERT_LE(-uavcan::ErrLogic, dnidac.start(hwver)); +} diff --git a/libuavcan/test/transport/frame.cpp b/libuavcan/test/transport/frame.cpp index 5f55033f91..719dcf325b 100644 --- a/libuavcan/test/transport/frame.cpp +++ b/libuavcan/test/transport/frame.cpp @@ -162,7 +162,18 @@ TEST(Frame, FrameParsing) can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageUnicast << 17) | (456 << 19); - ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID + ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with unicast transfer + + can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression + (2 << 0) | (0 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19); + ASSERT_FALSE(frame.parse(can)); // Broadcast Src Node ID with multiframe broadcast transfer + + /* + * Broadcast SNID exceptions + */ + can.id = CanFrame::FlagEFF | // cppcheck-suppress duplicateExpression + (2 << 0) | (1 << 3) | (0 << 4) | (0 << 10) | (uavcan::TransferTypeMessageBroadcast << 17) | (456 << 19); + ASSERT_TRUE(frame.parse(can)); // Broadcast Src Node ID with single frame broadcast transfer } diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp index 5a4265e168..fd42c1f118 100644 --- a/libuavcan/test/transport/transfer_listener.cpp +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -243,3 +243,38 @@ TEST(TransferListener, MaximumTransferLength) ASSERT_TRUE(subscriber.isEmpty()); } + + +TEST(TransferListener, AnonymousTransfers) +{ + const uavcan::DataTypeDescriptor type(uavcan::DataTypeKindMessage, 123, uavcan::DataTypeSignature(123456789), "A"); + + uavcan::PoolManager<1> poolmgr; + uavcan::TransferPerfCounter perf; + TestListener<0, 0, 0> subscriber(perf, type, poolmgr); + + TransferListenerEmulator emulator(subscriber, type); + const Transfer transfers[] = + { + emulator.makeTransfer(uavcan::TransferTypeMessageUnicast, 0, "12345678"), // Invalid - not broadcast + emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "12345678"), // Valid + emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "123456789"), // Invalid - not SFT + emulator.makeTransfer(uavcan::TransferTypeMessageBroadcast, 0, "") // Valid + }; + + emulator.send(transfers); + + // Nothing will be received, because anonymous transfers are disabled by default + ASSERT_TRUE(subscriber.isEmpty()); + + subscriber.allowAnonymousTransfers(); + + // Re-send everything again + emulator.send(transfers); + + // Now the anonymous transfers are enabled + ASSERT_TRUE(subscriber.matchAndPop(transfers[1])); // Only SFT broadcast will be accepted + ASSERT_TRUE(subscriber.matchAndPop(transfers[3])); + + ASSERT_TRUE(subscriber.isEmpty()); +} diff --git a/libuavcan/test/transport/transfer_sender.cpp b/libuavcan/test/transport/transfer_sender.cpp index 5d65943511..1fe2e22dbc 100644 --- a/libuavcan/test/transport/transfer_sender.cpp +++ b/libuavcan/test/transport/transfer_sender.cpp @@ -235,11 +235,24 @@ TEST(TransferSender, PassiveMode) static const uint8_t Payload[] = {1, 2, 3, 4, 5}; + // By default, sending in passive mode is not enabled ASSERT_EQ(-uavcan::ErrPassiveMode, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + // Overriding the default + sender.allowAnonymousTransfers(); + + // OK, now we can broadcast in any mode + ASSERT_LE(0, sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageBroadcast, uavcan::NodeID::Broadcast)); + + // ...but not unicast or anything else + ASSERT_EQ(-uavcan::ErrPassiveMode, + sender.send(Payload, sizeof(Payload), tsMono(1000), uavcan::MonotonicTime(), + uavcan::TransferTypeMessageUnicast, uavcan::NodeID(42))); + EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getErrorCount()); - EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getTxTransferCount()); + EXPECT_EQ(1, dispatcher.getTransferPerfCounter().getTxTransferCount()); EXPECT_EQ(0, dispatcher.getTransferPerfCounter().getRxTransferCount()); } diff --git a/libuavcan/test/transport/transfer_test_helpers.hpp b/libuavcan/test/transport/transfer_test_helpers.hpp index 449c0afb9a..f27a603c49 100644 --- a/libuavcan/test/transport/transfer_test_helpers.hpp +++ b/libuavcan/test/transport/transfer_test_helpers.hpp @@ -129,6 +129,14 @@ public: const Transfer rx(transfer, Base::getDataTypeDescriptor()); transfers_.push(rx); std::cout << "Received transfer: " << rx.toString() << std::endl; + + const bool single_frame = dynamic_cast(&transfer) != NULL; + + const bool anonymous = single_frame && + transfer.getSrcNodeID().isBroadcast() && + (transfer.getTransferType() == uavcan::TransferTypeMessageBroadcast); + + ASSERT_EQ(anonymous, transfer.isAnonymousTransfer()); } bool matchAndPop(const Transfer& reference)