mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-23 05:53:33 +08:00
365 lines
10 KiB
C++
365 lines
10 KiB
C++
/*
|
|
* Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include <stdint.h>
|
|
#include <algorithm>
|
|
#include <limits>
|
|
#include <uavcan/internal/transport/transfer.hpp>
|
|
#include <uavcan/internal/linked_list.hpp>
|
|
#include <uavcan/internal/impl_constants.hpp>
|
|
#include <uavcan/internal/debug.hpp>
|
|
|
|
namespace uavcan
|
|
{
|
|
#pragma pack(push, 1)
|
|
/**
|
|
* API for transfer buffer users.
|
|
*/
|
|
class TransferBufferBase : Noncopyable
|
|
{
|
|
public:
|
|
virtual ~TransferBufferBase() { }
|
|
|
|
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();
|
|
}
|
|
|
|
~DynamicTransferBuffer()
|
|
{
|
|
resetImpl();
|
|
}
|
|
|
|
static DynamicTransferBuffer* instantiate(IAllocator* allocator);
|
|
static void destroy(DynamicTransferBuffer*& obj, IAllocator* allocator);
|
|
|
|
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;
|
|
}
|
|
|
|
bool migrateFrom(const TransferBufferManagerEntry* tbme)
|
|
{
|
|
if (tbme == NULL || tbme->isEmpty())
|
|
{
|
|
assert(0);
|
|
return false;
|
|
}
|
|
|
|
// Resetting self and moving all data from the source
|
|
reset(tbme->getNodeID());
|
|
const int res = tbme->read(0, data_, SIZE);
|
|
if (res < 0)
|
|
{
|
|
reset();
|
|
return false;
|
|
}
|
|
max_write_pos_ = res;
|
|
if (res < int(SIZE))
|
|
return true;
|
|
|
|
// Now we need to make sure that all data can fit the storage
|
|
uint8_t dummy = 0;
|
|
if (tbme->read(SIZE, &dummy, 1) > 0)
|
|
{
|
|
reset(); // Damn, the buffer was too large
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
};
|
|
|
|
/**
|
|
* 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;
|
|
};
|
|
|
|
/**
|
|
* Buffer manager implementation.
|
|
*/
|
|
template <unsigned int STATIC_BUF_SIZE, unsigned int NUM_STATIC_BUFS>
|
|
class TransferBufferManager : public ITransferBufferManager, Noncopyable
|
|
{
|
|
typedef StaticTransferBuffer<STATIC_BUF_SIZE> StaticBufferType;
|
|
|
|
StaticBufferType static_buffers_[NUM_STATIC_BUFS];
|
|
LinkedListRoot<DynamicTransferBuffer> dynamic_buffers_;
|
|
IAllocator* const allocator_;
|
|
|
|
StaticBufferType* findFirstStatic(uint8_t node_id)
|
|
{
|
|
assert((node_id == NODE_ID_INVALID) || (node_id <= NODE_ID_MAX));
|
|
for (unsigned int i = 0; i < NUM_STATIC_BUFS; i++)
|
|
{
|
|
if (static_buffers_[i].getNodeID() == node_id)
|
|
return static_buffers_ + i;
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
DynamicTransferBuffer* findFirstDynamic(uint8_t node_id)
|
|
{
|
|
DynamicTransferBuffer* dyn = dynamic_buffers_.get();
|
|
while (dyn)
|
|
{
|
|
assert(!dyn->isEmpty());
|
|
if (dyn->getNodeID() == node_id)
|
|
return dyn;
|
|
dyn = dyn->getNextListNode();
|
|
}
|
|
return NULL;
|
|
}
|
|
|
|
void optimizeStorage()
|
|
{
|
|
while (!dynamic_buffers_.isEmpty())
|
|
{
|
|
StaticBufferType* const sb = findFirstStatic(NODE_ID_INVALID);
|
|
if (sb == NULL)
|
|
break;
|
|
DynamicTransferBuffer* dyn = dynamic_buffers_.get();
|
|
assert(dyn);
|
|
assert(!dyn->isEmpty());
|
|
if (sb->migrateFrom(dyn))
|
|
{
|
|
assert(!dyn->isEmpty());
|
|
UAVCAN_TRACE("TransferBufferManager", "Storage optimization: Migrated NID %i", int(dyn->getNodeID()));
|
|
dynamic_buffers_.remove(dyn);
|
|
DynamicTransferBuffer::destroy(dyn, allocator_);
|
|
}
|
|
else
|
|
{
|
|
/* Migration can fail if a dynamic buffer contains more data than a static buffer can accomodate (more
|
|
* than STATIC_BUF_SIZE). This means that there is probably something wrong with the network. Logic
|
|
* that uses this class should explicitly ensure the proper maximum data size.
|
|
*/
|
|
UAVCAN_TRACE("TransferBufferManager", "Storage optimization: MIGRATION FAILURE NID %i BUFSIZE %u",
|
|
int(dyn->getNodeID()), STATIC_BUF_SIZE);
|
|
sb->reset();
|
|
break; // Probably we should try to migrate the rest?
|
|
}
|
|
}
|
|
}
|
|
|
|
public:
|
|
TransferBufferManager(IAllocator* allocator)
|
|
: allocator_(allocator)
|
|
{ }
|
|
|
|
~TransferBufferManager()
|
|
{
|
|
DynamicTransferBuffer* dyn = dynamic_buffers_.get();
|
|
while (dyn)
|
|
{
|
|
DynamicTransferBuffer* const next = dyn->getNextListNode();
|
|
dynamic_buffers_.remove(dyn);
|
|
DynamicTransferBuffer::destroy(dyn, allocator_);
|
|
dyn = next;
|
|
}
|
|
}
|
|
|
|
unsigned int getNumDynamicBuffers() const { return dynamic_buffers_.length(); }
|
|
|
|
unsigned int getNumStaticBuffers() const
|
|
{
|
|
unsigned int res = 0;
|
|
for (unsigned int i = 0; i < NUM_STATIC_BUFS; i++)
|
|
{
|
|
if (!static_buffers_[i].isEmpty())
|
|
res++;
|
|
}
|
|
return res;
|
|
}
|
|
|
|
TransferBufferBase* access(uint8_t node_id)
|
|
{
|
|
if (node_id > NODE_ID_MAX || node_id == NODE_ID_INVALID)
|
|
{
|
|
assert(0);
|
|
return NULL;
|
|
}
|
|
TransferBufferManagerEntry* tbme = findFirstStatic(node_id);
|
|
if (tbme)
|
|
return tbme;
|
|
return findFirstDynamic(node_id);
|
|
}
|
|
|
|
TransferBufferBase* create(uint8_t node_id)
|
|
{
|
|
if (node_id > NODE_ID_MAX || node_id == NODE_ID_INVALID)
|
|
{
|
|
assert(0);
|
|
return NULL;
|
|
}
|
|
remove(node_id);
|
|
|
|
TransferBufferManagerEntry* tbme = findFirstStatic(NODE_ID_INVALID);
|
|
if (tbme == NULL)
|
|
{
|
|
DynamicTransferBuffer* dyn = DynamicTransferBuffer::instantiate(allocator_);
|
|
tbme = dyn;
|
|
if (dyn == NULL)
|
|
return NULL; // Epic fail.
|
|
dynamic_buffers_.insert(dyn);
|
|
}
|
|
|
|
if (tbme)
|
|
{
|
|
assert(tbme->isEmpty());
|
|
tbme->reset(node_id);
|
|
}
|
|
return tbme;
|
|
}
|
|
|
|
void remove(uint8_t node_id)
|
|
{
|
|
assert((node_id <= NODE_ID_MAX) && (node_id != NODE_ID_INVALID));
|
|
|
|
TransferBufferManagerEntry* const tbme = findFirstStatic(node_id);
|
|
if (tbme)
|
|
{
|
|
tbme->reset();
|
|
optimizeStorage();
|
|
return;
|
|
}
|
|
|
|
DynamicTransferBuffer* dyn = findFirstDynamic(node_id);
|
|
if (dyn)
|
|
{
|
|
dynamic_buffers_.remove(dyn);
|
|
DynamicTransferBuffer::destroy(dyn, allocator_);
|
|
}
|
|
}
|
|
};
|
|
|
|
}
|