mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-19 01:23:34 +08:00
Implemented TransferBufferManager
This commit is contained in:
Pavel Kirienko
@@ -6,9 +6,11 @@
|
||||
|
||||
#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
|
||||
{
|
||||
@@ -20,6 +22,9 @@ class TransferBufferBase
|
||||
{
|
||||
uint64_t update_timestamp_;
|
||||
|
||||
protected:
|
||||
void reset() { update_timestamp_ = 0; }
|
||||
|
||||
public:
|
||||
TransferBufferBase()
|
||||
: update_timestamp_(0)
|
||||
@@ -55,6 +60,7 @@ public:
|
||||
void reset(uint8_t node_id = NODE_ID_INVALID)
|
||||
{
|
||||
node_id_ = node_id;
|
||||
TransferBufferBase::reset();
|
||||
resetImpl();
|
||||
}
|
||||
};
|
||||
@@ -101,6 +107,9 @@ public:
|
||||
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);
|
||||
};
|
||||
@@ -163,6 +172,37 @@ public:
|
||||
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());
|
||||
setUpdateTimestamp(tbme->getUpdateTimestamp());
|
||||
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;
|
||||
}
|
||||
};
|
||||
|
||||
/**
|
||||
@@ -178,4 +218,190 @@ public:
|
||||
virtual void cleanup(uint64_t oldest_timestamp) = 0;
|
||||
};
|
||||
|
||||
/**
|
||||
* Buffer manager implementation.
|
||||
*/
|
||||
template <unsigned int STATIC_BUF_SIZE, unsigned int NUM_STATIC_BUFS>
|
||||
class TransferBufferManager : public ITransferBufferManager
|
||||
{
|
||||
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());
|
||||
assert(dyn->getUpdateTimestamp() == sb->getUpdateTimestamp());
|
||||
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()
|
||||
{
|
||||
cleanup(std::numeric_limits<uint64_t>::max());
|
||||
}
|
||||
|
||||
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_);
|
||||
}
|
||||
}
|
||||
|
||||
void cleanup(uint64_t oldest_timestamp)
|
||||
{
|
||||
int num_released_statics = 0;
|
||||
for (unsigned int i = 0; i < NUM_STATIC_BUFS; i++)
|
||||
{
|
||||
TransferBufferManagerEntry* const buf = static_buffers_ + i;
|
||||
if (buf->isEmpty())
|
||||
continue;
|
||||
if (buf->getUpdateTimestamp() <= oldest_timestamp)
|
||||
{
|
||||
UAVCAN_TRACE("TransferBufferManager", "Cleanup: Dead static buffer, NID %i", int(buf->getNodeID()));
|
||||
buf->reset();
|
||||
num_released_statics++;
|
||||
}
|
||||
}
|
||||
|
||||
DynamicTransferBuffer* dyn = dynamic_buffers_.get();
|
||||
while (dyn)
|
||||
{
|
||||
assert(!dyn->isEmpty());
|
||||
DynamicTransferBuffer* const next = dyn->getNextListNode();
|
||||
if (dyn->getUpdateTimestamp() <= oldest_timestamp)
|
||||
{
|
||||
UAVCAN_TRACE("TransferBufferManager", "Cleanup: Dead dynamic buffer, NID %i", int(dyn->getNodeID()));
|
||||
dynamic_buffers_.remove(dyn);
|
||||
DynamicTransferBuffer::destroy(dyn, allocator_);
|
||||
}
|
||||
dyn = next;
|
||||
}
|
||||
|
||||
if (num_released_statics > 0)
|
||||
optimizeStorage();
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user