Using global RX object buffer

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
 /**
  * @}
  */

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<DataStruct>
-    {
-        using ReceivedDataStructure<DataStruct>::setTransfer;
-    };
-
     LazyConstructor<TransferForwarder> 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<DataStruct>
+    {
+        using ReceivedDataStructure<DataStruct>::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<DataStruct>& getReceivedStructStorage()             { return message_; }
-    const ReceivedDataStructure<DataStruct>& getReceivedStructStorage() const { return message_; }
 };
 
 // ----------------------------------------------------------------------------
@@ -250,48 +288,76 @@ int GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::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<SelfType&, const DataTypeDescriptor&, IPoolAllocator&>
         (*this, *descr, node_.getAllocator());
+
     return 0;
 }
 
 template <typename DataSpec, typename DataStruct, typename TransferListenerType>
-bool GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::decodeTransfer(IncomingTransfer& transfer)
+void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::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 <typename DataSpec, typename DataStruct, typename TransferListenerType>
-void GenericSubscriber<DataSpec, DataStruct, TransferListenerType>::handleIncomingTransfer(IncomingTransfer& transfer)
-{
-    if (decodeTransfer(transfer))
-    {
-        handleReceivedDataStruct(message_);
-    }
+    /*
+     * Invoking the callback
+     */
+    handleReceivedDataStruct(*rx_struct_buffer.getObjectPtr());
 }
 
 template <typename DataSpec, typename DataStruct, typename TransferListenerType>

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<IMarshalBuffer*>(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(); }
 

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 <typename DataType>
-class UAVCAN_EXPORT ServiceCallResult
+class UAVCAN_EXPORT ServiceCallResult : Noncopyable
 {
 public:
     typedef ReceivedDataStructure<typename DataType::Response> 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);
             }
         }

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

libuavcan/test/protocol/data_type_info_provider.cpp

@@ -19,7 +19,8 @@ using uavcan::DefaultDataTypeRegistrator;
 using uavcan::MonotonicDuration;
 
 template <typename DataType>
-static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResult<GetDataTypeInfo> >& resp, unsigned mask)
+static bool validateDataTypeInfoResponse(const std::auto_ptr<ServiceCallResultCollector<GetDataTypeInfo>::Result>& resp,
+                                         unsigned mask)
 {
     if (!resp.get())
     {

libuavcan/test/protocol/helpers.hpp

@@ -51,18 +51,47 @@ struct SubscriberWithCollector
 template <typename DataType>
 class ServiceCallResultCollector : uavcan::Noncopyable
 {
-    typedef uavcan::ServiceCallResult<DataType> ResultType;
+    typedef uavcan::ServiceCallResult<DataType> 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<DataType>& 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<ResultType> result;
+    std::auto_ptr<Result> result;
 
     typedef uavcan::MethodBinder<ServiceCallResultCollector*,
-                                 void (ServiceCallResultCollector::*)(const ResultType&)> Binder;
+                                 void (ServiceCallResultCollector::*)(const uavcan::ServiceCallResult<DataType>&)>
+            Binder;
 
     Binder bind() { return Binder(this, &ServiceCallResultCollector::handler); }
 };