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

libuavcan/include/uavcan/can_driver.hpp

@@ -1,19 +1,21 @@
 /*
  * CAN bus driver interface.
- * Copyright (C) 2013 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
 #pragma once
 
 #include <cassert>
-#include <cstdint>
+#include <stdint.h>
 #include <cstring>
+#include <string>
 
 namespace uavcan
 {
 /**
  * Raw CAN frame, as passed to/from the CAN driver.
  */
+#pragma pack(push, 1)
 struct CanFrame
 {
     static const uint32_t MASK_STDID = 0x000007FF;
@@ -25,12 +27,12 @@ struct CanFrame
     uint8_t data[8];
     uint8_t dlc;        ///< Data Length Code
 
-    Frame()
+    CanFrame()
     : id(0)
     , dlc(0)
     { }
 
-    Frame(uint32_t id, const uint8_t* data, uint8_t dlc)
+    CanFrame(uint32_t id, const uint8_t* data, unsigned int dlc)
     : id(id)
     , dlc(dlc)
     {
@@ -38,9 +40,30 @@ struct CanFrame
         std::memmove(this->data, data, dlc);
     }
 
+    bool operator!=(const CanFrame& rhs) const { return !operator==(rhs); }
+    bool operator==(const CanFrame& rhs) const
+    {
+        return (id == rhs.id) && (dlc == rhs.dlc) && (memcmp(data, rhs.data, dlc) == 0);
+    }
+
     bool isExtended() const { return id & FLAG_EFF; }
     bool isRemoteTransmissionRequest() const { return id & FLAG_RTR; }
+
+    enum StringRepresentation { STR_TIGHT, STR_ALIGNED };
+    std::string toString(StringRepresentation mode = STR_TIGHT) const;
+
+    // TODO: priority comparison for EXT vs STD frames
+    bool priorityHigherThan(const CanFrame& rhs) const
+    {
+        return (id & CanFrame::MASK_EXTID) < (rhs.id & CanFrame::MASK_EXTID);
+    }
+
+    bool priorityLowerThan(const CanFrame& rhs) const
+    {
+        return (id & CanFrame::MASK_EXTID) > (rhs.id & CanFrame::MASK_EXTID);
+    }
 };
+#pragma pack(pop)
 
 /**
  * CAN hardware filter config struct. @ref ICanDriver::filter().
@@ -64,20 +87,20 @@ public:
      * If the frame wasn't transmitted upon TX timeout expiration, the driver should discard it.
      * @return 1 = one frame transmitted, 0 = TX buffer full, negative for error.
      */
-    virtual int send(const Frame& frame, uint64_t tx_timeout_usec) = 0;
+    virtual int send(const CanFrame& frame, uint64_t tx_timeout_usec) = 0;
 
     /**
      * Non-blocking reception.
      * out_utc_timestamp_usec must be provided by the driver, ideally by the hardware CAN controller; 0 if unknown.
      * @return 1 = one frame received, 0 = RX buffer empty, negative for error.
      */
-    virtual int receive(Frame& out_frame, uint64_t& out_utc_timestamp_usec) = 0;
+    virtual int receive(CanFrame& out_frame, uint64_t& out_utc_timestamp_usec) = 0;
 
     /**
      * Configure the hardware CAN filters. @ref CanFilterConfig.
      * @return 0 = success, negative for error.
      */
-    virtual int filter(const CanFilterConfig* filter_configs, int num_configs) = 0;
+    virtual int configureFilters(const CanFilterConfig* filter_configs, int num_configs) = 0;
 
     /**
      * Number of available hardware filters.

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

@@ -1,6 +1,6 @@
 /*
  * Singly-linked list.
- * Copyright (C) 2013 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
 #pragma once
@@ -46,6 +46,7 @@ public:
     { }
 
     T* get() const { return root_; }
+    bool isEmpty() const { return get() == NULL; }
 
     unsigned int length() const
     {
@@ -61,11 +62,40 @@ public:
 
     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)
@@ -91,17 +121,6 @@ public:
             return false;
         }
     }
-
-    template <typename Fun>
-    void map(Fun& fun)
-    {
-        T* node = root_;
-        while (node)
-        {
-            fun(node);
-            node = node->getNextListNode();
-        }
-    }
 };
 
 }

libuavcan/include/uavcan/system_clock.hpp

@@ -1,11 +1,11 @@
 /*
  * System clock driver interface.
- * Copyright (C) 2013 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
 #pragma once
 
-#include <cstdint>
+#include <stdint.h>
 
 namespace uavcan
 {
@@ -17,6 +17,7 @@ namespace uavcan
  */
 class ISystemClock
 {
+public:
     /**
      * Monototic system clock in microseconds.
      * This shall never jump during UTC timestamp adjustments; the base time is irrelevant.