Basic functionality: CAN IO Manager, unit tests, debug stuff, memory management

libuavcan/include/uavcan/internal/can_io.hpp

@@ -0,0 +1,161 @@
+/*
+ * CAN bus IO logic.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <cstring>
+#include <stdint.h>
+#include <uavcan/internal/linked_list.hpp>
+#include <uavcan/internal/dynamic_memory.hpp>
+#include <uavcan/can_driver.hpp>
+#include <uavcan/system_clock.hpp>
+
+namespace uavcan
+{
+
+struct CanRxFrame
+{
+    CanFrame frame;
+    uint64_t timestamp;
+    uint8_t iface_index;
+};
+
+
+class CanTxQueue
+{
+public:
+    enum Qos { VOLATILE, PERSISTENT };
+
+#pragma pack(push, 1)
+    struct Entry : public LinkedListNode<Entry>
+    {
+        uint64_t monotonic_deadline;
+        CanFrame frame;
+        uint8_t qos;
+
+        Entry(const CanFrame& frame, uint64_t monotonic_deadline, Qos qos)
+        : monotonic_deadline(monotonic_deadline)
+        , frame(frame)
+        , qos(uint8_t(qos))
+        {
+            assert(qos == VOLATILE || qos == PERSISTENT);
+        }
+
+        bool isExpired(uint64_t monotonic_timestamp) const { return monotonic_timestamp > monotonic_deadline; }
+
+        bool qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
+        bool qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const;
+        bool qosHigherThan(const Entry& rhs) const { return qosHigherThan(rhs.frame, Qos(rhs.qos)); }
+        bool qosLowerThan(const Entry& rhs)  const { return qosLowerThan(rhs.frame, Qos(rhs.qos)); }
+
+        std::string toString() const;
+    };
+#pragma pack(pop)
+
+private:
+    class PriorityInsertionComparator
+    {
+        const CanFrame& frm_;
+    public:
+        PriorityInsertionComparator(const CanFrame& frm) : frm_(frm) { }
+        bool operator()(const Entry* entry)
+        {
+            assert(entry);
+            return frm_.priorityHigherThan(entry->frame);
+        }
+    };
+
+    LinkedListRoot<Entry> queue_;
+    IAllocator* const allocator_;
+    ISystemClock* const sysclock_;
+    uint32_t rejected_frames_cnt_;
+
+    void registerRejectedFrame();
+
+public:
+    CanTxQueue()
+    : allocator_(NULL)
+    , sysclock_(NULL)
+    , rejected_frames_cnt_(0)
+    { }
+
+    CanTxQueue(IAllocator* allocator, ISystemClock* sysclock)
+    : allocator_(allocator)
+    , sysclock_(sysclock)
+    , rejected_frames_cnt_(0)
+    {
+        assert(allocator);
+        assert(sysclock);
+    }
+
+    ~CanTxQueue();
+
+    void push(const CanFrame& frame, uint64_t monotonic_tx_deadline, Qos qos);
+
+    Entry* peek();               // Modifier
+    void remove(Entry* entry);
+
+    bool topPriorityHigherOrEqual(const CanFrame& rhs_frame) const;
+
+    uint32_t getNumRejectedFrames() const { return rejected_frames_cnt_; }
+
+    bool isEmpty() const { return queue_.isEmpty(); }
+};
+
+
+class CanIOManager
+{
+public:
+    enum { MAX_IFACES = 3 };
+
+private:
+    ICanDriver* const driver_;
+    ISystemClock* const sysclock_;
+
+    CanTxQueue tx_queues_[MAX_IFACES];
+
+    // Noncopyable
+    CanIOManager(CanIOManager&);
+    CanIOManager& operator=(CanIOManager&);
+
+    int sendToIface(int iface_index, const CanFrame& frame, uint64_t monotonic_tx_deadline);
+    int sendFromTxQueue(int iface_index);
+    int makePendingTxMask() const;
+    uint64_t getTimeUntilMonotonicDeadline(uint64_t monotonic_deadline) const;
+
+public:
+    CanIOManager(ICanDriver* driver, IAllocator* allocator, ISystemClock* sysclock)
+    : driver_(driver)
+    , sysclock_(sysclock)
+    {
+        assert(driver);
+        assert(allocator);
+        assert(sysclock);
+        assert(driver->getNumIfaces() <= MAX_IFACES);
+        // We can't initialize member array with non-default constructors in C++03
+        for (int i = 0; i < MAX_IFACES; i++)
+        {
+            tx_queues_[i].~CanTxQueue();
+            new (tx_queues_ + i) CanTxQueue(allocator, sysclock);
+        }
+    }
+
+    int getNumIfaces() const;
+
+    uint64_t getNumErrors(int iface_index) const;
+
+    /**
+     * Returns:
+     *  0 - rejected/timedout/enqueued
+     *  1+ - sent/received
+     *  negative - failure
+     */
+    int send(const CanFrame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
+             int iface_mask, CanTxQueue::Qos qos);
+    int receive(CanRxFrame& frame, uint64_t monotonic_blocking_deadline);
+};
+
+}

libuavcan/include/uavcan/internal/debug.hpp

@@ -0,0 +1,30 @@
+/*
+ * Debug stuff, should only be used for library development.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#if UAVCAN_DEBUG
+
+#include <cstdio>
+#include <cstdarg>
+
+#if __GNUC__
+__attribute__ ((format (printf, 2, 3)))
+#endif
+static void UAVCAN_TRACE(const char* src, const char* fmt, ...)
+{
+    va_list args;
+    std::printf("UAVCAN: %s: ", src);
+    va_start(args, fmt);
+    std::vprintf(fmt, args);
+    va_end(args);
+    std::puts("");
+}
+
+#else
+
+#  define UAVCAN_TRACE(...) ((void)0)
+
+#endif

libuavcan/include/uavcan/internal/dynamic_memory.hpp

@@ -0,0 +1,166 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cassert>
+#include <cstdlib>
+#include <cstring>
+#include <stdint.h>
+
+namespace uavcan
+{
+/**
+ * This interface is used by other library components that need dynamic memory.
+ */
+class IAllocator
+{
+public:
+    virtual ~IAllocator() { }
+    virtual void* allocate(std::size_t size) = 0;
+    virtual void deallocate(const void* ptr) = 0;
+};
+
+
+class IPoolAllocator : public IAllocator
+{
+public:
+    virtual bool isInPool(const void* ptr) const = 0;
+};
+
+
+template <int MAX_POOLS>
+class PoolManager : public IAllocator
+{
+    IPoolAllocator* pools_[MAX_POOLS];
+
+public:
+    PoolManager()
+    {
+        std::memset(pools_, 0, sizeof(pools_));
+    }
+
+    bool addPool(IPoolAllocator* pool)
+    {
+        assert(pool);
+        for (int i = 0; i < MAX_POOLS; i++)
+        {
+            assert(pools_[i] != pool);
+            if (pools_[i] == NULL || pools_[i] == pool)
+            {
+                pools_[i] = pool;
+                return true;
+            }
+        }
+        return false;
+    }
+
+    void* allocate(std::size_t size)
+    {
+        for (int i = 0; i < MAX_POOLS; i++)
+        {
+            if (pools_[i] == NULL)
+                break;
+            void* const pmem = pools_[i]->allocate(size);
+            if (pmem != NULL)
+                return pmem;
+        }
+        return NULL;
+    }
+
+    void deallocate(const void* ptr)
+    {
+        for (int i = 0; i < MAX_POOLS; i++)
+        {
+            if (pools_[i] == NULL)
+            {
+                assert(0);
+                break;
+            }
+            if (pools_[i]->isInPool(ptr))
+            {
+                pools_[i]->deallocate(ptr);
+                break;
+            }
+        }
+    }
+};
+
+
+template <std::size_t POOL_SIZE, std::size_t BLOCK_SIZE>
+class PoolAllocator : public IPoolAllocator
+{
+    union Node
+    {
+        uint8_t data[BLOCK_SIZE];
+        Node* next;
+    };
+
+    Node* free_list_;
+    uint8_t pool_[POOL_SIZE] __attribute__((aligned(16)));  // TODO: compiler-independent alignment
+
+    // Noncopyable
+    PoolAllocator(const PoolAllocator&);
+    PoolAllocator& operator=(const PoolAllocator&);
+
+public:
+    static const int NUM_BLOCKS = int(POOL_SIZE / BLOCK_SIZE);
+
+    PoolAllocator()
+    : free_list_(reinterpret_cast<Node*>(pool_)) // TODO: alignment
+    {
+        memset(pool_, 0, POOL_SIZE);
+        for (int i = 0; i < NUM_BLOCKS - 1; i++)
+            free_list_[i].next = free_list_ + i + 1;
+        free_list_[NUM_BLOCKS - 1].next = NULL;
+    }
+
+    void* allocate(std::size_t size)
+    {
+        if (free_list_ == NULL || size > BLOCK_SIZE)
+            return NULL;
+        void* pmem = free_list_;
+        free_list_ = free_list_->next;
+        return pmem;
+    }
+
+    void deallocate(const void* ptr)
+    {
+        if (ptr == NULL)
+            return;
+        Node* p = static_cast<Node*>(const_cast<void*>(ptr));
+#if DEBUG || UAVCAN_DEBUG
+        std::memset(p, 0, sizeof(Node));
+#endif
+        p->next = free_list_;
+        free_list_ = p;
+    }
+
+    bool isInPool(const void* ptr) const
+    {
+        return
+            ptr >= pool_ &&
+            ptr < (pool_ + POOL_SIZE);
+    }
+
+    int getNumFreeBlocks() const
+    {
+        int num = 0;
+        Node* p = free_list_;
+        while (p)
+        {
+            num++;
+            assert(num <= NUM_BLOCKS);
+            p = p->next;
+        }
+        return num;
+    }
+
+    int getNumUsedBlocks() const
+    {
+        return NUM_BLOCKS - getNumFreeBlocks();
+    }
+};
+
+}

libuavcan/include/uavcan/internal/linked_list.hpp

@@ -0,0 +1,126 @@
+/*
+ * Singly-linked list.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <cstdlib>
+#include <cassert>
+
+namespace uavcan
+{
+/**
+ * Classes that are supposed to be linked-listed should derive this.
+ */
+template <typename T>
+class LinkedListNode
+{
+    T* next_;
+
+protected:
+    LinkedListNode()
+    : next_(NULL)
+    { }
+
+public:
+    T* getNextListNode() const { return next_; }
+
+    void setNextListNode(T* node)
+    {
+        next_ = node;
+    }
+};
+
+/**
+ * Linked list root.
+ */
+template <typename T>
+class LinkedListRoot
+{
+    T* root_;
+
+public:
+    LinkedListRoot()
+    : root_(NULL)
+    { }
+
+    T* get() const { return root_; }
+    bool isEmpty() const { return get() == NULL; }
+
+    unsigned int length() const
+    {
+        T* node = root_;
+        unsigned int cnt = 0;
+        while (node)
+        {
+            cnt++;
+            node = node->getNextListNode();
+        }
+        return cnt;
+    }
+
+    void insert(T* node)
+    {
+        assert(node);
+        remove(node);  // Making sure there will be no loops
+        node->setNextListNode(root_);
+        root_ = node;
+    }
+
+    /**
+     * Inserts node A immediately before the node B where
+     *  predicate(B) -> true.
+     */
+    template <typename Predicate>
+    void insertBefore(T* node, Predicate predicate)
+    {
+        assert(node);
+        remove(node);
+
+        if (root_ == NULL || predicate(root_))
+        {
+            insert(node);
+        }
+        else
+        {
+            T* p = root_;
+            while (p->getNextListNode())
+            {
+                if (predicate(p->getNextListNode()))
+                    break;
+                p = p->getNextListNode();
+            }
+            node->setNextListNode(p->getNextListNode());
+            p->setNextListNode(node);
+        }
+    }
+
+    bool remove(const T* node)
+    {
+        if (root_ == NULL || node == NULL)
+            return false;
+
+        if (root_ == node)
+        {
+            root_ = root_->getNextListNode();
+            return true;
+        }
+        else
+        {
+            T* p = root_;
+            while (p->getNextListNode())
+            {
+                if (p->getNextListNode() == node)
+                {
+                    p->setNextListNode(p->getNextListNode()->getNextListNode());
+                    return true;
+                }
+                p = p->getNextListNode();
+            }
+            return false;
+        }
+    }
+};
+
+}