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