Transport buffers - dynamic and static

libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp

@@ -0,0 +1,176 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <stdint.h>
+#include <algorithm>
+#include <uavcan/internal/transport/transfer.hpp>
+#include <uavcan/internal/linked_list.hpp>
+#include <uavcan/internal/impl_constants.hpp>
+
+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<DynamicTransferBuffer>
+{
+    struct Block : LinkedListNode<Block>
+    {
+        enum { SIZE = MEM_POOL_BLOCK_SIZE - sizeof(LinkedListNode<Block>) };
+        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<Block> 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<Block>::check();
+        IsDynamicallyAllocatable<DynamicTransferBuffer>::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 <unsigned int SIZE>
+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;
+};
+
+}

libuavcan/src/transport/transfer_buffer.cpp

@@ -0,0 +1,162 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cassert>
+#include <cstdlib>
+#include <uavcan/internal/transport/transfer_buffer.hpp>
+
+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;
+}
+
+}

libuavcan/test/transport/transfer_buffer.cpp

@@ -0,0 +1,183 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <gtest/gtest.h>
+#include <uavcan/internal/transport/transfer_buffer.hpp>
+
+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 <typename T>
+static bool allEqual(const T a)
+{
+    int n = sizeof(a) / sizeof(a[0]);
+    while (--n > 0 && a[n] == a[0]) { }
+    return n == 0;
+}
+
+template <typename T>
+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<unsigned int>(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<TEST_BUFFER_SIZE> buf;
+
+    uint8_t local_buffer[TEST_BUFFER_SIZE * 2];
+    const uint8_t* const test_data_ptr = reinterpret_cast<const uint8_t*>(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<uavcan::MEM_POOL_BLOCK_SIZE * POOL_BLOCKS, uavcan::MEM_POOL_BLOCK_SIZE> 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<const uint8_t*>(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));
+}