From 06cb11b6ecc84576fd577dcc23bb5bfd4ee9075d Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Wed, 5 Feb 2014 01:23:02 +0400 Subject: [PATCH] Transport buffers - dynamic and static --- .../internal/transport/transfer_buffer.hpp | 176 +++++++++++++++++ libuavcan/src/transport/transfer_buffer.cpp | 162 ++++++++++++++++ libuavcan/test/transport/transfer_buffer.cpp | 183 ++++++++++++++++++ 3 files changed, 521 insertions(+) create mode 100644 libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp create mode 100644 libuavcan/src/transport/transfer_buffer.cpp create mode 100644 libuavcan/test/transport/transfer_buffer.cpp diff --git a/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp b/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp new file mode 100644 index 0000000000..0bdd18c8db --- /dev/null +++ b/libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp @@ -0,0 +1,176 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include + +namespace uavcan +{ +#pragma pack(push, 1) +/** + * API for transfer buffer users. + */ +class TransferBufferBase +{ + uint64_t update_timestamp_; + +public: + TransferBufferBase() + : update_timestamp_(0) + { } + + virtual ~TransferBufferBase() { } + + uint64_t getUpdateTimestamp() const { return update_timestamp_; } + void setUpdateTimestamp(uint64_t val) { update_timestamp_ = val; } + + virtual int read(unsigned int offset, uint8_t* data, unsigned int len) const = 0; + virtual int write(unsigned int offset, const uint8_t* data, unsigned int len) = 0; +}; + +/** + * Internal for TransferBufferManager + */ +class TransferBufferManagerEntry : public TransferBufferBase +{ + uint8_t node_id_; + +protected: + virtual void resetImpl() = 0; + +public: + TransferBufferManagerEntry(uint8_t node_id = NODE_ID_INVALID) + : node_id_(node_id) + { } + + uint8_t getNodeID() const { return node_id_; } + bool isEmpty() const { return node_id_ == NODE_ID_INVALID; } + + void reset(uint8_t node_id = NODE_ID_INVALID) + { + node_id_ = node_id; + resetImpl(); + } +}; + +/** + * Resizable gather/scatter storage. + * reset() call releases all memory blocks. + * Supports unordered write operations - from higher to lower offsets + */ +class DynamicTransferBuffer : public TransferBufferManagerEntry, public LinkedListNode +{ + struct Block : LinkedListNode + { + enum { SIZE = MEM_POOL_BLOCK_SIZE - sizeof(LinkedListNode) }; + uint8_t data[SIZE]; + + static Block* instantiate(IAllocator* allocator); + static void destroy(Block*& obj, IAllocator* allocator); + + void read(uint8_t*& outptr, unsigned int target_offset, + unsigned int& total_offset, unsigned int& left_to_read); + void write(const uint8_t*& inptr, unsigned int target_offset, + unsigned int& total_offset, unsigned int& left_to_write); + }; + + unsigned int max_write_pos_; + IAllocator* allocator_; + LinkedListRoot blocks_; // Blocks are ordered from lower to higher buffer offset + + void resetImpl(); + +public: + DynamicTransferBuffer(IAllocator* allocator) + : max_write_pos_(0) + , allocator_(allocator) + { + StaticAssert<(Block::SIZE > 8)>::check(); + IsDynamicallyAllocatable::check(); + IsDynamicallyAllocatable::check(); + } + + int read(unsigned int offset, uint8_t* data, unsigned int len) const; + int write(unsigned int offset, const uint8_t* data, unsigned int len); +}; +#pragma pack(pop) + +/** + * Statically allocated storage + */ +template +class StaticTransferBuffer : public TransferBufferManagerEntry +{ + uint8_t data_[SIZE]; + unsigned int max_write_pos_; + + void resetImpl() + { + max_write_pos_ = 0; +#if UAVCAN_DEBUG + std::fill(data_, data_ + SIZE, 0); +#endif + } + +public: + StaticTransferBuffer() + : max_write_pos_(0) + { + StaticAssert<(SIZE > 0)>::check(); + std::fill(data_, data_ + SIZE, 0); + } + + int read(unsigned int offset, uint8_t* data, unsigned int len) const + { + if (!data) + { + assert(0); + return -1; + } + if (offset >= max_write_pos_) + return 0; + if ((offset + len) > max_write_pos_) + len = max_write_pos_ - offset; + assert((offset + len) <= max_write_pos_); + std::copy(data_ + offset, data_ + offset + len, data); + return len; + } + + int write(unsigned int offset, const uint8_t* data, unsigned int len) + { + if (!data) + { + assert(0); + return -1; + } + if (offset >= SIZE) + return 0; + if ((offset + len) > SIZE) + len = SIZE - offset; + assert((offset + len) <= SIZE); + std::copy(data, data + len, data_ + offset); + max_write_pos_ = std::max(offset + len, max_write_pos_); + return len; + } +}; + +/** + * Manages different storage types (static/dynamic) for transfer reception logic. + */ +class ITransferBufferManager +{ +public: + virtual ~ITransferBufferManager() { } + virtual TransferBufferBase* access(uint8_t node_id) = 0; + virtual TransferBufferBase* create(uint8_t node_id) = 0; + virtual void remove(uint8_t node_id) = 0; + virtual void cleanup(uint64_t oldest_timestamp) = 0; +}; + +} diff --git a/libuavcan/src/transport/transfer_buffer.cpp b/libuavcan/src/transport/transfer_buffer.cpp new file mode 100644 index 0000000000..d84ce9b23e --- /dev/null +++ b/libuavcan/src/transport/transfer_buffer.cpp @@ -0,0 +1,162 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +namespace uavcan +{ +/* + * DynamicTransferBuffer::Block + */ +DynamicTransferBuffer::Block* DynamicTransferBuffer::Block::instantiate(IAllocator* allocator) +{ + assert(allocator); + void* const praw = allocator->allocate(sizeof(Block)); + if (praw == NULL) + return NULL; + return new (praw) Block; +} + +void DynamicTransferBuffer::Block::destroy(Block*& obj, IAllocator* allocator) +{ + assert(allocator); + if (obj != NULL) + { + obj->~Block(); + allocator->deallocate(obj); + obj = NULL; + } +} + +void DynamicTransferBuffer::Block::read(uint8_t*& outptr, unsigned int target_offset, + unsigned int& total_offset, unsigned int& left_to_read) +{ + assert(outptr); + for (int i = 0; (i < Block::SIZE) && (left_to_read > 0); i++, total_offset++) + { + if (total_offset >= target_offset) + { + *outptr++ = data[i]; + left_to_read--; + } + } +} + +void DynamicTransferBuffer::Block::write(const uint8_t*& inptr, unsigned int target_offset, + unsigned int& total_offset, unsigned int& left_to_write) +{ + assert(inptr); + for (int i = 0; (i < Block::SIZE) && (left_to_write > 0); i++, total_offset++) + { + if (total_offset >= target_offset) + { + data[i] = *inptr++; + left_to_write--; + } + } +} + +/* + * DynamicTransferBuffer + */ +void DynamicTransferBuffer::resetImpl() +{ + max_write_pos_ = 0; + Block* p = blocks_.get(); + while (p) + { + Block* const next = p->getNextListNode(); + blocks_.remove(p); + Block::destroy(p, allocator_); + p = next; + } +} + +int DynamicTransferBuffer::read(unsigned int offset, uint8_t* data, unsigned int len) const +{ + if (!data) + { + assert(0); + return -1; + } + if (offset >= max_write_pos_) + return 0; + if ((offset + len) > max_write_pos_) + len = max_write_pos_ - offset; + assert((offset + len) <= max_write_pos_); + + // This shall be optimized. + unsigned int total_offset = 0, left_to_read = len; + uint8_t* outptr = data; + Block* p = blocks_.get(); + while (p) + { + p->read(outptr, offset, total_offset, left_to_read); + if (left_to_read == 0) + break; + p = p->getNextListNode(); + } + + assert(left_to_read == 0); + return len; +} + +int DynamicTransferBuffer::write(unsigned int offset, const uint8_t* data, unsigned int len) +{ + if (!data) + { + assert(0); + return -1; + } + + unsigned int total_offset = 0, left_to_write = len; + const uint8_t* inptr = data; + Block* p = blocks_.get(), *last_written_block = NULL; + + // First we need to write the part that is already allocated + while (p) + { + last_written_block = p; + p->write(inptr, offset, total_offset, left_to_write); + if (left_to_write == 0) + break; + p = p->getNextListNode(); + } + + // Then we need to append new chunks until all data is written + while (left_to_write > 0) + { + // cppcheck-suppress nullPointer + assert(p == NULL); + + // Allocating the chunk + Block* new_block = Block::instantiate(allocator_); + if (new_block == NULL) + break; // We're in deep shit. + + // Appending the chain with the new block + if (last_written_block != NULL) + { + assert(last_written_block->getNextListNode() == NULL); // Because it is last in the chain + last_written_block->setNextListNode(new_block); + new_block->setNextListNode(NULL); + } + else + { + blocks_.insert(new_block); + } + last_written_block = new_block; + + // Writing the data + new_block->write(inptr, offset, total_offset, left_to_write); + } + + const int actually_written = len - left_to_write; + max_write_pos_ = std::max(offset + actually_written, max_write_pos_); + return actually_written; +} + +} diff --git a/libuavcan/test/transport/transfer_buffer.cpp b/libuavcan/test/transport/transfer_buffer.cpp new file mode 100644 index 0000000000..07d716311c --- /dev/null +++ b/libuavcan/test/transport/transfer_buffer.cpp @@ -0,0 +1,183 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +static const std::string TEST_DATA = + "It was like this: I asked myself one day this question - what if Napoleon, for instance, had happened to be in my " + "place, and if he had not had Toulon nor Egypt nor the passage of Mont Blanc to begin his career with, but " + "instead of all those picturesque and monumental things, there had simply been some ridiculous old hag, a " + "pawnbroker, who had to be murdered too to get money from her trunk (for his career, you understand). " + "Well, would he have brought himself to that if there had been no other means?"; + +template +static bool allEqual(const T a) +{ + int n = sizeof(a) / sizeof(a[0]); + while (--n > 0 && a[n] == a[0]) { } + return n == 0; +} + +template +static void fill(const T a, int value) +{ + for (unsigned int i = 0; i < sizeof(a) / sizeof(a[0]); i++) + a[i] = value; +} + +static bool matchAgainstTestData(const uavcan::TransferBufferBase& tbb, unsigned int offset, int len = -1) +{ + uint8_t local_buffer[1024]; + fill(local_buffer, 0); + assert((len < 0) || (sizeof(local_buffer) >= static_cast(len))); + + if (len < 0) + { + const int res = tbb.read(offset, local_buffer, sizeof(local_buffer)); + if (res < 0) + { + std::cout << "matchAgainstTestData(): res " << res << std::endl; + return false; + } + len = res; + } + else + { + const int res = tbb.read(offset, local_buffer, len); + if (res != len) + { + std::cout << "matchAgainstTestData(): res " << res << " expected " << len << std::endl; + return false; + } + } + const bool equals = std::equal(local_buffer, local_buffer + len, TEST_DATA.begin() + offset); + if (!equals) + { + std::cout + << "local_buffer:\n\t" << local_buffer + << std::endl; + std::cout + << "test_data:\n\t" << std::string(TEST_DATA.begin() + offset, TEST_DATA.begin() + offset + len) + << std::endl; + } + return equals; +} + +TEST(TransferBuffer, TestDataValidation) +{ + ASSERT_LE(4, TEST_DATA.length() / uavcan::MEM_POOL_BLOCK_SIZE); + uint8_t local_buffer[50]; + std::copy(TEST_DATA.begin(), TEST_DATA.begin() + sizeof(local_buffer), local_buffer); + ASSERT_FALSE(allEqual(local_buffer)); +} + +static const int TEST_BUFFER_SIZE = 200; + +TEST(StaticTransferBuffer, Basic) +{ + using uavcan::StaticTransferBuffer; + StaticTransferBuffer buf; + + uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; + const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); + + // Empty reads + fill(local_buffer, 0xA5); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(999, local_buffer, 0)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Bulk write + ASSERT_EQ(TEST_BUFFER_SIZE, buf.write(0, test_data_ptr, TEST_DATA.length())); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4)); + + // Reset + fill(local_buffer, 0xA5); + buf.reset(); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Random write + ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 12, 21)); + + ASSERT_EQ(12, buf.write(0, test_data_ptr, 12)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + + ASSERT_EQ(0, buf.write(21, test_data_ptr + 21, 0)); + ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, 999)); + ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); +} + + +TEST(DynamicTransferBuffer, Basic) +{ + using uavcan::DynamicTransferBuffer; + + static const int POOL_BLOCKS = 8; + uavcan::PoolAllocator pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + DynamicTransferBuffer buf(&poolmgr); + + uint8_t local_buffer[TEST_BUFFER_SIZE * 2]; + const uint8_t* const test_data_ptr = reinterpret_cast(TEST_DATA.c_str()); + + // Empty reads + fill(local_buffer, 0xA5); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(999, local_buffer, 0)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Bulk write + const int max_size = buf.write(0, test_data_ptr, TEST_DATA.length()); + ASSERT_LE(max_size, TEST_DATA.length()); + ASSERT_GT(max_size, 0); + + ASSERT_EQ(0, pool.getNumFreeBlocks()); // Making sure all memory was used up + + ASSERT_TRUE(matchAgainstTestData(buf, 0)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 2, TEST_BUFFER_SIZE / 4)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE / 4, TEST_BUFFER_SIZE / 2)); + ASSERT_TRUE(matchAgainstTestData(buf, 0, TEST_BUFFER_SIZE / 4)); + + // Reset + fill(local_buffer, 0xA5); + buf.reset(); + ASSERT_EQ(0, buf.read(0, local_buffer, 0)); + ASSERT_EQ(0, buf.read(0, local_buffer, 999)); + ASSERT_TRUE(allEqual(local_buffer)); + + // Random write + ASSERT_EQ(21, buf.write(12, test_data_ptr + 12, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 12, 21)); + + ASSERT_EQ(60, buf.write(TEST_BUFFER_SIZE - 60, test_data_ptr + TEST_BUFFER_SIZE - 60, 60)); + ASSERT_TRUE(matchAgainstTestData(buf, TEST_BUFFER_SIZE - 60)); + + // Now we have two empty regions: empty-data-empty-data + + ASSERT_EQ(0, buf.write(0, test_data_ptr, 0)); + ASSERT_EQ(TEST_BUFFER_SIZE - 21, buf.write(21, test_data_ptr + 21, TEST_BUFFER_SIZE - 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 21, TEST_BUFFER_SIZE - 21)); + + // Now: empty-data-data-data + + ASSERT_EQ(21, buf.write(0, test_data_ptr, 21)); + ASSERT_TRUE(matchAgainstTestData(buf, 0)); +}