Transfer ID registry

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
 };
 
 

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

@@ -0,0 +1,154 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <stdint.h>
+#include <uavcan/internal/transport/transfer.hpp>
+#include <uavcan/internal/linked_list.hpp>
+#include <uavcan/internal/static_assert.hpp>
+#include <uavcan/internal/dynamic_memory.hpp>
+
+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<StorageEntryGroup>
+    {
+        enum
+        {
+            NUM_ENTRIES = (32 - sizeof(LinkedListNode<StorageEntryGroup>)) / sizeof(StorageEntry)
+        };
+        StorageEntry entries[NUM_ENTRIES];
+
+        StorageEntryGroup()
+        {
+            StaticAssert<sizeof(StorageEntryGroup) <= 32>::check();
+            StaticAssert<NUM_ENTRIES >= 2>::check();
+        }
+    };
+#pragma pack(pop)
+
+    class List
+    {
+        LinkedListRoot<StorageEntryGroup> 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 <typename Predicate>
+    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<const Entry&>(*entry));
+                        if (result)
+                            p->entries[i] = StorageEntry();
+                    }
+                }
+                p = p->getNextListNode();
+            }
+            lists_by_transfer_type_[transfer_type].compact(allocator_);
+        }
+    }
+};
+
+}

libuavcan/src/transport/transfer_id_registry.cpp

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

libuavcan/test/transport/transfer_id_registry.cpp

@@ -0,0 +1,136 @@
+/*
+ * Copyright (C) 2014 <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/internal/transport/transfer.hpp>
+#include <uavcan/internal/transport/transfer_id_registry.hpp>
+
+
+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]);
+        }
+    }
+}