From d4e49d518a13c01bf15c6c34f6e64bff1fec80a8 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 May 2015 13:49:01 +0300 Subject: [PATCH 1/8] First step towards introducing the global RX object buffer --- .../include/uavcan/node/marshal_buffer.hpp | 11 ++++--- libuavcan/include/uavcan/node/node.hpp | 31 ++++++++++--------- libuavcan/src/node/uc_generic_publisher.cpp | 4 +-- libuavcan/test/node/test_node.hpp | 2 +- 4 files changed, 25 insertions(+), 23 deletions(-) diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index 35a7e0885a..3dad463a83 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -18,7 +18,8 @@ class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer { public: virtual const uint8_t* getDataPtr() const = 0; - virtual unsigned getDataLength() const = 0; + virtual unsigned getMaxWritePos() const = 0; + virtual unsigned getSize() const = 0; }; /** @@ -43,10 +44,8 @@ public: /** * Default implementation of marshal buffer provider. - * This implementation provides the buffer large enough to - * serialize any UAVCAN data structure. */ -template +template class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider { class Buffer : public IMarshalBuffer @@ -65,7 +64,9 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } - virtual unsigned getDataLength() const { return buf_.getMaxWritePos(); } + virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } + + virtual unsigned getSize() const { return MaxSize_; } public: void reset() { buf_.reset(); } diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index 65b478b7a7..5175bd2115 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -47,21 +47,24 @@ namespace uavcan * be allocated in the memory pool if needed. * Default value is acceptable for any use case. * - * @tparam OutgoingTransferMaxPayloadLen Maximum outgoing transfer payload length. - * It's pointless to make this value larger than - * @ref MaxTransferPayloadLen, which is default. - * Note that in tiny mode the default value is actually - * smaller than @ref MaxTransferPayloadLen (may cause - * run-time failures). + * @tparam MarshalBufferSize Size of the marshal buffer, that is used to provide short-term temporary storage for: + * 1. Serialized data for TX transfers; + * 2. De-serialized data for RX transfers. + * The buffer must be large enough to accommodate largest serialized TX transfer and + * largest de-serialized RX data structure. The former value is constant for UAVCAN, the + * latter is platform-dependent (depends on the field padding, memory alignment, pointer + * size, etc.). + * The default value should be enough for all use cases on virtually all platforms. + * If this value is not large enough, transport objects (such as Subscriber<>, + * Publisher<>, Service*<>) will be failing at run time during initialization. */ template class UAVCAN_EXPORT Node : public INode { @@ -73,7 +76,7 @@ class UAVCAN_EXPORT Node : public INode typedef PoolAllocator Allocator; Allocator pool_allocator_; - MarshalBufferProvider marsh_buf_; + MarshalBufferProvider marsh_buf_; OutgoingTransferRegistry outgoing_trans_reg_; Scheduler scheduler_; @@ -265,9 +268,8 @@ public: // ---------------------------------------------------------------------------- -template -int Node::start( +template +int Node::start( const TransferPriority priority) { if (started_) @@ -313,9 +315,8 @@ fail: #if !UAVCAN_TINY -template -int Node:: +template +int Node:: checkNetworkCompatibility(NetworkCompatibilityCheckResult& result) { if (!started_) diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp index 3ebc7bd699..09f361f5b0 100644 --- a/libuavcan/src/node/uc_generic_publisher.cpp +++ b/libuavcan/src/node/uc_generic_publisher.cpp @@ -47,12 +47,12 @@ int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferT { if (tid) { - return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), + return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), blocking_deadline, transfer_type, dst_node_id, *tid); } else { - return sender_->send(buffer.getDataPtr(), buffer.getDataLength(), getTxDeadline(), + return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), blocking_deadline, transfer_type, dst_node_id); } } diff --git a/libuavcan/test/node/test_node.hpp b/libuavcan/test/node/test_node.hpp index 543f99a50d..5a943ae6da 100644 --- a/libuavcan/test/node/test_node.hpp +++ b/libuavcan/test/node/test_node.hpp @@ -20,7 +20,7 @@ struct TestNode : public uavcan::INode { uavcan::PoolAllocator pool; uavcan::PoolManager<1> poolmgr; - uavcan::MarshalBufferProvider<> buffer_provider; + uavcan::MarshalBufferProvider<512> buffer_provider; uavcan::OutgoingTransferRegistry<8> otr; uavcan::Scheduler scheduler; uint64_t internal_failure_count; From d20f8e7356def145b7f70d7a9fd913018bd493ba Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 16:47:02 +0300 Subject: [PATCH 2/8] Using global RX object buffer --- libuavcan/include/uavcan/error.hpp | 1 + .../uavcan/node/generic_subscriber.hpp | 130 +++++++++++++----- .../include/uavcan/node/marshal_buffer.hpp | 6 +- .../include/uavcan/node/service_client.hpp | 12 +- libuavcan/test/node/service_client.cpp | 7 +- .../test/protocol/data_type_info_provider.cpp | 3 +- libuavcan/test/protocol/helpers.hpp | 39 +++++- 7 files changed, 152 insertions(+), 46 deletions(-) diff --git a/libuavcan/include/uavcan/error.hpp b/libuavcan/include/uavcan/error.hpp index 4f67e9a6ec..db9b3992e8 100644 --- a/libuavcan/include/uavcan/error.hpp +++ b/libuavcan/include/uavcan/error.hpp @@ -35,6 +35,7 @@ const int16_t ErrLogic = 10; const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type const int16_t ErrInvalidConfiguration = 13; +const int16_t ErrBufferTooSmall = 14; ///< Statically allocated buffer is not large enough /** * @} */ diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 235f5518df..5b958f0a97 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -40,7 +40,6 @@ class UAVCAN_EXPORT ReceivedDataStructure : public DataType_ { if (_transfer_ == NULL) { - UAVCAN_ASSERT(0); return Ret(); } return (_transfer_->*Fun)(); @@ -162,23 +161,71 @@ class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase { } }; - struct ReceivedDataStructureSpec : public ReceivedDataStructure - { - using ReceivedDataStructure::setTransfer; - }; - LazyConstructor forwarder_; - ReceivedDataStructureSpec message_; int checkInit(); - bool decodeTransfer(IncomingTransfer& transfer); - void handleIncomingTransfer(IncomingTransfer& transfer); int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase*)); + /* + * Received object and its allocators + */ + struct ReceivedDataStructureSpec : public ReceivedDataStructure + { + using ReceivedDataStructure::setTransfer; + }; + + class StructBufferStatic + { + ReceivedDataStructureSpec object_; + + public: + explicit StructBufferStatic(GenericSubscriber&) { } + + ReceivedDataStructureSpec* getObjectPtr() { return &object_; } + }; + + class StructBufferShared + { + ReceivedDataStructureSpec* object_; + + public: + explicit StructBufferShared(GenericSubscriber& owner) + : object_(NULL) + { + IMarshalBuffer* const buf = + owner.getNode().getMarshalBufferProvider().getBuffer(sizeof(ReceivedDataStructureSpec)); + if (buf != NULL && buf->getSize() >= sizeof(ReceivedDataStructureSpec)) + { + object_ = new (buf->getDataPtr()) ReceivedDataStructureSpec; + } + } + + /* + * Destructor MUST NOT be invoked + * TODO explain why + */ + + ReceivedDataStructureSpec* getObjectPtr() { return object_; } + }; + protected: + /** + * This value defines the threshold for stack allocation. See the typedef below. + */ + enum { MaxObjectSizeForStackAllocation = 80 }; + + /** + * This type resolves to either type of buffer, depending on the object size: + * - if the object is small, it will be allocated on the stack (StructBufferStatic); + * - if the object is large, it will be allocated in the shared buffer (StructBufferShared). + */ + typedef typename Select<(sizeof(DataStruct) > MaxObjectSizeForStackAllocation), + StructBufferShared, + StructBufferStatic>::Result ReceivedDataStructureBuffer; + explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) { } @@ -227,15 +274,6 @@ protected: } TransferListenerType* getTransferListener() { return forwarder_; } - - /** - * Returns a reference to the temporary storage for decoded received messages. - * Reference to this storage is used as a parameter for subscription callbacks. - * This storage is guaranteed to stay intact after the last message was decoded, i.e. - * the application can use it to access the last received message object. - */ - ReceivedDataStructure& getReceivedStructStorage() { return message_; } - const ReceivedDataStructure& getReceivedStructStorage() const { return message_; } }; // ---------------------------------------------------------------------------- @@ -250,48 +288,76 @@ int GenericSubscriber::checkInit() { return 0; } + + /* + * Making sure that the buffer is large enough + * This check MUST be performed BEFORE initialization, otherwise we may encounter some terrible runtime failures. + */ + if (ReceivedDataStructureBuffer(*this).getObjectPtr() == NULL) + { + return -ErrBufferTooSmall; + } + + /* + * Initializing the transfer forwarder + */ GlobalDataTypeRegistry::instance().freeze(); const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName()); - if (!descr) + if (descr == NULL) { UAVCAN_TRACE("GenericSubscriber", "Type [%s] is not registered", DataSpec::getDataTypeFullName()); return -ErrUnknownDataType; } + forwarder_.template construct (*this, *descr, node_.getAllocator()); + return 0; } template -bool GenericSubscriber::decodeTransfer(IncomingTransfer& transfer) +void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) { + /* + * Initializing the temporary RX structure + */ + ReceivedDataStructureBuffer rx_struct_buffer(*this); + + if (rx_struct_buffer.getObjectPtr() == NULL) + { + UAVCAN_ASSERT(0); // The init method should have caught this error. + failure_count_++; + node_.getDispatcher().getTransferPerfCounter().addError(); + return; + } + + rx_struct_buffer.getObjectPtr()->setTransfer(&transfer); + + /* + * Decoding into the temporary storage + */ BitStream bitstream(transfer); ScalarCodec codec(bitstream); - message_.setTransfer(&transfer); + const int decode_res = DataStruct::decode(*rx_struct_buffer.getObjectPtr(), codec); - const int decode_res = DataStruct::decode(message_, codec); // We don't need the data anymore, the memory can be reused from the callback: transfer.release(); + if (decode_res <= 0) { UAVCAN_TRACE("GenericSubscriber", "Unable to decode the message [%i] [%s]", decode_res, DataSpec::getDataTypeFullName()); failure_count_++; node_.getDispatcher().getTransferPerfCounter().addError(); - return false; + return; } - return true; -} -template -void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) -{ - if (decodeTransfer(transfer)) - { - handleReceivedDataStruct(message_); - } + /* + * Invoking the callback + */ + handleReceivedDataStruct(*rx_struct_buffer.getObjectPtr()); } template diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index 3dad463a83..e90a9b396f 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -17,9 +17,11 @@ namespace uavcan class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer { public: - virtual const uint8_t* getDataPtr() const = 0; + virtual uint8_t* getDataPtr() = 0; virtual unsigned getMaxWritePos() const = 0; virtual unsigned getSize() const = 0; + + const uint8_t* getDataPtr() const { return const_cast(this)->getDataPtr(); } }; /** @@ -62,7 +64,7 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider return buf_.write(offset, data, len); } - virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } + virtual uint8_t* getDataPtr() { return buf_.getRawPtr(); } virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index b8467c4694..e166c07420 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -61,7 +61,7 @@ struct ServiceCallID * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. */ template -class UAVCAN_EXPORT ServiceCallResult +class UAVCAN_EXPORT ServiceCallResult : Noncopyable { public: typedef ReceivedDataStructure ResponseFieldType; @@ -257,8 +257,16 @@ private: UAVCAN_TRACE("ServiceClient::TimeoutCallbackCaller", "Timeout from nid=%d, tid=%d, dtname=%s", int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()), DataType::getDataTypeFullName()); + + typename SubscriberType::ReceivedDataStructureBuffer rx_struct_buffer(owner); + if (rx_struct_buffer.getObjectPtr() == NULL) + { + handleFatalError("RX obj buf"); + } + ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), - owner.getReceivedStructStorage()); // Mutable! + *rx_struct_buffer.getObjectPtr()); // Mutable! + owner.invokeCallback(result); } } diff --git a/libuavcan/test/node/service_client.cpp b/libuavcan/test/node/service_client.cpp index 82d99a66ac..8306ea1c16 100644 --- a/libuavcan/test/node/service_client.cpp +++ b/libuavcan/test/node/service_client.cpp @@ -178,8 +178,7 @@ TEST(ServiceClient, Basic) ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Third has timed out :( // Validating - // We use expected response instead of empty response because the last valid will be reused on fauilure - ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, expected_response)); + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); // Stray request ASSERT_LT(0, client3.call(99, request)); // Will timeout! @@ -341,7 +340,7 @@ TEST(ServiceClient, ConcurrentCalls) ASSERT_EQ(1, client.getNumPendingCalls()); // One dropped ASSERT_EQ(1, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Still listening - ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, last_response)); + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 88, root_ns_a::StringService::Response())); /* * Validating the 500 ms timeout @@ -352,7 +351,7 @@ TEST(ServiceClient, ConcurrentCalls) ASSERT_EQ(0, client.getNumPendingCalls()); // All finished ASSERT_EQ(0, nodes.b.getDispatcher().getNumServiceResponseListeners()); // Not listening - ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, last_response)); + ASSERT_TRUE(handler.match(ResultType::ErrorTimeout, 99, root_ns_a::StringService::Response())); } diff --git a/libuavcan/test/protocol/data_type_info_provider.cpp b/libuavcan/test/protocol/data_type_info_provider.cpp index 1780d5f801..fc1bd40cc3 100644 --- a/libuavcan/test/protocol/data_type_info_provider.cpp +++ b/libuavcan/test/protocol/data_type_info_provider.cpp @@ -19,7 +19,8 @@ using uavcan::DefaultDataTypeRegistrator; using uavcan::MonotonicDuration; template -static bool validateDataTypeInfoResponse(const std::auto_ptr >& resp, unsigned mask) +static bool validateDataTypeInfoResponse(const std::auto_ptr::Result>& resp, + unsigned mask) { if (!resp.get()) { diff --git a/libuavcan/test/protocol/helpers.hpp b/libuavcan/test/protocol/helpers.hpp index e0fb164f2f..3c2bc6c7ec 100644 --- a/libuavcan/test/protocol/helpers.hpp +++ b/libuavcan/test/protocol/helpers.hpp @@ -51,18 +51,47 @@ struct SubscriberWithCollector template class ServiceCallResultCollector : uavcan::Noncopyable { - typedef uavcan::ServiceCallResult ResultType; + typedef uavcan::ServiceCallResult ServiceCallResult; - void handler(const ResultType& result) +public: + class Result { - this->result.reset(new ResultType(result)); + const typename ServiceCallResult::Status status_; + uavcan::ServiceCallID call_id_; + typename DataType::Response response_; + + public: + Result(typename ServiceCallResult::Status arg_status, + uavcan::ServiceCallID arg_call_id, + const typename DataType::Response& arg_response) + : status_(arg_status) + , call_id_(arg_call_id) + , response_(arg_response) + { } + + bool isSuccessful() const { return status_ == ServiceCallResult::Success; } + + typename ServiceCallResult::Status getStatus() const { return status_; } + + uavcan::ServiceCallID getCallID() const { return call_id_; } + + const typename DataType::Response& getResponse() const { return response_; } + typename DataType::Response& getResponse() { return response_; } + }; + +private: + void handler(const uavcan::ServiceCallResult& tmp_result) + { + std::cout << tmp_result << std::endl; + result.reset(new Result(tmp_result.getStatus(), tmp_result.getCallID(), tmp_result.getResponse())); } public: - std::auto_ptr result; + std::auto_ptr result; typedef uavcan::MethodBinder Binder; + void (ServiceCallResultCollector::*)(const uavcan::ServiceCallResult&)> + Binder; Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); } }; From af09237dd22a6ab9b3feebc69fd7e1004db5db5b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 22:04:19 +0300 Subject: [PATCH 3/8] Stack-allocating RX objects --- libuavcan/include/uavcan/error.hpp | 1 - .../uavcan/node/generic_subscriber.hpp | 100 +++--------------- .../include/uavcan/node/marshal_buffer.hpp | 9 +- libuavcan/include/uavcan/node/node.hpp | 19 ++-- .../include/uavcan/node/service_client.hpp | 9 +- 5 files changed, 28 insertions(+), 110 deletions(-) diff --git a/libuavcan/include/uavcan/error.hpp b/libuavcan/include/uavcan/error.hpp index db9b3992e8..4f67e9a6ec 100644 --- a/libuavcan/include/uavcan/error.hpp +++ b/libuavcan/include/uavcan/error.hpp @@ -35,7 +35,6 @@ const int16_t ErrLogic = 10; const int16_t ErrPassiveMode = 11; ///< Operation not permitted in passive mode const int16_t ErrTransferTooLong = 12; ///< Transfer of this length cannot be sent with given transfer type const int16_t ErrInvalidConfiguration = 13; -const int16_t ErrBufferTooSmall = 14; ///< Statically allocated buffer is not large enough /** * @} */ diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index 5b958f0a97..b749f8614e 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -33,7 +33,7 @@ namespace uavcan template class UAVCAN_EXPORT ReceivedDataStructure : public DataType_ { - const IncomingTransfer* _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields + const IncomingTransfer* const _transfer_; ///< Such weird name is necessary to avoid clashing with DataType fields template Ret safeget() const @@ -46,12 +46,14 @@ class UAVCAN_EXPORT ReceivedDataStructure : public DataType_ } protected: - ReceivedDataStructure() : _transfer_(NULL) { } + ReceivedDataStructure() + : _transfer_(NULL) + { } - void setTransfer(const IncomingTransfer* arg_transfer) + ReceivedDataStructure(const IncomingTransfer* arg_transfer) + : _transfer_(arg_transfer) { UAVCAN_ASSERT(arg_transfer != NULL); - _transfer_ = arg_transfer; } public: @@ -169,63 +171,16 @@ class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase int genericStart(bool (Dispatcher::*registration_method)(TransferListenerBase*)); - /* - * Received object and its allocators - */ +protected: struct ReceivedDataStructureSpec : public ReceivedDataStructure { - using ReceivedDataStructure::setTransfer; + ReceivedDataStructureSpec() { } + + ReceivedDataStructureSpec(const IncomingTransfer* arg_transfer) + : ReceivedDataStructure(arg_transfer) + { } }; - class StructBufferStatic - { - ReceivedDataStructureSpec object_; - - public: - explicit StructBufferStatic(GenericSubscriber&) { } - - ReceivedDataStructureSpec* getObjectPtr() { return &object_; } - }; - - class StructBufferShared - { - ReceivedDataStructureSpec* object_; - - public: - explicit StructBufferShared(GenericSubscriber& owner) - : object_(NULL) - { - IMarshalBuffer* const buf = - owner.getNode().getMarshalBufferProvider().getBuffer(sizeof(ReceivedDataStructureSpec)); - if (buf != NULL && buf->getSize() >= sizeof(ReceivedDataStructureSpec)) - { - object_ = new (buf->getDataPtr()) ReceivedDataStructureSpec; - } - } - - /* - * Destructor MUST NOT be invoked - * TODO explain why - */ - - ReceivedDataStructureSpec* getObjectPtr() { return object_; } - }; - -protected: - /** - * This value defines the threshold for stack allocation. See the typedef below. - */ - enum { MaxObjectSizeForStackAllocation = 80 }; - - /** - * This type resolves to either type of buffer, depending on the object size: - * - if the object is small, it will be allocated on the stack (StructBufferStatic); - * - if the object is large, it will be allocated in the shared buffer (StructBufferShared). - */ - typedef typename Select<(sizeof(DataStruct) > MaxObjectSizeForStackAllocation), - StructBufferShared, - StructBufferStatic>::Result ReceivedDataStructureBuffer; - explicit GenericSubscriber(INode& node) : GenericSubscriberBase(node) { } @@ -289,18 +244,6 @@ int GenericSubscriber::checkInit() return 0; } - /* - * Making sure that the buffer is large enough - * This check MUST be performed BEFORE initialization, otherwise we may encounter some terrible runtime failures. - */ - if (ReceivedDataStructureBuffer(*this).getObjectPtr() == NULL) - { - return -ErrBufferTooSmall; - } - - /* - * Initializing the transfer forwarder - */ GlobalDataTypeRegistry::instance().freeze(); const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(DataTypeKind(DataSpec::DataTypeKind), DataSpec::getDataTypeFullName()); @@ -319,20 +262,7 @@ int GenericSubscriber::checkInit() template void GenericSubscriber::handleIncomingTransfer(IncomingTransfer& transfer) { - /* - * Initializing the temporary RX structure - */ - ReceivedDataStructureBuffer rx_struct_buffer(*this); - - if (rx_struct_buffer.getObjectPtr() == NULL) - { - UAVCAN_ASSERT(0); // The init method should have caught this error. - failure_count_++; - node_.getDispatcher().getTransferPerfCounter().addError(); - return; - } - - rx_struct_buffer.getObjectPtr()->setTransfer(&transfer); + ReceivedDataStructureSpec rx_struct(&transfer); /* * Decoding into the temporary storage @@ -340,7 +270,7 @@ void GenericSubscriber::handleIncomi BitStream bitstream(transfer); ScalarCodec codec(bitstream); - const int decode_res = DataStruct::decode(*rx_struct_buffer.getObjectPtr(), codec); + const int decode_res = DataStruct::decode(rx_struct, codec); // We don't need the data anymore, the memory can be reused from the callback: transfer.release(); @@ -357,7 +287,7 @@ void GenericSubscriber::handleIncomi /* * Invoking the callback */ - handleReceivedDataStruct(*rx_struct_buffer.getObjectPtr()); + handleReceivedDataStruct(rx_struct); } template diff --git a/libuavcan/include/uavcan/node/marshal_buffer.hpp b/libuavcan/include/uavcan/node/marshal_buffer.hpp index e90a9b396f..353233bc33 100644 --- a/libuavcan/include/uavcan/node/marshal_buffer.hpp +++ b/libuavcan/include/uavcan/node/marshal_buffer.hpp @@ -17,11 +17,8 @@ namespace uavcan class UAVCAN_EXPORT IMarshalBuffer : public ITransferBuffer { public: - virtual uint8_t* getDataPtr() = 0; + virtual const uint8_t* getDataPtr() const = 0; virtual unsigned getMaxWritePos() const = 0; - virtual unsigned getSize() const = 0; - - const uint8_t* getDataPtr() const { return const_cast(this)->getDataPtr(); } }; /** @@ -64,12 +61,10 @@ class UAVCAN_EXPORT MarshalBufferProvider : public IMarshalBufferProvider return buf_.write(offset, data, len); } - virtual uint8_t* getDataPtr() { return buf_.getRawPtr(); } + virtual const uint8_t* getDataPtr() const { return buf_.getRawPtr(); } virtual unsigned getMaxWritePos() const { return buf_.getMaxWritePos(); } - virtual unsigned getSize() const { return MaxSize_; } - public: void reset() { buf_.reset(); } }; diff --git a/libuavcan/include/uavcan/node/node.hpp b/libuavcan/include/uavcan/node/node.hpp index d12cec6558..9cc2773c5c 100644 --- a/libuavcan/include/uavcan/node/node.hpp +++ b/libuavcan/include/uavcan/node/node.hpp @@ -47,24 +47,21 @@ namespace uavcan * be allocated in the memory pool if needed. * Default value is acceptable for any use case. * - * @tparam MarshalBufferSize Size of the marshal buffer, that is used to provide short-term temporary storage for: - * 1. Serialized data for TX transfers; - * 2. De-serialized data for RX transfers. - * The buffer must be large enough to accommodate largest serialized TX transfer and - * largest de-serialized RX data structure. The former value is constant for UAVCAN, the - * latter is platform-dependent (depends on the field padding, memory alignment, pointer - * size, etc.). - * The default value should be enough for all use cases on virtually all platforms. - * If this value is not large enough, transport objects (such as Subscriber<>, - * Publisher<>, Service*<>) will be failing at run time during initialization. + * @tparam MarshalBufferSize Size of the marshal buffer that is used to provide short-term temporary storage for + * serialized data for TX transfers. The buffer must be large enough to accommodate + * largest serialized TX transfer. The default value is guaranteed to be large enough, + * but it can be reduced if long TX transfers are not used to optimize memory use. + * If UAVCAN_TINY mode is enabled, this value defaults to the maximum length of a + * response transfer of uavcan.protocol.GetNodeInfo. */ template ::Result #else unsigned OutgoingTransferRegistryStaticEntries = 10, + unsigned MarshalBufferSize = MaxPossibleTransferPayloadLen #endif - unsigned MarshalBufferSize = 512 > class UAVCAN_EXPORT Node : public INode { diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index e166c07420..35e38cd535 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -59,6 +59,7 @@ struct ServiceCallID /** * Object of this type will be returned to the application as a result of service call. * Note that application ALWAYS gets this result, even when it times out or fails because of some other reason. + * The class is made noncopyable because it keeps a reference to a stack-allocated object. */ template class UAVCAN_EXPORT ServiceCallResult : Noncopyable @@ -258,14 +259,10 @@ private: int(state.getCallID().server_node_id.get()), int(state.getCallID().transfer_id.get()), DataType::getDataTypeFullName()); - typename SubscriberType::ReceivedDataStructureBuffer rx_struct_buffer(owner); - if (rx_struct_buffer.getObjectPtr() == NULL) - { - handleFatalError("RX obj buf"); - } + typename SubscriberType::ReceivedDataStructureSpec rx_struct; // Default-initialized ServiceCallResultType result(ServiceCallResultType::ErrorTimeout, state.getCallID(), - *rx_struct_buffer.getObjectPtr()); // Mutable! + rx_struct); // Mutable! owner.invokeCallback(result); } From d8c096430f33e2a57448a3a6b026e01a588f38b9 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 22:23:41 +0300 Subject: [PATCH 4/8] Added some comments concerning stack allocation and references --- libuavcan/include/uavcan/node/generic_subscriber.hpp | 5 ++++- libuavcan/include/uavcan/node/service_client.hpp | 7 +++++++ libuavcan/include/uavcan/node/service_server.hpp | 3 +++ 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/libuavcan/include/uavcan/node/generic_subscriber.hpp b/libuavcan/include/uavcan/node/generic_subscriber.hpp index b749f8614e..bb203788c6 100644 --- a/libuavcan/include/uavcan/node/generic_subscriber.hpp +++ b/libuavcan/include/uavcan/node/generic_subscriber.hpp @@ -140,7 +140,10 @@ public: typedef TransferListenerTemplate Type; }; - +/** + * Please note that the reference passed to the RX callback points to a stack-allocated object, which means + * that it gets invalidated shortly after the callback returns. + */ template class UAVCAN_EXPORT GenericSubscriber : public GenericSubscriberBase { diff --git a/libuavcan/include/uavcan/node/service_client.hpp b/libuavcan/include/uavcan/node/service_client.hpp index 35e38cd535..97d0a55fb3 100644 --- a/libuavcan/include/uavcan/node/service_client.hpp +++ b/libuavcan/include/uavcan/node/service_client.hpp @@ -93,6 +93,9 @@ public: ServiceCallID getCallID() const { return call_id_; } + /** + * Returned reference points to a stack-allocated object. + */ const ResponseFieldType& getResponse() const { return response_; } ResponseFieldType& getResponse() { return response_; } }; @@ -201,6 +204,10 @@ public: * This class can manage multiple concurrent calls to the same or different remote servers. Number of concurrent * calls is limited only by amount of available pool memory. * + * Note that the reference passed to the callback points to a stack-allocated object, which means that the + * reference invalidates once the callback returns. If you want to use this object after the callback execution, + * you need to copy it somewhere. + * * @tparam DataType_ Service data type. * * @tparam Callback_ Service response will be delivered through the callback of this type. diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index 1602e6b70f..d7181fd0d0 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -64,6 +64,9 @@ public: /** * Use this class to implement UAVCAN service servers. * + * Note that the references passed to the callback may point to stack-allocated objects, which means that the + * references get invalidated once the callback returns. + * * @tparam DataType_ Service data type. * * @tparam Callback_ Service calls will be delivered through the callback of this type, and service From 3499db227b1c0fffce3068fdda481036915c2429 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 22:36:16 +0300 Subject: [PATCH 5/8] Stack-allocating the service response structure --- libuavcan/include/uavcan/node/service_server.hpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/libuavcan/include/uavcan/node/service_server.hpp b/libuavcan/include/uavcan/node/service_server.hpp index d7181fd0d0..17076b4c5a 100644 --- a/libuavcan/include/uavcan/node/service_server.hpp +++ b/libuavcan/include/uavcan/node/service_server.hpp @@ -120,26 +120,26 @@ private: PublisherType publisher_; Callback callback_; uint32_t response_failure_count_; - ServiceResponseDataStructure response_; virtual void handleReceivedDataStruct(ReceivedDataStructure& request) { UAVCAN_ASSERT(request.getTransferType() == TransferTypeServiceRequest); + + ServiceResponseDataStructure response; + if (try_implicit_cast(callback_, true)) { - // The application needs newly initialized structure - response_ = ServiceResponseDataStructure(); - UAVCAN_ASSERT(response_.isResponseEnabled()); // Enabled by default - callback_(request, response_); + UAVCAN_ASSERT(response.isResponseEnabled()); // Enabled by default + callback_(request, response); } else { handleFatalError("Srv serv clbk"); } - if (response_.isResponseEnabled()) + if (response.isResponseEnabled()) { - const int res = publisher_.publish(response_, TransferTypeServiceResponse, request.getSrcNodeID(), + const int res = publisher_.publish(response, TransferTypeServiceResponse, request.getSrcNodeID(), request.getTransferID()); if (res < 0) { From 0ce23a4f341fac3c35447e453827cb7348514cac Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 23:29:15 +0300 Subject: [PATCH 6/8] Lazy initialization of TransferSender removed --- .../include/uavcan/node/generic_publisher.hpp | 17 +++------ libuavcan/include/uavcan/node/publisher.hpp | 36 +++---------------- .../protocol/global_time_sync_master.hpp | 17 +++------ libuavcan/include/uavcan/protocol/logger.hpp | 3 +- .../uavcan/transport/transfer_sender.hpp | 26 ++++++++++---- libuavcan/src/node/uc_generic_publisher.cpp | 22 +++++------- .../protocol/uc_dynamic_node_id_client.cpp | 6 +--- .../src/protocol/uc_node_status_provider.cpp | 6 +--- .../src/transport/uc_transfer_sender.cpp | 17 ++++++--- libuavcan/test/node/publisher.cpp | 17 ++------- 10 files changed, 64 insertions(+), 103 deletions(-) diff --git a/libuavcan/include/uavcan/node/generic_publisher.hpp b/libuavcan/include/uavcan/node/generic_publisher.hpp index 7dde549c00..7991cc5044 100644 --- a/libuavcan/include/uavcan/node/generic_publisher.hpp +++ b/libuavcan/include/uavcan/node/generic_publisher.hpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -20,15 +19,14 @@ namespace uavcan class GenericPublisherBase : Noncopyable { - const MonotonicDuration max_transfer_interval_; // TODO: memory usage can be reduced + TransferSender sender_; MonotonicDuration tx_timeout_; INode& node_; - LazyConstructor sender_; protected: GenericPublisherBase(INode& node, MonotonicDuration tx_timeout, MonotonicDuration max_transfer_interval) - : max_transfer_interval_(max_transfer_interval) + : sender_(node.getDispatcher(), max_transfer_interval) , tx_timeout_(tx_timeout) , node_(node) { @@ -51,7 +49,8 @@ protected: int genericPublish(const IMarshalBuffer& buffer, TransferType transfer_type, NodeID dst_node_id, TransferID* tid, MonotonicTime blocking_deadline); - TransferSender* getTransferSender(); + TransferSender& getTransferSender() { return sender_; } + const TransferSender& getTransferSender() const { return sender_; } public: static MonotonicDuration getMinTxTimeout() { return MonotonicDuration::fromUSec(200); } @@ -66,7 +65,7 @@ public: */ void allowAnonymousTransfers() { - sender_->allowAnonymousTransfers(); + sender_.allowAnonymousTransfers(); } INode& getNode() const { return node_; } @@ -124,12 +123,6 @@ public: { return genericPublish(message, transfer_type, dst_node_id, &tid, blocking_deadline); } - - TransferSender* getTransferSender() - { - (void)checkInit(); - return GenericPublisherBase::getTransferSender(); - } }; // ---------------------------------------------------------------------------- diff --git a/libuavcan/include/uavcan/node/publisher.hpp b/libuavcan/include/uavcan/node/publisher.hpp index 4b95313d24..2b9222d1b5 100644 --- a/libuavcan/include/uavcan/node/publisher.hpp +++ b/libuavcan/include/uavcan/node/publisher.hpp @@ -81,46 +81,20 @@ public: /** * Returns priority of outgoing transfers. - * TODO: Make const. */ - TransferPriority getPriority() + TransferPriority getPriority() const { - // TODO probably TransferSender must be transformed into regular field? - TransferSender* const ts = getTransferSender(); - if (ts != NULL) - { - return ts->getPriority(); - } - else - { - return TransferPriorityNormal; // This is default - } + return BaseType::getTransferSender().getPriority(); } /** * Allows to change the priority of outgoing transfers. * Note that only High, Normal and Low priorities can be used; Service priority is not available for messages. - * Returns negative error code if priority cannot be set, non-negative on success. + * If the priority value is invalid, an assertion failure will be generated, and the value will not be updated. */ - int setPriority(const TransferPriority prio) + void setPriority(const TransferPriority prio) { - if (prio < NumTransferPriorities && prio != TransferPriorityService) - { - TransferSender* const ts = getTransferSender(); // TODO: Static TransferSender? - if (ts != NULL) - { - ts->setPriority(prio); - return 0; - } - else - { - return -ErrLogic; - } - } - else - { - return -ErrInvalidParam; - } + BaseType::getTransferSender().setPriority(prio); } static MonotonicDuration getDefaultTxTimeout() { return MonotonicDuration::fromMSec(10); } diff --git a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp index 4b69451a0a..4cab0c4abf 100644 --- a/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp +++ b/libuavcan/include/uavcan/protocol/global_time_sync_master.hpp @@ -51,16 +51,9 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase const int res = pub_.init(); if (res >= 0) { - TransferSender* const ts = pub_.getTransferSender(); - UAVCAN_ASSERT(ts != NULL); - ts->setIfaceMask(uint8_t(1 << iface_index_)); - ts->setCanIOFlags(CanIOFlagLoopback); - - const int prio_res = pub_.setPriority(TransferPriorityHigh); // Fixed priority - if (prio_res < 0) - { - return prio_res; - } + pub_.getTransferSender().setIfaceMask(uint8_t(1 << iface_index_)); + pub_.getTransferSender().setCanIOFlags(CanIOFlagLoopback); + pub_.setPriority(TransferPriorityHigh); // Fixed priority } return res; } @@ -84,8 +77,8 @@ class UAVCAN_EXPORT GlobalTimeSyncMaster : protected LoopbackFrameListenerBase int publish(TransferID tid, MonotonicTime current_time) { - UAVCAN_ASSERT(pub_.getTransferSender()->getCanIOFlags() == CanIOFlagLoopback); - UAVCAN_ASSERT(pub_.getTransferSender()->getIfaceMask() == (1 << iface_index_)); + UAVCAN_ASSERT(pub_.getTransferSender().getCanIOFlags() == CanIOFlagLoopback); + UAVCAN_ASSERT(pub_.getTransferSender().getIfaceMask() == (1 << iface_index_)); const MonotonicDuration since_prev_pub = current_time - iface_prev_pub_mono_; iface_prev_pub_mono_ = current_time; diff --git a/libuavcan/include/uavcan/protocol/logger.hpp b/libuavcan/include/uavcan/protocol/logger.hpp index 5856e07355..e6e3358152 100644 --- a/libuavcan/include/uavcan/protocol/logger.hpp +++ b/libuavcan/include/uavcan/protocol/logger.hpp @@ -101,7 +101,8 @@ public: { return res; } - return logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority + logmsg_pub_.setPriority(TransferPriorityLow); // Fixed priority + return 0; } /** diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 26d6e0e628..6c45e524bb 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -20,10 +20,10 @@ namespace uavcan class UAVCAN_EXPORT TransferSender { const MonotonicDuration max_transfer_interval_; - const DataTypeDescriptor& data_type_; + DataTypeID data_type_id_; TransferPriority priority_; - const CanTxQueue::Qos qos_; - const TransferCRC crc_base_; + CanTxQueue::Qos qos_; + TransferCRC crc_base_; CanIOFlags flags_; uint8_t iface_mask_; bool allow_anonymous_transfers_; @@ -43,16 +43,30 @@ public: TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) : max_transfer_interval_(max_transfer_interval) - , data_type_(data_type) , priority_(TransferPriorityNormal) - , qos_(qos) - , crc_base_(data_type.getSignature().toTransferCRC()) + , qos_(CanTxQueue::Qos()) + , flags_(CanIOFlags(0)) + , iface_mask_(AllIfacesMask) + , allow_anonymous_transfers_(false) + , dispatcher_(dispatcher) + { + init(data_type, qos); + } + + TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) + : max_transfer_interval_(max_transfer_interval) + , priority_(TransferPriorityNormal) + , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) , dispatcher_(dispatcher) { } + void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos); + + bool isInitialized() const { return data_type_id_ != DataTypeID(); } + CanIOFlags getCanIOFlags() const { return flags_; } void setCanIOFlags(CanIOFlags flags) { flags_ = flags; } diff --git a/libuavcan/src/node/uc_generic_publisher.cpp b/libuavcan/src/node/uc_generic_publisher.cpp index 09f361f5b0..73ab834d96 100644 --- a/libuavcan/src/node/uc_generic_publisher.cpp +++ b/libuavcan/src/node/uc_generic_publisher.cpp @@ -9,7 +9,7 @@ namespace uavcan bool GenericPublisherBase::isInited() const { - return bool(sender_); + return sender_.isInitialized(); } int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQueue::Qos qos) @@ -22,13 +22,14 @@ int GenericPublisherBase::doInit(DataTypeKind dtkind, const char* dtname, CanTxQ GlobalDataTypeRegistry::instance().freeze(); const DataTypeDescriptor* const descr = GlobalDataTypeRegistry::instance().find(dtkind, dtname); - if (!descr) + if (descr == NULL) { UAVCAN_TRACE("GenericPublisher", "Type [%s] is not registered", dtname); return -ErrUnknownDataType; } - sender_.construct - (node_.getDispatcher(), *descr, qos, max_transfer_interval_); + + sender_.init(*descr, qos); + return 0; } @@ -47,21 +48,16 @@ int GenericPublisherBase::genericPublish(const IMarshalBuffer& buffer, TransferT { if (tid) { - return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id, *tid); + return sender_.send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id, *tid); } else { - return sender_->send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), - blocking_deadline, transfer_type, dst_node_id); + return sender_.send(buffer.getDataPtr(), buffer.getMaxWritePos(), getTxDeadline(), + blocking_deadline, transfer_type, dst_node_id); } } -TransferSender* GenericPublisherBase::getTransferSender() -{ - return sender_.isConstructed() ? static_cast(sender_) : NULL; -} - void GenericPublisherBase::setTxTimeout(MonotonicDuration tx_timeout) { tx_timeout = max(tx_timeout, getMinTxTimeout()); diff --git a/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp index f6150ed3ac..173314ed81 100644 --- a/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp +++ b/libuavcan/src/protocol/uc_dynamic_node_id_client.cpp @@ -160,11 +160,7 @@ int DynamicNodeIDClient::start(const protocol::HardwareVersion& hardware_version return res; } dnida_pub_.allowAnonymousTransfers(); - res = dnida_pub_.setPriority(transfer_priority); - if (res < 0) - { - return res; - } + dnida_pub_.setPriority(transfer_priority); res = dnida_sub_.start(AllocationCallback(this, &DynamicNodeIDClient::handleAllocation)); if (res < 0) diff --git a/libuavcan/src/protocol/uc_node_status_provider.cpp b/libuavcan/src/protocol/uc_node_status_provider.cpp index bd090adb10..736050628f 100644 --- a/libuavcan/src/protocol/uc_node_status_provider.cpp +++ b/libuavcan/src/protocol/uc_node_status_provider.cpp @@ -71,11 +71,7 @@ int NodeStatusProvider::startAndPublish(TransferPriority priority) int res = -1; - res = node_status_pub_.setPriority(priority); - if (res < 0) - { - goto fail; - } + node_status_pub_.setPriority(priority); if (!getNode().isPassiveMode()) { diff --git a/libuavcan/src/transport/uc_transfer_sender.cpp b/libuavcan/src/transport/uc_transfer_sender.cpp index 6f49f7a635..232ce82235 100644 --- a/libuavcan/src/transport/uc_transfer_sender.cpp +++ b/libuavcan/src/transport/uc_transfer_sender.cpp @@ -15,6 +15,15 @@ void TransferSender::registerError() const dispatcher_.getTransferPerfCounter().addError(); } +void TransferSender::init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos) +{ + UAVCAN_ASSERT(!isInitialized()); + + qos_ = qos; + data_type_id_ = dtid.getID(); + crc_base_ = dtid.getSignature().toTransferCRC(); +} + int TransferSender::send(const uint8_t* payload, unsigned payload_len, MonotonicTime tx_deadline, MonotonicTime blocking_deadline, TransferType transfer_type, NodeID dst_node_id, TransferID tid) const @@ -24,7 +33,7 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic return -ErrTransferTooLong; } - Frame frame(data_type_.getID(), transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); + Frame frame(data_type_id_, transfer_type, dispatcher_.getNodeID(), dst_node_id, 0, tid); if (transfer_type == TransferTypeMessageBroadcast || transfer_type == TransferTypeMessageUnicast) { @@ -143,7 +152,7 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic /* * TODO: TID is not needed for anonymous transfers, this part of the code can be skipped? */ - const OutgoingTransferRegistryKey otr_key(data_type_.getID(), transfer_type, dst_node_id); + const OutgoingTransferRegistryKey otr_key(data_type_id_, transfer_type, dst_node_id); UAVCAN_ASSERT(!tx_deadline.isZero()); const MonotonicTime otr_deadline = tx_deadline + max_transfer_interval_; @@ -151,8 +160,8 @@ int TransferSender::send(const uint8_t* payload, unsigned payload_len, Monotonic TransferID* const tid = dispatcher_.getOutgoingTransferRegistry().accessOrCreate(otr_key, otr_deadline); if (tid == NULL) { - UAVCAN_TRACE("TransferSender", "OTR access failure, dtd=%s tt=%i", - data_type_.toString().c_str(), int(transfer_type)); + UAVCAN_TRACE("TransferSender", "OTR access failure, dtid=%d tt=%i", + int(data_type_id_.get()), int(transfer_type)); return -ErrMemory; } diff --git a/libuavcan/test/node/publisher.cpp b/libuavcan/test/node/publisher.cpp index 61a57ea1c2..231a25b934 100644 --- a/libuavcan/test/node/publisher.cpp +++ b/libuavcan/test/node/publisher.cpp @@ -18,6 +18,8 @@ TEST(Publisher, Basic) uavcan::Publisher publisher(node); + ASSERT_FALSE(publisher.getTransferSender().isInitialized()); + std::cout << "sizeof(uavcan::Publisher): " << sizeof(uavcan::Publisher) << std::endl; @@ -110,18 +112,5 @@ TEST(Publisher, Basic) * Misc */ ASSERT_TRUE(uavcan::GlobalDataTypeRegistry::instance().isFrozen()); - ASSERT_TRUE(publisher.getTransferSender()); -} - - -TEST(Publisher, ImplicitInitialization) -{ - SystemClockMock clock_mock(100); - CanDriverMock can_driver(2, clock_mock); - TestNode node(can_driver, clock_mock, 1); - - uavcan::Publisher publisher(node); - - // Will be initialized ad-hoc - ASSERT_TRUE(publisher.getTransferSender()); + ASSERT_TRUE(publisher.getTransferSender().isInitialized()); } From 941981066c0d4acf9ec93f405cc2f029d4b9c073 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 29 May 2015 23:56:41 +0300 Subject: [PATCH 7/8] CRC64 removed, file messages refactored --- dsdl/uavcan/protocol/file/215.GetInfo.uavcan | 3 +-- dsdl/uavcan/protocol/file/218.Read.uavcan | 2 +- dsdl/uavcan/protocol/file/219.Write.uavcan | 4 ++-- libuavcan/include/uavcan/protocol/file_server.hpp | 13 ++++--------- libuavcan/test/protocol/file_server.cpp | 10 ++-------- 5 files changed, 10 insertions(+), 22 deletions(-) diff --git a/dsdl/uavcan/protocol/file/215.GetInfo.uavcan b/dsdl/uavcan/protocol/file/215.GetInfo.uavcan index d26d085ac0..f018a2a835 100644 --- a/dsdl/uavcan/protocol/file/215.GetInfo.uavcan +++ b/dsdl/uavcan/protocol/file/215.GetInfo.uavcan @@ -11,7 +11,6 @@ Path path --- -uint64 crc64 # Undefined for directories -uint32 size # Undefined for directories +uint40 size # Undefined for directories Error error EntryType entry_type diff --git a/dsdl/uavcan/protocol/file/218.Read.uavcan b/dsdl/uavcan/protocol/file/218.Read.uavcan index b120a86301..ebc1c6d4a1 100644 --- a/dsdl/uavcan/protocol/file/218.Read.uavcan +++ b/dsdl/uavcan/protocol/file/218.Read.uavcan @@ -12,7 +12,7 @@ # will be returned, and data array will be empty. # -uint32 offset +uint40 offset Path path --- diff --git a/dsdl/uavcan/protocol/file/219.Write.uavcan b/dsdl/uavcan/protocol/file/219.Write.uavcan index 1cbec0666d..a661803863 100644 --- a/dsdl/uavcan/protocol/file/219.Write.uavcan +++ b/dsdl/uavcan/protocol/file/219.Write.uavcan @@ -15,9 +15,9 @@ # example by means of creating a staging area for uncompleted writes (like FTP servers do). # -uint32 offset +uint40 offset Path path -uint8[<=200] data +uint8[<=192] data --- diff --git a/libuavcan/include/uavcan/protocol/file_server.hpp b/libuavcan/include/uavcan/protocol/file_server.hpp index 1e5fed04a2..f1ca877cd8 100644 --- a/libuavcan/include/uavcan/protocol/file_server.hpp +++ b/libuavcan/include/uavcan/protocol/file_server.hpp @@ -30,11 +30,6 @@ public: typedef protocol::file::EntryType EntryType; typedef protocol::file::Error Error; - /** - * Use this class to compute CRC64 for uavcan.protocol.file.GetInfo. - */ - typedef DataTypeSignatureCRC FileCRC; - /** * All read operations must return this number of bytes, unless end of file is reached. */ @@ -51,7 +46,7 @@ public: * Implementation of this method is required. * On success the method must return zero. */ - virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) = 0; + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) = 0; /** * Backend for uavcan.protocol.file.Read. @@ -60,7 +55,7 @@ public: * if the end of file is reached. * On success the method must return zero. */ - virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0; + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) = 0; // Methods below are optional. @@ -69,7 +64,7 @@ public: * Implementation of this method is NOT required; by default it returns uavcan.protocol.file.Error.NOT_IMPLEMENTED. * On success the method must return zero. */ - virtual int16_t write(const Path& path, const uint32_t offset, const uint8_t* buffer, const uint16_t size) + virtual int16_t write(const Path& path, const uint64_t offset, const uint8_t* buffer, const uint16_t size) { (void)path; (void)offset; @@ -129,7 +124,7 @@ class BasicFileServer void handleGetInfo(const protocol::file::GetInfo::Request& req, protocol::file::GetInfo::Response& resp) { - resp.error.value = backend_.getInfo(req.path.path, resp.crc64, resp.size, resp.entry_type); + resp.error.value = backend_.getInfo(req.path.path, resp.size, resp.entry_type); } void handleRead(const protocol::file::Read::Request& req, protocol::file::Read::Response& resp) diff --git a/libuavcan/test/protocol/file_server.cpp b/libuavcan/test/protocol/file_server.cpp index f3591e79e3..be970b4d04 100644 --- a/libuavcan/test/protocol/file_server.cpp +++ b/libuavcan/test/protocol/file_server.cpp @@ -13,15 +13,10 @@ public: static const std::string file_name; static const std::string file_data; - virtual int16_t getInfo(const Path& path, uint64_t& out_crc64, uint32_t& out_size, EntryType& out_type) + virtual int16_t getInfo(const Path& path, uint64_t& out_size, EntryType& out_type) { if (path == file_name) { - { - FileCRC crc; - crc.add(reinterpret_cast(file_data.c_str()), unsigned(file_data.length())); - out_crc64 = crc.get(); - } out_size = uint16_t(file_data.length()); out_type.flags |= EntryType::FLAG_FILE; out_type.flags |= EntryType::FLAG_READABLE; @@ -33,7 +28,7 @@ public: } } - virtual int16_t read(const Path& path, const uint32_t offset, uint8_t* out_buffer, uint16_t& inout_size) + virtual int16_t read(const Path& path, const uint64_t offset, uint8_t* out_buffer, uint16_t& inout_size) { if (path == file_name) { @@ -90,7 +85,6 @@ TEST(BasicFileServer, Basic) ASSERT_TRUE(get_info.collector.result.get()); ASSERT_TRUE(get_info.collector.result->isSuccessful()); - ASSERT_EQ(0x62EC59E3F1A4F00A, get_info.collector.result->getResponse().crc64); ASSERT_EQ(0, get_info.collector.result->getResponse().error.value); ASSERT_EQ(9, get_info.collector.result->getResponse().size); ASSERT_EQ(EntryType::FLAG_FILE | EntryType::FLAG_READABLE, From b73dbd3f41f714e96bf3168ff81a1f688b872a24 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Sat, 30 May 2015 01:34:05 +0300 Subject: [PATCH 8/8] Padding optimization in TransferSender --- .../include/uavcan/transport/transfer_sender.hpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/libuavcan/include/uavcan/transport/transfer_sender.hpp b/libuavcan/include/uavcan/transport/transfer_sender.hpp index 6c45e524bb..2b95c20b42 100644 --- a/libuavcan/include/uavcan/transport/transfer_sender.hpp +++ b/libuavcan/include/uavcan/transport/transfer_sender.hpp @@ -20,16 +20,17 @@ namespace uavcan class UAVCAN_EXPORT TransferSender { const MonotonicDuration max_transfer_interval_; - DataTypeID data_type_id_; + + Dispatcher& dispatcher_; + TransferPriority priority_; CanTxQueue::Qos qos_; TransferCRC crc_base_; + DataTypeID data_type_id_; CanIOFlags flags_; uint8_t iface_mask_; bool allow_anonymous_transfers_; - Dispatcher& dispatcher_; - void registerError() const; public: @@ -43,24 +44,24 @@ public: TransferSender(Dispatcher& dispatcher, const DataTypeDescriptor& data_type, CanTxQueue::Qos qos, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) , priority_(TransferPriorityNormal) , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) - , dispatcher_(dispatcher) { init(data_type, qos); } TransferSender(Dispatcher& dispatcher, MonotonicDuration max_transfer_interval = getDefaultMaxTransferInterval()) : max_transfer_interval_(max_transfer_interval) + , dispatcher_(dispatcher) , priority_(TransferPriorityNormal) , qos_(CanTxQueue::Qos()) , flags_(CanIOFlags(0)) , iface_mask_(AllIfacesMask) , allow_anonymous_transfers_(false) - , dispatcher_(dispatcher) { } void init(const DataTypeDescriptor& dtid, CanTxQueue::Qos qos);