diff --git a/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp b/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp new file mode 100644 index 0000000000..2892998a00 --- /dev/null +++ b/libuavcan/include/uavcan/internal/transport/transfer_listener.hpp @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ +/** + * Container for received transfer. + */ +class IncomingTransfer +{ + uint64_t ts_monotonic_; + uint64_t ts_utc_; + TransferType transfer_type_; + TransferID transfer_id_; + uint8_t source_node_id_; + +protected: + IncomingTransfer(uint64_t ts_monotonic, uint64_t ts_utc, TransferType transfer_type, + TransferID transfer_id, uint8_t source_node_id) + : ts_monotonic_(ts_monotonic) + , ts_utc_(ts_utc) + , transfer_type_(transfer_type) + , transfer_id_(transfer_id) + , source_node_id_(source_node_id) + { } + +public: + virtual ~IncomingTransfer() { } + + /** + * Read pure payload, no service fields are included (e.g. Target Node ID, Transfer CRC) + */ + virtual int read(unsigned int offset, uint8_t* data, unsigned int len) const = 0; + + /** + * Dispose the payload buffer. Further calls to read() will not be possible. + */ + virtual void release() { } + + uint64_t getMonotonicTimestamp() const { return ts_monotonic_; } + uint64_t getUtcTimestamp() const { return ts_utc_; } + TransferType getTransferType() const { return transfer_type_; } + TransferID getTransferID() const { return transfer_id_; } + uint8_t getSourceNodeID() const { return source_node_id_; } +}; + +/** + * Internal. + */ +class SingleFrameIncomingTransfer : public IncomingTransfer +{ + uint8_t payload_[Frame::PAYLOAD_LEN_MAX]; + const uint8_t payload_len_; +public: + SingleFrameIncomingTransfer(const RxFrame& frm); + int read(unsigned int offset, uint8_t* data, unsigned int len) const; +}; + +/** + * Internal. + */ +class MultiFrameIncomingTransfer : public IncomingTransfer, Noncopyable +{ + ITransferBufferManager* const bufmgr_; + const TransferBufferManagerKey bufmgr_key_; + const uint8_t buffer_offset_; ///< Number of bytes to skip from the beginning of the buffer space +public: + MultiFrameIncomingTransfer(const RxFrame& last_frame, uint8_t buffer_offset, ITransferBufferManager* bufmgr, + const TransferBufferManagerKey& bufmgr_key); + int read(unsigned int offset, uint8_t* data, unsigned int len) const; + void release() { bufmgr_->remove(bufmgr_key_); } +}; + +/** + * Transport internal, for dispatcher. + */ +class TransferListenerBase : public LinkedListNode +{ + const Crc16 crc_base_; ///< Pre-initialized with data type hash, thus constant + +protected: + TransferListenerBase(const DataTypeDescriptor* data_type) + : crc_base_(data_type->hash.value, DataTypeHash::NUM_BYTES) + { + assert(data_type); + } + + virtual ~TransferListenerBase() { } + + void handleReception(TransferReceiver& receiver, const RxFrame& frame); + + virtual void handleIncomingTransfer(IncomingTransfer& transfer) = 0; + +public: + virtual void handleFrame(const RxFrame& frame) = 0; + virtual void cleanup(uint64_t ts_monotonic) = 0; +}; + +/** + * This class should be derived by transfer receivers (subscribers, servers, callers). + */ +template +class TransferListener : protected TransferListenerBase, Noncopyable +{ + TransferBufferManager bufmgr_; + Map receivers_; + + void handleFrame(const RxFrame& frame) + { + // TODO + } + + struct TimedOutReceiverPredicate + { + const uint64_t ts_monotonic; + TimedOutReceiverPredicate(uint64_t ts_monotonic) : ts_monotonic(ts_monotonic) { } + bool operator()(const TransferBufferManagerKey& key, const TransferReceiver& value) const + { + (void)key; + return value.isTimedOut(ts_monotonic); + } + }; + + void cleanup(uint64_t ts_monotonic) + { + receivers_.removeWhere(TimedOutReceiverPredicate(ts_monotonic)); + } + +public: + TransferListener(const DataTypeDescriptor* data_type) + : TransferListenerBase(data_type) + { } + + ~TransferListener() + { + // Map must be cleared before bufmgr is destructed + receivers_.removeAll(); + } +}; + +} diff --git a/libuavcan/src/transport/transfer_listener.cpp b/libuavcan/src/transport/transfer_listener.cpp new file mode 100644 index 0000000000..d14837ce1a --- /dev/null +++ b/libuavcan/src/transport/transfer_listener.cpp @@ -0,0 +1,77 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include + +namespace uavcan +{ + +/* + * SingleFrameIncomingTransfer + */ +SingleFrameIncomingTransfer::SingleFrameIncomingTransfer(const RxFrame& frm) +: IncomingTransfer(frm.ts_monotonic, frm.ts_utc, frm.transfer_type, frm.transfer_id, frm.source_node_id) +, payload_len_(frm.payload_len) +{ + assert(frm.payload_len <= Frame::PAYLOAD_LEN_MAX); + assert(frm.frame_index == 0); + assert(frm.last_frame); + std::copy(frm.payload, frm.payload + frm.payload_len, payload_); +} + +int SingleFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const +{ + if (data == NULL) + { + assert(0); + return -1; + } + if (offset >= payload_len_) + return 0; + if ((offset + len) > payload_len_) + len = payload_len_ - offset; + assert((offset + len) <= payload_len_); + std::copy(payload_ + offset, payload_ + offset + len, data); + return len; +} + +/* + * MultiFrameIncomingTransfer + */ +MultiFrameIncomingTransfer::MultiFrameIncomingTransfer(const RxFrame& last_frame, uint8_t buffer_offset, + ITransferBufferManager* bufmgr, + const TransferBufferManagerKey& bufmgr_key) +: IncomingTransfer(last_frame.ts_monotonic, last_frame.ts_utc, last_frame.transfer_type, + last_frame.transfer_id, last_frame.source_node_id) +, bufmgr_(bufmgr) +, bufmgr_key_(bufmgr_key) +, buffer_offset_(buffer_offset) +{ + assert(bufmgr); + assert(!bufmgr_key.isEmpty()); + assert(last_frame.last_frame); +} + +int MultiFrameIncomingTransfer::read(unsigned int offset, uint8_t* data, unsigned int len) const +{ + const TransferBufferBase* const tbb = const_cast(bufmgr_)->access(bufmgr_key_); + if (tbb == NULL) + { + UAVCAN_TRACE("MultiFrameIncomingTransfer", "Read failed: no such buffer %s", + bufmgr_key_.toString().c_str()); + return -1; + } + return tbb->read(offset + buffer_offset_, data, len); +} + +/* + * TransferListenerBase + */ +void TransferListenerBase::handleReception(TransferReceiver& receiver, const RxFrame& frame) +{ + (void)receiver; + (void)frame; +} + +} diff --git a/libuavcan/test/transport/transfer_listener.cpp b/libuavcan/test/transport/transfer_listener.cpp new file mode 100644 index 0000000000..ba2148c9c8 --- /dev/null +++ b/libuavcan/test/transport/transfer_listener.cpp @@ -0,0 +1,110 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + + +static uavcan::RxFrame makeFrame() +{ + uavcan::RxFrame frame; + frame.data_type_id = 123; + frame.last_frame = true; + frame.source_node_id = 42; + frame.transfer_id.increment(); + frame.ts_monotonic = 123; + frame.ts_utc = 456; + frame.payload_len = uavcan::RxFrame::PAYLOAD_LEN_MAX; + for (int i = 0; i < uavcan::RxFrame::PAYLOAD_LEN_MAX; i++) + frame.payload[i] = i; + return frame; +} + + +static bool match(const uavcan::IncomingTransfer& it, const uavcan::RxFrame& frame, + const uint8_t* payload, unsigned int payload_len) +{ + // Fields extracted from the frame struct + EXPECT_EQ(it.getMonotonicTimestamp(), frame.ts_monotonic); + EXPECT_EQ(it.getUtcTimestamp(), frame.ts_utc); + EXPECT_EQ(it.getSourceNodeID(), frame.source_node_id); + EXPECT_EQ(it.getTransferID(), frame.transfer_id); + EXPECT_EQ(it.getTransferType(), frame.transfer_type); + + // Payload comparison + static const unsigned int BUFLEN = 1024; + uint8_t buf_reference[BUFLEN], buf_actual[BUFLEN]; + + if (payload_len > BUFLEN) + { + std::cout << "match(): Payload is too long" << std::endl; + exit(1); + } + + std::fill(buf_reference, buf_reference + BUFLEN, 0); + std::fill(buf_actual, buf_actual + BUFLEN, 0); + std::copy(payload, payload + payload_len, buf_reference); + + EXPECT_EQ(payload_len, it.read(0, buf_actual, payload_len * 3)); + EXPECT_EQ(0, it.read(payload_len, buf_actual, payload_len * 3)); + + return std::equal(buf_reference, buf_reference + BUFLEN, buf_actual); +} + + +TEST(SingleFrameIncomingTransfer, Basic) +{ + using uavcan::RxFrame; + using uavcan::SingleFrameIncomingTransfer; + + const RxFrame frame = makeFrame(); + SingleFrameIncomingTransfer it(frame); + + ASSERT_TRUE(match(it, frame, frame.payload, frame.payload_len)); +} + + +TEST(MultiFrameIncomingTransfer, Basic) +{ + using uavcan::RxFrame; + using uavcan::MultiFrameIncomingTransfer; + + uavcan::PoolManager<1> poolmgr; // We don't need dynamic memory + uavcan::TransferBufferManager<256, 1> bufmgr(&poolmgr); + + const RxFrame frame = makeFrame(); + uavcan::TransferBufferManagerKey bufmgr_key(frame.source_node_id, frame.transfer_type); + + MultiFrameIncomingTransfer it(frame, 3, &bufmgr, bufmgr_key); + + /* + * Empty read must fail + */ + uint8_t data_byte = 0; + ASSERT_GT(0, it.read(0, &data_byte, 1)); // Error - no such buffer + + /* + * Filling the test data + */ + const std::string data = "123Hello world"; + const uint8_t* const data_ptr = reinterpret_cast(data.c_str()); + ASSERT_FALSE(bufmgr.access(bufmgr_key)); + ASSERT_TRUE(bufmgr.create(bufmgr_key)); + ASSERT_EQ(data.length(), bufmgr.access(bufmgr_key)->write(0, data_ptr, data.length())); + + /* + * Check + * Offset 3 comes from the MultiFrameIncomingTransfer initialization + */ + ASSERT_TRUE(match(it, frame, data_ptr + 3, data.length() - 3)); + + /* + * Buffer release + */ + ASSERT_TRUE(bufmgr.access(bufmgr_key)); + it.release(); + ASSERT_FALSE(bufmgr.access(bufmgr_key)); +} + diff --git a/libuavcan/test/transport/transfer_receiver.cpp b/libuavcan/test/transport/transfer_receiver.cpp index 4981e17738..795e37d94d 100644 --- a/libuavcan/test/transport/transfer_receiver.cpp +++ b/libuavcan/test/transport/transfer_receiver.cpp @@ -4,8 +4,6 @@ #include #include -#include -#include #include