mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-04-28 16:44:06 +08:00
Transport buffers - dynamic and static
This commit is contained in:
Pavel Kirienko
parent
fb0f44c4b4
commit
06cb11b6ec
176
libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp
Normal file
176
libuavcan/include/uavcan/internal/transport/transfer_buffer.hpp
Normal file
@ -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;
|
||||
};
|
||||
|
||||
}
|
||||
162
libuavcan/src/transport/transfer_buffer.cpp
Normal file
162
libuavcan/src/transport/transfer_buffer.cpp
Normal file
@ -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;
|
||||
}
|
||||
|
||||
}
|
||||
183
libuavcan/test/transport/transfer_buffer.cpp
Normal file
183
libuavcan/test/transport/transfer_buffer.cpp
Normal file
@ -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));
|
||||
}
|
||||
Loading…
x
Reference in New Issue
Block a user