diff --git a/libuavcan/include/uavcan/internal/transport/transfer.hpp b/libuavcan/include/uavcan/internal/transport/transfer.hpp index a2908a5fdd..efc5d94188 100644 --- a/libuavcan/include/uavcan/internal/transport/transfer.hpp +++ b/libuavcan/include/uavcan/internal/transport/transfer.hpp @@ -11,12 +11,20 @@ namespace uavcan { +enum NodeIDConstants +{ + NODE_ID_BROADCAST = 0, + NODE_ID_MAX = 127, + NODE_ID_INVALID = 255 +}; + enum TransferType { SERVICE_RESPONSE = 0, SERVICE_REQUEST = 1, MESSAGE_BROADCAST = 2, - MESSAGE_UNICAST = 3 + MESSAGE_UNICAST = 3, + NUM_TRANSFER_TYPES = 4 }; diff --git a/libuavcan/include/uavcan/internal/transport/transfer_id_registry.hpp b/libuavcan/include/uavcan/internal/transport/transfer_id_registry.hpp new file mode 100644 index 0000000000..1ae06071ef --- /dev/null +++ b/libuavcan/include/uavcan/internal/transport/transfer_id_registry.hpp @@ -0,0 +1,154 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include + +namespace uavcan +{ + +class TransferIDRegistry +{ +public: + struct Key + { + TransferType transfer_type; + uint16_t data_type_id; + uint8_t node_id; + + Key() + : transfer_type(TransferType(0)) + , data_type_id(0) + , node_id(NODE_ID_INVALID) + { } + + Key(uint8_t node_id, TransferType transfer_type, uint16_t data_type_id) + : transfer_type(transfer_type) + , data_type_id(data_type_id) + , node_id(node_id) + { } + }; + +#pragma pack(push, 1) + struct Entry + { + uint64_t timestamp; + TransferID transfer_id; + + Entry() + : timestamp(0) + { } + + Entry(TransferID transfer_id, uint64_t timestamp) + : timestamp(timestamp) + , transfer_id(transfer_id) + { } + + bool operator==(const Entry& rhs) const + { + return (timestamp == rhs.timestamp) && (transfer_id == rhs.transfer_id); + } + }; + +private: + struct StorageEntry : public Entry + { + uint16_t data_type_id; + uint8_t node_id; + + StorageEntry() + : data_type_id(0) + , node_id(NODE_ID_INVALID) + { } + + StorageEntry(uint8_t node_id, uint16_t data_type_id, const Entry& entry) + : Entry(entry) + , data_type_id(data_type_id) + , node_id(node_id) + { } + + bool isEmpty() const { return node_id == NODE_ID_INVALID; } + }; + + struct StorageEntryGroup : LinkedListNode + { + enum + { + NUM_ENTRIES = (32 - sizeof(LinkedListNode)) / sizeof(StorageEntry) + }; + StorageEntry entries[NUM_ENTRIES]; + + StorageEntryGroup() + { + StaticAssert::check(); + StaticAssert= 2>::check(); + } + }; +#pragma pack(pop) + + class List + { + LinkedListRoot list_; + + public: + StorageEntryGroup* getHead() const { return list_.get(); } + StorageEntry* find(const Key& key); + bool create(const StorageEntry& entry, IAllocator* allocator); + void remove(const Key& key, IAllocator* allocator); + void compact(IAllocator* allocator); + }; + + List lists_by_transfer_type_[NUM_TRANSFER_TYPES]; + IAllocator* const allocator_; + +public: + TransferIDRegistry(IAllocator* allocator) + : allocator_(allocator) + { + assert(allocator); + } + + Entry* access(const Key& key); + + bool create(const Key& key, const Entry& entry); + void remove(const Key& key); + + /** + * Removes entries where predicate returns true. + * Predicate prototype: + * bool (const TransferIDRegistry::Key& key, const TransferIDRegistry::Entry& entry) + */ + template + void removeWhere(Predicate predicate) + { + for (int transfer_type = 0; transfer_type < NUM_TRANSFER_TYPES; transfer_type++) + { + StorageEntryGroup* p = lists_by_transfer_type_[transfer_type].getHead(); + while (p) + { + for (int i = 0; i < StorageEntryGroup::NUM_ENTRIES; i++) + { + const StorageEntry* const entry = p->entries + i; + if (!entry->isEmpty()) + { + const Key key(entry->node_id, TransferType(transfer_type), entry->data_type_id); + const bool result = predicate(key, static_cast(*entry)); + if (result) + p->entries[i] = StorageEntry(); + } + } + p = p->getNextListNode(); + } + lists_by_transfer_type_[transfer_type].compact(allocator_); + } + } +}; + +} diff --git a/libuavcan/src/transport/transfer_id_registry.cpp b/libuavcan/src/transport/transfer_id_registry.cpp new file mode 100644 index 0000000000..7561615205 --- /dev/null +++ b/libuavcan/src/transport/transfer_id_registry.cpp @@ -0,0 +1,133 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +namespace uavcan +{ + +/* + * TransferIDRegistry::List + * TODO: faster search + */ +TransferIDRegistry::StorageEntry* TransferIDRegistry::List::find(const Key& key) +{ + StorageEntryGroup* p = list_.get(); + while (p) + { + for (int i = 0; i < StorageEntryGroup::NUM_ENTRIES; i++) + { + if (p->entries[i].node_id == key.node_id && p->entries[i].data_type_id == key.data_type_id) + return p->entries + i; + } + p = p->getNextListNode(); + } + return NULL; +} + +bool TransferIDRegistry::List::create(const StorageEntry& entry, IAllocator* allocator) +{ + assert(allocator); + StorageEntryGroup* p = list_.get(); + while (p) + { + for (int i = 0; i < StorageEntryGroup::NUM_ENTRIES; i++) + { + if (p->entries[i].isEmpty()) + { + p->entries[i] = entry; + return true; + } + } + p = p->getNextListNode(); + } + + void* praw = allocator->allocate(sizeof(StorageEntryGroup)); + if (praw == NULL) + return false; + + StorageEntryGroup* seg = new (praw) StorageEntryGroup(); + assert(seg); + + seg->entries[0] = entry; + list_.insert(seg); + return true; +} + +void TransferIDRegistry::List::remove(const Key& key, IAllocator* allocator) +{ + assert(allocator); + StorageEntryGroup* p = list_.get(); + while (p) + { + for (int i = 0; i < StorageEntryGroup::NUM_ENTRIES; i++) + { + if (p->entries[i].node_id == key.node_id && p->entries[i].data_type_id == key.data_type_id) + p->entries[i] = StorageEntry(); + } + p = p->getNextListNode(); + } + compact(allocator); +} + +void TransferIDRegistry::List::compact(IAllocator* allocator) +{ + // TODO: defragment + assert(allocator); + StorageEntryGroup* p = list_.get(); + while (p) + { + StorageEntryGroup* const next = p->getNextListNode(); + bool remove_this = true; + for (int i = 0; i < StorageEntryGroup::NUM_ENTRIES; i++) + { + if (!p->entries[i].isEmpty()) + remove_this = false; + } + if (remove_this) + { + list_.remove(p); + p->~StorageEntryGroup(); + allocator->deallocate(p); + } + p = next; + } +} + +/* + * TransferIDRegistry + */ +TransferIDRegistry::Entry* TransferIDRegistry::access(const Key& key) +{ + if (key.node_id > NODE_ID_MAX || key.transfer_type >= NUM_TRANSFER_TYPES) + { + assert(0); + return NULL; + } + return static_cast(lists_by_transfer_type_[key.transfer_type].find(key)); +} + +bool TransferIDRegistry::create(const Key& key, const Entry& entry) +{ + if (key.node_id > NODE_ID_MAX || key.transfer_type >= NUM_TRANSFER_TYPES) + { + assert(0); + return false; + } + return lists_by_transfer_type_[key.transfer_type].create( + StorageEntry(key.node_id, key.data_type_id, entry), allocator_); +} + +void TransferIDRegistry::remove(const Key& key) +{ + if (key.node_id > NODE_ID_MAX || key.transfer_type >= NUM_TRANSFER_TYPES) + { + assert(0); + return; + } + lists_by_transfer_type_[key.transfer_type].remove(key, allocator_); +} + +} diff --git a/libuavcan/test/transport/transfer_id_registry.cpp b/libuavcan/test/transport/transfer_id_registry.cpp new file mode 100644 index 0000000000..e83c51b853 --- /dev/null +++ b/libuavcan/test/transport/transfer_id_registry.cpp @@ -0,0 +1,136 @@ +/* + * Copyright (C) 2014 + */ + +#include +#include +#include + + +struct OddNodeIDPredicate +{ + bool operator()(const uavcan::TransferIDRegistry::Key& key, const uavcan::TransferIDRegistry::Entry& entry) const + { + return key.node_id & 1; + } +}; + + +TEST(TransferIDRegistry, Basic) +{ + using uavcan::Frame; + using uavcan::TransferID; + using uavcan::TransferType; + using uavcan::TransferIDRegistry; + typedef TransferIDRegistry::Key Key; + typedef TransferIDRegistry::Entry Entry; + + static const int POOL_BLOCKS = 8; + uavcan::PoolAllocator<32 * POOL_BLOCKS, 32> pool; + uavcan::PoolManager<2> poolmgr; + poolmgr.addPool(&pool); + + TransferIDRegistry reg(&poolmgr); + + ASSERT_EQ(NULL, reg.access(Key(0, uavcan::MESSAGE_BROADCAST, 0))); + + static const int NUM_ITEMS = 100; // Just to make sure it will be enough + Key keys[NUM_ITEMS]; + Entry entries[NUM_ITEMS]; + Entry immutable_entries[NUM_ITEMS]; + + // Initializing the test data + for (int i = 0; i < NUM_ITEMS; i++) + { + keys[i].data_type_id = i * (Frame::DATA_TYPE_ID_MAX / NUM_ITEMS); + keys[i].node_id = i * (uavcan::NODE_ID_MAX / NUM_ITEMS); + keys[i].transfer_type = TransferType(i % uavcan::NUM_TRANSFER_TYPES); + + entries[i].timestamp = i * 10000000; + entries[i].transfer_id = TransferID(i % TransferID::MAX); + immutable_entries[i] = entries[i]; + } + + // Filling the registry + bool filled = false; + int num_registered = 0; + for (int i = 0; i < NUM_ITEMS; i++) + { + const bool res = reg.create(keys[i], entries[i]); + if (!res) + { + ASSERT_EQ(0, pool.getNumFreeBlocks()); + const int num_entries_per_block = (i + 1) / POOL_BLOCKS; + ASSERT_LE(2, num_entries_per_block); // Ensuring minimal number of entries per block + filled = true; + break; + } + num_registered++; + } + ASSERT_TRUE(filled); // No free buffer space left by now + ASSERT_LT(POOL_BLOCKS, num_registered); // Being paranoid + + // Checking each value + for (int i = 0; i < num_registered; i++) + { + const Entry* const entry = reg.access(keys[i]); + ASSERT_TRUE(entry); + ASSERT_EQ(*entry, immutable_entries[i]); + } + + // Removing half of the values, making sure some of the memory blocks were released + const int num_blocks_to_remove = num_registered / 2; + for (int i = 0; i < num_blocks_to_remove; i++) + { + reg.remove(keys[i]); + } + for (int i = 0; i < num_blocks_to_remove; i++) + { + ASSERT_FALSE(reg.access(keys[i])); + } + ASSERT_LT(1, pool.getNumFreeBlocks()); // At least one should be freed + + // Adding another entries, making sure they all fit the memory + const int new_limit = num_registered + num_blocks_to_remove; + for (int i = num_registered; i < new_limit; i++) + { + ASSERT_TRUE(reg.create(keys[i], entries[i])); + } + ASSERT_EQ(0, pool.getNumFreeBlocks()); + + // Making sure the old entries didn't creep into the registry + for (int i = 0; i < new_limit; i++) + { + const Entry* const entry = reg.access(keys[i]); + if (i < num_blocks_to_remove) + { + ASSERT_FALSE(entry); + } + else + { + ASSERT_TRUE(entry); + ASSERT_EQ(*entry, immutable_entries[i]); + } + } + + // Removing something, making sure it was removed indeed + reg.removeWhere(OddNodeIDPredicate()); + + for (int i = 0; i < new_limit; i++) + { + const Entry* const entry = reg.access(keys[i]); + if (i < num_blocks_to_remove) + { + ASSERT_FALSE(entry); + } + else if (keys[i].node_id & 1) + { + ASSERT_FALSE(entry); + } + else + { + ASSERT_TRUE(entry); + ASSERT_EQ(*entry, immutable_entries[i]); + } + } +}