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

libuavcan/CMakeLists.txt

@@ -13,14 +13,17 @@ project(libuavcan)
 set(CMAKE_CXX_FLAGS_RELWITHDEBINFO "-O1 -g")
 set(CMAKE_CXX_FLAGS_RELEASE "-O1 -DNDEBUG")
 set(CMAKE_CXX_FLAGS_DEBUG "-g3 -DUAVCAN_DEBUG=1")
-set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++03 -Wall -Wextra -Werror -pedantic")
+set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++03 -Wall -Wextra -Werror -pedantic -Wno-variadic-macros")
 
 include_directories(include)
 
 #
 # libuavcan
 #
-# TODO
+file(GLOB_RECURSE LIBUAVCAN_CXX_FILES RELATIVE ${CMAKE_SOURCE_DIR} "src/*.cpp")
+add_library(uavcan SHARED ${LIBUAVCAN_CXX_FILES})
+
+# TODO installation rules
 
 #
 # Test
@@ -32,14 +35,14 @@ if (GTEST_FOUND)
 
     file(GLOB_RECURSE TEST_CXX_FILES RELATIVE ${CMAKE_SOURCE_DIR} "test/*.cpp")
     add_executable(libuavcan_test ${TEST_CXX_FILES})
-    #add_dependencies(libuavcan_test libuavcan)
+    add_dependencies(libuavcan_test uavcan)
 
     set_target_properties(libuavcan_test PROPERTIES
-        COMPILE_FLAGS "-Wno-unused-parameter -Wno-unused-function"
+        COMPILE_FLAGS "-fno-rtti -fno-exceptions -Wno-unused-parameter -Wno-unused-function"
     )
 
     target_link_libraries(libuavcan_test ${GTEST_BOTH_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
-    #target_link_libraries(libuavcan_test ${CMAKE_BINARY_DIR}/libuavcan.so)
+    target_link_libraries(libuavcan_test ${CMAKE_BINARY_DIR}/libuavcan.so)
 
     add_custom_command(TARGET libuavcan_test POST_BUILD
                        COMMAND "./libuavcan_test"

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.

libuavcan/src/can_driver.cpp

@@ -0,0 +1,60 @@
+/*
+ * CAN bus driver interface.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <cstdio>
+#include <cassert>
+#include <uavcan/can_driver.hpp>
+
+namespace uavcan
+{
+
+std::string CanFrame::toString(StringRepresentation mode) const
+{
+    using std::snprintf;
+
+    assert(mode == STR_TIGHT || mode == STR_ALIGNED);
+
+    static const int ASCII_COLUMN_OFFSET = 36;
+
+    char buf[50];
+    char* wpos = buf, *epos = buf + sizeof(buf);
+    memset(buf, 0, sizeof(buf));
+
+    if (id & FLAG_EFF)
+    {
+        wpos += snprintf(wpos, epos - wpos, "0x%08x  ", (unsigned int)(id & MASK_EXTID));
+    }
+    else
+    {
+        const char* const fmt = (mode == STR_ALIGNED) ? "     0x%03x  " : "0x%03x  ";
+        wpos += snprintf(wpos, epos - wpos, fmt, (unsigned int)(id & MASK_STDID));
+    }
+
+    if (id & FLAG_RTR)
+    {
+        wpos += snprintf(wpos, epos - wpos, " RTR");
+    }
+    else
+    {
+        for (int dlen = 0; dlen < dlc; dlen++)                                 // hex bytes
+            wpos += snprintf(wpos, epos - wpos, " %02x", (unsigned int)data[dlen]);
+
+        while (mode == STR_ALIGNED && wpos < buf + ASCII_COLUMN_OFFSET)        // alignment
+            *wpos++ = ' ';
+
+        wpos += snprintf(wpos, epos - wpos, "  \'");                           // ascii
+        for (int dlen = 0; dlen < dlc; dlen++)
+        {
+            uint8_t ch = data[dlen];
+            if (ch < 0x20 || ch > 0x7E)
+                ch = '.';
+            wpos += snprintf(wpos, epos - wpos, "%c", ch);
+        }
+        wpos += snprintf(wpos, epos - wpos, "\'");
+    }
+    return std::string(buf);
+}
+
+}

libuavcan/src/can_io.cpp

@@ -0,0 +1,367 @@
+/*
+ * CAN bus IO logic.
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <algorithm>
+#include <limits>
+#include <uavcan/internal/can_io.hpp>
+#include <uavcan/internal/debug.hpp>
+
+namespace uavcan
+{
+/*
+ * CanTxQueue::Entry
+ */
+bool CanTxQueue::Entry::qosHigherThan(const CanFrame& rhs_frame, Qos rhs_qos) const
+{
+    if (qos != rhs_qos)
+        return qos > rhs_qos;
+    return frame.priorityHigherThan(rhs_frame);
+}
+
+bool CanTxQueue::Entry::qosLowerThan(const CanFrame& rhs_frame, Qos rhs_qos) const
+{
+    if (qos != rhs_qos)
+        return qos < rhs_qos;
+    return frame.priorityLowerThan(rhs_frame);
+}
+
+std::string CanTxQueue::Entry::toString() const
+{
+    std::string str_qos;
+    switch (qos)
+    {
+    case VOLATILE:
+        str_qos = "<volat> ";
+        break;
+    case PERSISTENT:
+        str_qos = "<perst> ";
+        break;
+    default:
+        assert(0);
+        str_qos = "<?WTF?> ";
+    }
+    return str_qos + frame.toString();
+}
+
+/*
+ * CanTxQueue
+ */
+CanTxQueue::~CanTxQueue()
+{
+    Entry* p = queue_.get();
+    while (p)
+    {
+        Entry* const next = p->getNextListNode();
+        remove(p);
+        p = next;
+    }
+}
+
+void CanTxQueue::registerRejectedFrame()
+{
+    if (rejected_frames_cnt_ < std::numeric_limits<uint32_t>::max())
+        rejected_frames_cnt_++;
+}
+
+void CanTxQueue::push(const CanFrame& frame, uint64_t monotonic_tx_deadline, Qos qos)
+{
+    const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
+
+    if (timestamp >= monotonic_tx_deadline)
+    {
+        UAVCAN_TRACE("CanTxQueue", "Push rejected: already expired");
+        registerRejectedFrame();
+        return;
+    }
+
+    void* praw = allocator_->allocate(sizeof(Entry));
+    if (praw == NULL)
+    {
+        UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup");
+        // No memory left in the pool, so we try to remove expired frames
+        Entry* p = queue_.get();
+        while (p)
+        {
+            Entry* const next = p->getNextListNode();
+            if (p->isExpired(timestamp))
+            {
+                UAVCAN_TRACE("CanTxQueue", "Push: Expired %s", p->toString().c_str());
+                registerRejectedFrame();
+                remove(p);
+            }
+            p = next;
+        }
+        praw = allocator_->allocate(sizeof(Entry));         // Try again
+    }
+
+    if (praw == NULL)
+    {
+        UAVCAN_TRACE("CanTxQueue", "Push OOM #2, QoS arbitration");
+        registerRejectedFrame();
+
+        // Find a frame with lowest QoS
+        Entry* p = queue_.get();
+        Entry* lowestqos = p;
+        while (p)
+        {
+            if (lowestqos->qosHigherThan(*p))
+                lowestqos = p;
+            p = p->getNextListNode();
+        }
+        // Note that frame with *equal* QoS will be replaced too.
+        if (lowestqos->qosHigherThan(frame, qos))           // Frame that we want to transmit has lowest QoS
+        {
+            UAVCAN_TRACE("CanTxQueue", "Push rejected: low QoS");
+            return;                                         // What a loser.
+        }
+        UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str());
+        remove(lowestqos);
+        praw = allocator_->allocate(sizeof(Entry));        // Try again
+    }
+
+    if (praw == NULL)
+        return;                                            // Seems that there is no memory at all.
+
+    Entry* entry = new (praw) Entry(frame, monotonic_tx_deadline, qos);
+    assert(entry);
+    queue_.insertBefore(entry, PriorityInsertionComparator(frame));
+}
+
+CanTxQueue::Entry* CanTxQueue::peek()
+{
+    const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
+    Entry* p = queue_.get();
+    while (p)
+    {
+        if (p->isExpired(timestamp))
+        {
+            UAVCAN_TRACE("CanTxQueue", "Peek: Expired %s", p->toString().c_str());
+            Entry* const next = p->getNextListNode();
+            registerRejectedFrame();
+            remove(p);
+            p = next;
+        }
+        else
+            return p;
+    }
+    return NULL;
+}
+
+void CanTxQueue::remove(Entry* entry)
+{
+    if (entry == NULL)
+    {
+        assert(0);
+        return;
+    }
+    queue_.remove(entry);
+    entry->~Entry();
+    allocator_->deallocate(entry);
+}
+
+bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const
+{
+    const Entry* entry = queue_.get();
+    if (entry == NULL)
+        return false;
+    return !rhs_frame.priorityHigherThan(entry->frame);
+}
+
+/*
+ * CanIOManager
+ */
+int CanIOManager::sendToIface(int iface_index, const CanFrame& frame, uint64_t monotonic_tx_deadline)
+{
+    assert(iface_index >= 0 && iface_index < MAX_IFACES);
+    ICanIface* const iface = driver_->getIface(iface_index);
+    if (iface == NULL)
+    {
+        assert(0);   // Nonexistent interface
+        return -1;
+    }
+    const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
+    const uint64_t timeout = (timestamp >= monotonic_tx_deadline) ? 0 : (monotonic_tx_deadline - timestamp);
+    const int res = iface->send(frame, timeout);
+    if (res != 1)
+    {
+        UAVCAN_TRACE("CanIOManager", "Send failed: code %i, iface %i, frame %s",
+                     res, iface_index, frame.toString().c_str());
+    }
+    return res;
+}
+
+int CanIOManager::sendFromTxQueue(int iface_index)
+{
+    assert(iface_index >= 0 && iface_index < MAX_IFACES);
+    CanTxQueue::Entry* const entry = tx_queues_[iface_index].peek();
+    if (entry == NULL)
+        return 0;
+    const int res = sendToIface(iface_index, entry->frame, entry->monotonic_deadline);
+    if (res > 0)
+        tx_queues_[iface_index].remove(entry);
+    return res;
+}
+
+int CanIOManager::makePendingTxMask() const
+{
+    int write_mask = 0;
+    for (int i = 0; i < getNumIfaces(); i++)
+    {
+        if (!tx_queues_[i].isEmpty())
+            write_mask |= 1 << i;
+    }
+    return write_mask;
+}
+
+uint64_t CanIOManager::getTimeUntilMonotonicDeadline(uint64_t monotonic_deadline) const
+{
+    const uint64_t timestamp = sysclock_->getMonotonicMicroseconds();
+    return (timestamp >= monotonic_deadline) ? 0 : (monotonic_deadline - timestamp);
+}
+
+int CanIOManager::getNumIfaces() const
+{
+    const int num = driver_->getNumIfaces();
+    assert(num > 0 && num <= MAX_IFACES);
+    return std::min(std::max(num, 0), (int)MAX_IFACES);
+}
+
+uint64_t CanIOManager::getNumErrors(int iface_index) const
+{
+    ICanIface* const iface = driver_->getIface(iface_index);
+    if (iface == NULL || iface_index >= MAX_IFACES || iface_index < 0)
+    {
+        assert(0);
+        return std::numeric_limits<uint64_t>::max();
+    }
+    return iface->getNumErrors() + tx_queues_[iface_index].getNumRejectedFrames();
+}
+
+int CanIOManager::send(const CanFrame& frame, uint64_t monotonic_tx_deadline, uint64_t monotonic_blocking_deadline,
+                        int iface_mask, CanTxQueue::Qos qos)
+{
+    const int num_ifaces = getNumIfaces();
+    const int all_ifaces_mask = (1 << num_ifaces) - 1;
+
+    assert((iface_mask & ~all_ifaces_mask) == 0);
+    iface_mask &= all_ifaces_mask;
+
+    if (monotonic_blocking_deadline > monotonic_tx_deadline)
+        monotonic_blocking_deadline = monotonic_tx_deadline;
+
+    int retval = 0;
+
+    while (true)
+    {
+        int write_mask = iface_mask | makePendingTxMask();
+        if (write_mask == 0)
+            break;
+
+        const uint64_t timeout = getTimeUntilMonotonicDeadline(monotonic_blocking_deadline);
+        {
+            int read_mask = 0;
+            const int select_res = driver_->select(write_mask, read_mask, timeout);
+            if (select_res < 0)
+                return select_res;
+        }
+
+        // Transmission
+        for (int i = 0; i < num_ifaces; i++)
+        {
+            if (write_mask & (1 << i))
+            {
+                int res = 0;
+                if (iface_mask & (1 << i))
+                {
+                    if (tx_queues_[i].topPriorityHigherOrEqual(frame))
+                    {
+                        res = sendFromTxQueue(i);                 // May return 0 if nothing to transmit (e.g. expired)
+                    }
+                    if (res <= 0)
+                    {
+                        res = sendToIface(i, frame, monotonic_tx_deadline);
+                        if (res > 0)
+                            iface_mask &= ~(1 << i);              // Mark transmitted
+                    }
+                }
+                else
+                {
+                    res = sendFromTxQueue(i);
+                }
+                if (res > 0)
+                    retval++;
+            }
+        }
+
+        // Timeout. Enqueue the frame if wasn't transmitted and leave.
+        if (write_mask == 0 || timeout == 0)
+        {
+            if ((timeout > 0) && (sysclock_->getMonotonicMicroseconds() < monotonic_blocking_deadline))
+            {
+                UAVCAN_TRACE("CanIOManager", "Send: Premature timeout in select(), will try again");
+                continue;
+            }
+            for (int i = 0; i < num_ifaces; i++)
+            {
+                if (iface_mask & (1 << i))
+                    tx_queues_[i].push(frame, monotonic_tx_deadline, qos);
+            }
+            break;
+        }
+    }
+    return retval;
+}
+
+int CanIOManager::receive(CanRxFrame& frame, uint64_t monotonic_deadline)
+{
+    const int num_ifaces = getNumIfaces();
+
+    while (true)
+    {
+        int write_mask = makePendingTxMask();
+        int read_mask = (1 << num_ifaces) - 1;
+        const uint64_t timeout = getTimeUntilMonotonicDeadline(monotonic_deadline);
+        {
+            const int select_res = driver_->select(write_mask, read_mask, timeout);
+            if (select_res < 0)
+                return select_res;
+        }
+
+        // Write - if buffers are not empty, one frame will be sent for each iface per one receive() call
+        for (int i = 0; i < num_ifaces; i++)
+        {
+            if (write_mask & (1 << i))
+                sendFromTxQueue(i);
+        }
+
+        // Read
+        for (int i = 0; i < num_ifaces; i++)
+        {
+            if (read_mask & (1 << i))
+            {
+                ICanIface* const iface = driver_->getIface(i);
+                if (iface == NULL)
+                {
+                    assert(0);   // Nonexistent interface
+                    continue;
+                }
+                const int res = iface->receive(frame.frame, frame.timestamp);
+                if (res == 0)
+                {
+                    assert(0);   // select() reported that iface has pending RX frames, but receive() returned none
+                    continue;
+                }
+                frame.iface_index = i;
+                return res;
+            }
+        }
+
+        if (timeout == 0)  // Timeout checked in the last order - this way we can operate with expired deadline
+            break;
+    }
+    return 0;
+}
+
+}

libuavcan/test/can/common.hpp

@@ -0,0 +1,78 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/internal/can_io.hpp>
+
+class SystemClockMock : public uavcan::ISystemClock
+{
+public:
+    uint64_t monotonic;
+    uint64_t utc;
+
+    SystemClockMock()
+    : monotonic(0)
+    , utc(0)
+    { }
+
+    void advance(uint64_t usec)
+    {
+        monotonic += usec;
+        utc += usec;
+    }
+
+    uint64_t getMonotonicMicroseconds() const
+    {
+        assert(this);
+        return monotonic;
+    }
+
+    uint64_t getUtcMicroseconds() const
+    {
+        assert(this);
+        return utc;
+    }
+
+    void adjustUtcMicroseconds(uint64_t new_timestamp_usec, int64_t offset_usec)
+    {
+        assert(0);
+    }
+};
+
+enum FrameType { STD, EXT };
+static uavcan::CanFrame makeFrame(uint32_t id, const std::string& str_data, FrameType type)
+{
+    id |= (type == EXT) ? uavcan::CanFrame::FLAG_EFF : 0;
+    return uavcan::CanFrame(id, reinterpret_cast<const uint8_t*>(str_data.c_str()), str_data.length());
+}
+
+namespace
+{
+
+int getQueueLength(uavcan::CanTxQueue& queue)
+{
+    const uavcan::CanTxQueue::Entry* p = queue.peek();
+    int length = 0;
+    while (p)
+    {
+        length++;
+        p = p->getNextListNode();
+    }
+    return length;
+}
+
+bool isInQueue(uavcan::CanTxQueue& queue, const uavcan::CanFrame& frame)
+{
+    const uavcan::CanTxQueue::Entry* p = queue.peek();
+    while (p)
+    {
+        if (frame == p->frame)
+            return true;
+        p = p->getNextListNode();
+    }
+    return false;
+}
+
+}

libuavcan/test/can/frame.cpp

@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "common.hpp"
+
+TEST(CanFrame, FrameProperties)
+{
+    EXPECT_TRUE(makeFrame(0, "", EXT).isExtended());
+    EXPECT_FALSE(makeFrame(0, "", STD).isExtended());
+    EXPECT_FALSE(makeFrame(0, "", EXT).isRemoteTransmissionRequest());
+    EXPECT_FALSE(makeFrame(0, "", STD).isRemoteTransmissionRequest());
+
+    uavcan::CanFrame frame = makeFrame(123, "", STD);
+    frame.id |= uavcan::CanFrame::FLAG_RTR;
+    EXPECT_TRUE(frame.isRemoteTransmissionRequest());
+}
+
+TEST(CanFrame, ToString)
+{
+    uavcan::CanFrame frame = makeFrame(123, "\x01\x02\x03\x04""1234", EXT);
+    EXPECT_EQ("0x0000007b   01 02 03 04 31 32 33 34  '....1234'", frame.toString());
+    EXPECT_TRUE(frame.toString() == frame.toString(uavcan::CanFrame::STR_ALIGNED));
+
+    frame = makeFrame(123, "z", EXT);
+    EXPECT_EQ("0x0000007b   7a                       'z'", frame.toString(uavcan::CanFrame::STR_ALIGNED));
+    EXPECT_EQ("0x0000007b   7a  'z'", frame.toString());
+
+    EXPECT_EQ("     0x141   61 62 63 64 aa bb cc dd  'abcd....'",
+        makeFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString(uavcan::CanFrame::STR_ALIGNED));
+
+    EXPECT_EQ("     0x100                            ''",
+        makeFrame(256, "", STD).toString(uavcan::CanFrame::STR_ALIGNED));
+
+    EXPECT_EQ("0x100    ''",
+        makeFrame(256, "", STD).toString());
+
+    EXPECT_EQ("0x141   61 62 63 64 aa bb cc dd  'abcd....'",
+        makeFrame(321, "abcd""\xaa\xbb\xcc\xdd", STD).toString());
+}

libuavcan/test/can/io.cpp

@@ -0,0 +1,446 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <queue>
+#include <vector>
+#include <gtest/gtest.h>
+#include "common.hpp"
+
+class CanIfaceMock : public uavcan::ICanIface
+{
+public:
+    struct FrameWithTime
+    {
+        uavcan::CanFrame frame;
+        uint64_t time;
+
+        FrameWithTime(const uavcan::CanFrame& frame, uint64_t time)
+        : frame(frame)
+        , time(time)
+        { }
+    };
+
+    std::queue<FrameWithTime> tx;       ///< Queue of outgoing frames (bus <-- library)
+    std::queue<FrameWithTime> rx;       ///< Queue of incoming frames (bus --> library)
+    bool writeable;
+    bool tx_failure;
+    bool rx_failure;
+    uint64_t num_errors;
+    SystemClockMock& clockmock;
+
+    CanIfaceMock(SystemClockMock& clockmock)
+    : writeable(true)
+    , tx_failure(false)
+    , rx_failure(false)
+    , num_errors(0)
+    , clockmock(clockmock)
+    { }
+
+    void pushRx(uavcan::CanFrame frame)
+    {
+        rx.push(FrameWithTime(frame, clockmock.utc));
+    }
+
+    bool matchAndPopTx(const uavcan::CanFrame& frame, uint64_t tx_deadline)
+    {
+        if (tx.empty())
+        {
+            std::cout << "Tx buffer is empty" << std::endl;
+            return false;
+        }
+        const FrameWithTime frame_time = tx.front();
+        tx.pop();
+        return (frame_time.frame == frame) && (frame_time.time == tx_deadline);
+    }
+
+    int send(const uavcan::CanFrame& frame, uint64_t tx_timeout_usec)
+    {
+        assert(this);
+        EXPECT_TRUE(writeable);        // Shall never be called when not writeable
+        if (tx_failure)
+            return -1;
+        if (!writeable)
+            return 0;
+        const uint64_t monotonic_deadline = tx_timeout_usec + clockmock.monotonic;
+        tx.push(FrameWithTime(frame, monotonic_deadline));
+        return 1;
+    }
+
+    int receive(uavcan::CanFrame& out_frame, uint64_t& out_utc_timestamp_usec)
+    {
+        assert(this);
+        EXPECT_TRUE(rx.size());        // Shall never be called when not readable
+        if (rx_failure)
+            return -1;
+        if (rx.empty())
+            return 0;
+        const FrameWithTime frame = rx.front();
+        rx.pop();
+        out_frame = frame.frame;
+        out_utc_timestamp_usec = frame.time;
+        return 1;
+    }
+
+    int configureFilters(const uavcan::CanFilterConfig* filter_configs, int num_configs) { return -1; }
+    int getNumFilters() const { return 0; }
+    uint64_t getNumErrors() const { return num_errors; }
+};
+
+class CanDriverMock : public uavcan::ICanDriver
+{
+public:
+    std::vector<CanIfaceMock> ifaces;
+    SystemClockMock& clockmock;
+    bool select_failure;
+
+    CanDriverMock(int num_ifaces, SystemClockMock& clockmock)
+    : ifaces(num_ifaces, CanIfaceMock(clockmock))
+    , clockmock(clockmock)
+    , select_failure(false)
+    { }
+
+    int select(int& inout_write_iface_mask, int& inout_read_iface_mask, uint64_t timeout_usec)
+    {
+        assert(this);
+        std::cout << "Write/read masks: " << inout_write_iface_mask << "/" << inout_read_iface_mask << std::endl;
+
+        if (select_failure)
+            return -1;
+
+        const int valid_iface_mask = (1 << getNumIfaces()) - 1;
+        EXPECT_FALSE(inout_write_iface_mask & ~valid_iface_mask);
+        EXPECT_FALSE(inout_read_iface_mask & ~valid_iface_mask);
+
+        int out_write_mask = 0;
+        int out_read_mask = 0;
+        for (int i = 0; i < getNumIfaces(); i++)
+        {
+            const int mask = 1 << i;
+            if ((inout_write_iface_mask & mask) && ifaces.at(i).writeable)
+                out_write_mask |= mask;
+            if ((inout_read_iface_mask & mask) && ifaces.at(i).rx.size())
+                out_read_mask |= mask;
+        }
+        inout_write_iface_mask = out_write_mask;
+        inout_read_iface_mask = out_read_mask;
+        if ((out_write_mask | out_read_mask) == 0)
+        {
+            clockmock.advance(timeout_usec);   // Emulating timeout
+            return 0;
+        }
+        return 1;  // This value is not being checked anyway, it just has to be greater than zero
+    }
+
+    uavcan::ICanIface* getIface(int iface_index) { return &ifaces.at(iface_index); }
+    int getNumIfaces() const { return ifaces.size(); }
+};
+
+TEST(CanIOManager, CanDriverMock)
+{
+    using uavcan::CanFrame;
+
+    SystemClockMock clockmock;
+    CanDriverMock driver(3, clockmock);
+
+    ASSERT_EQ(3, driver.getNumIfaces());
+
+    // All WR, no RD
+    int mask_wr = 7;
+    int mask_rd = 7;
+    EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100));
+    EXPECT_EQ(7, mask_wr);
+    EXPECT_EQ(0, mask_rd);
+
+    for (int i = 0; i < 3; i++)
+        driver.ifaces.at(i).writeable = false;
+
+    // No WR, no RD
+    mask_wr = 7;
+    mask_rd = 7;
+    EXPECT_EQ(0, driver.select(mask_wr, mask_rd, 100));
+    EXPECT_EQ(0, mask_wr);
+    EXPECT_EQ(0, mask_rd);
+    EXPECT_EQ(100, clockmock.monotonic);
+    EXPECT_EQ(100, clockmock.utc);
+
+    // No WR, #1 RD
+    const CanFrame fr1 = makeFrame(123, "foo", EXT);
+    driver.ifaces.at(1).pushRx(fr1);
+    mask_wr = 7;
+    mask_rd = 6;
+    EXPECT_LT(0, driver.select(mask_wr, mask_rd, 100));
+    EXPECT_EQ(0, mask_wr);
+    EXPECT_EQ(2, mask_rd);
+    CanFrame fr2;
+    uint64_t timestamp;
+    EXPECT_EQ(1, driver.getIface(1)->receive(fr2, timestamp));
+    EXPECT_EQ(fr1, fr2);
+    EXPECT_EQ(100, timestamp);
+
+    // #0 WR, #1 RD, Select failure
+    driver.ifaces.at(0).writeable = true;
+    driver.select_failure = true;
+    mask_wr = 1;
+    mask_rd = 7;
+    EXPECT_EQ(-1, driver.select(mask_wr, mask_rd, 100));
+    EXPECT_EQ(1, mask_wr);                                 // Leaving masks unchanged - library must ignore them
+    EXPECT_EQ(7, mask_rd);
+}
+
+static bool rxFrameEquals(const uavcan::CanRxFrame& rxframe, const uavcan::CanFrame& frame,
+                          uint64_t timestamp, int iface_index)
+{
+    if (rxframe.frame != frame)
+    {
+        std::cout << "Frame mismatch:\n"
+                  << "    " << rxframe.frame.toString(uavcan::CanFrame::STR_ALIGNED) << "\n"
+                  << "    " << frame.toString(uavcan::CanFrame::STR_ALIGNED) << std::endl;
+    }
+    return (rxframe.frame == frame) && (rxframe.timestamp == timestamp) && (rxframe.iface_index == iface_index);
+}
+
+TEST(CanIOManager, Reception)
+{
+    // Memory
+    uavcan::PoolAllocator<sizeof(uavcan::CanTxQueue::Entry) * 4, sizeof(uavcan::CanTxQueue::Entry)> pool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    // Platform interface
+    SystemClockMock clockmock;
+    CanDriverMock driver(2, clockmock);
+
+    // IO Manager
+    uavcan::CanIOManager iomgr(&driver, &poolmgr, &clockmock);
+    ASSERT_EQ(2, iomgr.getNumIfaces());
+
+    /*
+     * Empty, will time out
+     */
+    uavcan::CanRxFrame frame;
+    EXPECT_EQ(0, iomgr.receive(frame, 100));
+    EXPECT_EQ(100, clockmock.monotonic);
+    EXPECT_EQ(100, clockmock.utc);
+
+    /*
+     * Non empty from multiple ifaces
+     */
+    const uavcan::CanFrame frames[2][3] = {
+        { makeFrame(1, "a0", EXT),    makeFrame(99, "a1", EXT),  makeFrame(803, "a2", STD) },
+        { makeFrame(6341, "b0", EXT), makeFrame(196, "b1", STD), makeFrame(73, "b2", EXT) },
+    };
+
+    clockmock.advance(10);
+    driver.ifaces.at(0).pushRx(frames[0][0]);  // Timestamp 110
+    driver.ifaces.at(1).pushRx(frames[1][0]);
+    clockmock.advance(10);
+    driver.ifaces.at(0).pushRx(frames[0][1]);  // Timestamp 120
+    driver.ifaces.at(1).pushRx(frames[1][1]);
+    clockmock.advance(10);
+    driver.ifaces.at(0).pushRx(frames[0][2]);  // Timestamp 130
+    driver.ifaces.at(1).pushRx(frames[1][2]);
+    clockmock.advance(10);
+
+    EXPECT_EQ(1, iomgr.receive(frame, 0));
+    EXPECT_TRUE(rxFrameEquals(frame, frames[0][0], 110, 0));
+
+    EXPECT_EQ(1, iomgr.receive(frame, 0));
+    EXPECT_TRUE(rxFrameEquals(frame, frames[0][1], 120, 0));
+
+    EXPECT_EQ(1, iomgr.receive(frame, 0));
+    EXPECT_TRUE(rxFrameEquals(frame, frames[0][2], 130, 0));
+
+    EXPECT_EQ(1, iomgr.receive(frame, 0));
+    EXPECT_TRUE(rxFrameEquals(frame, frames[1][0], 110, 1));
+
+    EXPECT_EQ(1, iomgr.receive(frame, 0));
+    EXPECT_TRUE(rxFrameEquals(frame, frames[1][1], 120, 1));
+
+    EXPECT_EQ(1, iomgr.receive(frame, 0));
+    EXPECT_TRUE(rxFrameEquals(frame, frames[1][2], 130, 1));
+
+    EXPECT_EQ(0, iomgr.receive(frame, 0));  // Will time out
+
+    /*
+     * Errors
+     */
+    driver.select_failure = true;
+    EXPECT_EQ(-1, iomgr.receive(frame, 0));
+
+    driver.select_failure = false;
+    driver.ifaces.at(1).pushRx(frames[0][0]);
+    driver.ifaces.at(1).rx_failure = true;
+    EXPECT_EQ(-1, iomgr.receive(frame, 0));
+
+    driver.ifaces.at(0).num_errors = 9000;
+    driver.ifaces.at(1).num_errors = 100500;
+    EXPECT_EQ(9000, iomgr.getNumErrors(0));
+    EXPECT_EQ(100500, iomgr.getNumErrors(1));
+}
+
+TEST(CanIOManager, Transmission)
+{
+    using uavcan::CanIOManager;
+    using uavcan::CanTxQueue;
+
+    // Memory
+    typedef uavcan::PoolAllocator<sizeof(CanTxQueue::Entry) * 4, sizeof(CanTxQueue::Entry)> Pool1;
+    Pool1* ppool = new Pool1();
+    Pool1& pool = *ppool;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool);
+
+    // Platform interface
+    SystemClockMock clockmock;
+    CanDriverMock driver(2, clockmock);
+
+    // IO Manager
+    CanIOManager iomgr(&driver, &poolmgr, &clockmock);
+    ASSERT_EQ(2, iomgr.getNumIfaces());
+
+    const int ALL_IFACES_MASK = 3;
+
+    const uavcan::CanFrame frames[] = {
+        makeFrame(1, "a0", EXT),    makeFrame(99, "a1", EXT),  makeFrame(803, "a2", STD)
+    };
+
+    /*
+     * Simple transmission
+     */
+    EXPECT_EQ(2, iomgr.send(frames[0], 100, 0, ALL_IFACES_MASK, CanTxQueue::VOLATILE));  // To both
+    EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 100));
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 100));
+
+    EXPECT_EQ(1, iomgr.send(frames[1], 200, 100, 2, CanTxQueue::PERSISTENT));            // To #1
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 200));
+
+    EXPECT_EQ(0, clockmock.monotonic);
+    EXPECT_EQ(0, clockmock.utc);
+    EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+    EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+    EXPECT_EQ(0, iomgr.getNumErrors(0));
+    EXPECT_EQ(0, iomgr.getNumErrors(1));
+
+    /*
+     * TX Queue basics
+     */
+    EXPECT_EQ(0, pool.getNumUsedBlocks());
+
+    // Sending to both, #0 blocked
+    driver.ifaces.at(0).writeable = false;
+    EXPECT_LT(0, iomgr.send(frames[0], 201, 200, ALL_IFACES_MASK, CanTxQueue::PERSISTENT));
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 201));
+    EXPECT_EQ(200, clockmock.monotonic);
+    EXPECT_EQ(200, clockmock.utc);
+    EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+    EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+    EXPECT_EQ(1, pool.getNumUsedBlocks());          // One frame went into TX queue, and will expire soon
+
+    // Sending to both, both blocked
+    driver.ifaces.at(1).writeable = false;
+    EXPECT_EQ(0, iomgr.send(frames[1], 777, 300, ALL_IFACES_MASK, CanTxQueue::VOLATILE));
+    EXPECT_EQ(3, pool.getNumUsedBlocks());          // Total 3 frames in TX queue now
+
+    // Sending to #0, both blocked
+    EXPECT_EQ(0, iomgr.send(frames[2], 888, 400, 1, CanTxQueue::PERSISTENT));
+    EXPECT_EQ(400, clockmock.monotonic);
+    EXPECT_EQ(400, clockmock.utc);
+    EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+    EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+    EXPECT_EQ(4, pool.getNumUsedBlocks());
+
+    // At this time TX queues are containing the following data:
+    // iface 0: frames[0] (EXPIRED), frames[1], frames[2]
+    // iface 1: frames[1]
+
+    // Sending to #1, both writeable
+    driver.ifaces.at(0).writeable = true;
+    driver.ifaces.at(1).writeable = true;
+    EXPECT_LT(0, iomgr.send(frames[0], 999, 500, 2, CanTxQueue::PERSISTENT));
+    EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 777));   // Note that frame[0] on iface #0 has expired
+    EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 888));
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 999));   // In different order due to prioritization
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[1], 777));
+
+    // Final checks
+    ASSERT_EQ(0, driver.ifaces.at(0).tx.size());
+    ASSERT_EQ(0, driver.ifaces.at(1).tx.size());
+    EXPECT_EQ(0, pool.getNumUsedBlocks());          // Make sure the memory was properly released
+    EXPECT_EQ(1, iomgr.getNumErrors(0));            // This is because of expired frame[0]
+    EXPECT_EQ(0, iomgr.getNumErrors(1));
+
+    /*
+     * TX Queue updates from receive() call
+     */
+    driver.ifaces.at(0).writeable = false;
+    driver.ifaces.at(1).writeable = false;
+
+    // Sending 5 frames, one will be rejected
+    EXPECT_EQ(0, iomgr.send(frames[2], 2222, 1000, ALL_IFACES_MASK, CanTxQueue::PERSISTENT));
+    EXPECT_EQ(0, iomgr.send(frames[0], 3333, 1100, 2, CanTxQueue::PERSISTENT));
+    EXPECT_EQ(0, iomgr.send(frames[1], 4444, 1200, ALL_IFACES_MASK, CanTxQueue::VOLATILE));  // One frame kicked here
+
+    // State checks
+    EXPECT_EQ(4, pool.getNumUsedBlocks());          // TX queue is full
+    EXPECT_EQ(1200, clockmock.monotonic);
+    EXPECT_EQ(1200, clockmock.utc);
+    EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+    EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+
+    // Preparing the driver mock for receive() call
+    driver.ifaces.at(0).writeable = true;
+    driver.ifaces.at(1).writeable = true;
+    const uavcan::CanFrame rx_frames[] = { makeFrame(123, "rx0", STD), makeFrame(321, "rx1", EXT) };
+    driver.ifaces.at(0).pushRx(rx_frames[0]);
+    driver.ifaces.at(1).pushRx(rx_frames[1]);
+
+    // This shall transmit _some_ frames now, at least one per iface (exact number can be changed - it will be OK)
+    uavcan::CanRxFrame rx_frame;
+    EXPECT_EQ(1, iomgr.receive(rx_frame, 0));                         // Non-blocking
+    EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[0], 1200, 0));
+    EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[1], 4444));
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 3333));
+
+    EXPECT_EQ(1, iomgr.receive(rx_frame, 0));
+    EXPECT_TRUE(rxFrameEquals(rx_frame, rx_frames[1], 1200, 1));
+    EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[2], 2222));
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[2], 2222));  // Iface #1, frame[1] was rejected (VOLATILE)
+
+    // State checks
+    EXPECT_EQ(0, pool.getNumUsedBlocks());          // TX queue is empty
+    EXPECT_EQ(1200, clockmock.monotonic);
+    EXPECT_EQ(1200, clockmock.utc);
+    EXPECT_TRUE(driver.ifaces.at(0).tx.empty());
+    EXPECT_TRUE(driver.ifaces.at(1).tx.empty());
+    EXPECT_EQ(1, iomgr.getNumErrors(0));
+    EXPECT_EQ(1, iomgr.getNumErrors(1));            // This is because of rejected frame[1]
+
+    /*
+     * Error handling
+     */
+    // Select failure
+    driver.select_failure = true;
+    EXPECT_EQ(-1, iomgr.receive(rx_frame, 2000));
+    EXPECT_EQ(-1, iomgr.send(frames[0], 2100, 2000, ALL_IFACES_MASK, CanTxQueue::VOLATILE));
+    EXPECT_EQ(1200, clockmock.monotonic);
+    EXPECT_EQ(1200, clockmock.utc);
+
+    // Transmission failure
+    driver.select_failure = false;
+    driver.ifaces.at(0).writeable = true;
+    driver.ifaces.at(1).writeable = true;
+    driver.ifaces.at(0).tx_failure = true;
+    driver.ifaces.at(1).tx_failure = true;
+    EXPECT_GE(0, iomgr.send(frames[0], 2200, 0, ALL_IFACES_MASK, CanTxQueue::PERSISTENT)); // Non-blocking - return < 0
+
+    ASSERT_EQ(2, pool.getNumUsedBlocks());               // Untransmitted frames will be buffered
+
+    // Failure removed - transmission shall proceed
+    driver.ifaces.at(0).tx_failure = false;
+    driver.ifaces.at(1).tx_failure = false;
+    EXPECT_EQ(0, iomgr.receive(rx_frame, 2500));
+    EXPECT_TRUE(driver.ifaces.at(0).matchAndPopTx(frames[0], 2200));
+    EXPECT_TRUE(driver.ifaces.at(1).matchAndPopTx(frames[0], 2200));
+    EXPECT_EQ(0, pool.getNumUsedBlocks());               // All transmitted
+}

libuavcan/test/can/tx_queue.cpp

@@ -0,0 +1,178 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include "common.hpp"
+
+TEST(CanTxQueue, Qos)
+{
+    uavcan::CanTxQueue::Entry e1(makeFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE);
+    uavcan::CanTxQueue::Entry e2(makeFrame(100, "", EXT), 1000, uavcan::CanTxQueue::VOLATILE);
+
+    EXPECT_FALSE(e1.qosHigherThan(e2));
+    EXPECT_FALSE(e2.qosHigherThan(e1));
+    EXPECT_FALSE(e1.qosLowerThan(e2));
+    EXPECT_FALSE(e2.qosLowerThan(e1));
+
+    e2.qos = uavcan::CanTxQueue::PERSISTENT;
+
+    EXPECT_FALSE(e1.qosHigherThan(e2));
+    EXPECT_TRUE(e2.qosHigherThan(e1));
+    EXPECT_TRUE(e1.qosLowerThan(e2));
+    EXPECT_FALSE(e2.qosLowerThan(e1));
+
+    e1.qos = uavcan::CanTxQueue::PERSISTENT;
+    e1.frame.id -= 1;
+
+    EXPECT_TRUE(e1.qosHigherThan(e2));
+    EXPECT_FALSE(e2.qosHigherThan(e1));
+    EXPECT_FALSE(e1.qosLowerThan(e2));
+    EXPECT_TRUE(e2.qosLowerThan(e1));
+}
+
+TEST(CanTxQueue, TxQueue)
+{
+    using uavcan::CanTxQueue;
+    using uavcan::CanFrame;
+
+    ASSERT_GE(32, sizeof(CanTxQueue::Entry)); // should be true for any platforms, though not required
+
+    uavcan::PoolAllocator<32 * 4, 32> pool32;
+    uavcan::PoolManager<2> poolmgr;
+    poolmgr.addPool(&pool32);
+
+    SystemClockMock clockmock;
+
+    CanTxQueue queue(&poolmgr, &clockmock);
+    EXPECT_TRUE(queue.isEmpty());
+
+    // Descending priority
+    const CanFrame f0 = makeFrame(0, "f0", EXT);
+    const CanFrame f1 = makeFrame(10, "f1", EXT);
+    const CanFrame f2 = makeFrame(20, "f2", EXT);
+    const CanFrame f3 = makeFrame(100, "f3", EXT);
+    const CanFrame f4 = makeFrame(10000, "f4", EXT);
+    const CanFrame f5 = makeFrame(99999, "f5", EXT);
+    const CanFrame f5a = makeFrame(99999, "f5a", EXT);
+    const CanFrame f6 = makeFrame(999999, "f6", EXT);
+
+    /*
+     * Priority insertion
+     */
+    queue.push(f4, 100, CanTxQueue::PERSISTENT);
+    EXPECT_FALSE(queue.isEmpty());
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    EXPECT_EQ(f4, queue.peek()->frame);
+    EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5));
+    EXPECT_TRUE(queue.topPriorityHigherOrEqual(f4)); // Equal
+    EXPECT_FALSE(queue.topPriorityHigherOrEqual(f3));
+
+    queue.push(f3, 200, CanTxQueue::PERSISTENT);
+    EXPECT_EQ(f3, queue.peek()->frame);
+
+    queue.push(f0, 300, CanTxQueue::VOLATILE);
+    EXPECT_EQ(f0, queue.peek()->frame);
+
+    queue.push(f1, 400, CanTxQueue::VOLATILE);
+    EXPECT_EQ(f0, queue.peek()->frame);              // Still f0, since it is highest
+    EXPECT_TRUE(queue.topPriorityHigherOrEqual(f0)); // Equal
+    EXPECT_TRUE(queue.topPriorityHigherOrEqual(f1));
+
+    // Out of free memory now
+
+    EXPECT_EQ(0, queue.getNumRejectedFrames());
+    EXPECT_EQ(4, getQueueLength(queue));
+    EXPECT_TRUE(isInQueue(queue, f0));
+    EXPECT_TRUE(isInQueue(queue, f1));
+    EXPECT_TRUE(isInQueue(queue, f3));
+    EXPECT_TRUE(isInQueue(queue, f4));
+
+    const CanTxQueue::Entry* p = queue.peek();
+    while (p)
+    {
+        std::cout << p->toString() << std::endl;
+        p = p->getNextListNode();
+    }
+
+    /*
+     * QoS
+     */
+    EXPECT_FALSE(isInQueue(queue, f2));
+    queue.push(f2, 100, CanTxQueue::VOLATILE);     // Non preempting, will be rejected
+    EXPECT_FALSE(isInQueue(queue, f2));
+
+    queue.push(f2, 500, CanTxQueue::PERSISTENT);   // Will override f1 (f3 and f4 are presistent)
+    EXPECT_TRUE(isInQueue(queue, f2));
+    EXPECT_FALSE(isInQueue(queue, f1));
+    EXPECT_EQ(4, getQueueLength(queue));
+    EXPECT_EQ(2, queue.getNumRejectedFrames());
+    EXPECT_EQ(f0, queue.peek()->frame);            // Check the priority
+
+    queue.push(f5, 600, CanTxQueue::PERSISTENT);   // Will override f0 (rest are presistent)
+    EXPECT_TRUE(isInQueue(queue, f5));
+    EXPECT_FALSE(isInQueue(queue, f0));
+    EXPECT_EQ(f2, queue.peek()->frame);            // Check the priority
+
+    // No volatile frames left now
+
+    queue.push(f5a, 700, CanTxQueue::PERSISTENT);   // Will override f5 (same frame, same QoS)
+    EXPECT_TRUE(isInQueue(queue, f5a));
+    EXPECT_FALSE(isInQueue(queue, f5));
+
+    queue.push(f6, 700, CanTxQueue::PERSISTENT);    // Will be rejected (lowest QoS)
+    EXPECT_FALSE(isInQueue(queue, f6));
+
+    EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
+    EXPECT_TRUE(queue.topPriorityHigherOrEqual(f2));   // Equal
+    EXPECT_TRUE(queue.topPriorityHigherOrEqual(f5a));
+    EXPECT_EQ(4, getQueueLength(queue));
+    EXPECT_EQ(4, pool32.getNumUsedBlocks());
+    EXPECT_EQ(5, queue.getNumRejectedFrames());
+    EXPECT_TRUE(isInQueue(queue, f2));
+    EXPECT_TRUE(isInQueue(queue, f3));
+    EXPECT_TRUE(isInQueue(queue, f4));
+    EXPECT_TRUE(isInQueue(queue, f5a));
+    EXPECT_EQ(f2, queue.peek()->frame);            // Check the priority
+
+    /*
+     * Expiration
+     */
+    clockmock.monotonic = 101;
+    queue.push(f0, 800, CanTxQueue::VOLATILE);     // Will replace f4 which is expired now
+    EXPECT_TRUE(isInQueue(queue, f0));
+    EXPECT_FALSE(isInQueue(queue, f4));
+    EXPECT_EQ(6, queue.getNumRejectedFrames());
+
+    clockmock.monotonic = 1001;
+    queue.push(f5, 2000, CanTxQueue::VOLATILE);    // Entire queue is expired
+    EXPECT_TRUE(isInQueue(queue, f5));
+    EXPECT_EQ(1, getQueueLength(queue));           // Just one entry left - f5
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());       // Make sure there is no leaks
+    EXPECT_EQ(10, queue.getNumRejectedFrames());
+
+    queue.push(f0, 1000, CanTxQueue::PERSISTENT);  // This entry is already expired
+    EXPECT_EQ(1, getQueueLength(queue));
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    EXPECT_EQ(11, queue.getNumRejectedFrames());
+
+    /*
+     * Removing
+     */
+    queue.push(f4, 5000, CanTxQueue::VOLATILE);
+    EXPECT_EQ(2, getQueueLength(queue));
+    EXPECT_TRUE(isInQueue(queue, f4));
+    EXPECT_EQ(f4, queue.peek()->frame);
+
+    queue.remove(queue.peek());
+    EXPECT_FALSE(isInQueue(queue, f4));
+    EXPECT_TRUE(isInQueue(queue, f5));
+    queue.remove(queue.peek());
+    EXPECT_FALSE(isInQueue(queue, f5));
+
+    EXPECT_EQ(0, getQueueLength(queue));           // Final state checks
+    EXPECT_EQ(0, pool32.getNumUsedBlocks());
+    EXPECT_EQ(11, queue.getNumRejectedFrames());
+    EXPECT_FALSE(queue.peek());
+    EXPECT_FALSE(queue.topPriorityHigherOrEqual(f0));
+}

libuavcan/test/dynamic_memory.cpp

@@ -0,0 +1,94 @@
+/*
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include <gtest/gtest.h>
+#include <uavcan/internal/dynamic_memory.hpp>
+
+TEST(DynamicMemory, Basic)
+{
+    uavcan::PoolAllocator<128, 32> pool32;
+    uavcan::PoolAllocator<256, 64> pool64;
+    uavcan::PoolAllocator<512, 128> pool128;
+
+    EXPECT_EQ(4, pool32.getNumFreeBlocks());
+    EXPECT_EQ(4, pool64.getNumFreeBlocks());
+    EXPECT_EQ(4, pool128.getNumFreeBlocks());
+
+    uavcan::PoolManager<2> poolmgr;
+    EXPECT_TRUE(poolmgr.addPool(&pool32));
+    EXPECT_TRUE(poolmgr.addPool(&pool64));
+    EXPECT_FALSE(poolmgr.addPool(&pool128));
+
+    const void* ptr1 = poolmgr.allocate(16);
+    EXPECT_TRUE(ptr1);
+
+    const void* ptr2 = poolmgr.allocate(32);
+    EXPECT_TRUE(ptr2);
+
+    const void* ptr3 = poolmgr.allocate(64);
+    EXPECT_TRUE(ptr3);
+
+    EXPECT_FALSE(poolmgr.allocate(120));
+
+    EXPECT_EQ(2, pool32.getNumUsedBlocks());
+    EXPECT_EQ(1, pool64.getNumUsedBlocks());
+    EXPECT_EQ(0, pool128.getNumUsedBlocks());
+
+    poolmgr.deallocate(ptr1);
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    poolmgr.deallocate(ptr2);
+    EXPECT_EQ(0, pool32.getNumUsedBlocks());
+    EXPECT_EQ(1, pool64.getNumUsedBlocks());
+    poolmgr.deallocate(ptr3);
+    EXPECT_EQ(0, pool64.getNumUsedBlocks());
+    EXPECT_EQ(0, pool128.getNumUsedBlocks());
+}
+
+TEST(DynamicMemory, OutOfMemory)
+{
+    uavcan::PoolAllocator<64, 32> pool32;
+    uavcan::PoolAllocator<128, 64> pool64;
+
+    EXPECT_EQ(2, pool32.getNumFreeBlocks());
+    EXPECT_EQ(2, pool64.getNumFreeBlocks());
+
+    uavcan::PoolManager<4> poolmgr;
+    EXPECT_TRUE(poolmgr.addPool(&pool32));
+    EXPECT_TRUE(poolmgr.addPool(&pool64));
+
+    const void* ptr1 = poolmgr.allocate(32);
+    EXPECT_TRUE(ptr1);
+    EXPECT_TRUE(pool32.isInPool(ptr1));
+    EXPECT_FALSE(pool64.isInPool(ptr1));
+
+    const void* ptr2 = poolmgr.allocate(32);
+    EXPECT_TRUE(ptr2);
+    EXPECT_TRUE(pool32.isInPool(ptr2));
+    EXPECT_FALSE(pool64.isInPool(ptr2));
+
+    const void* ptr3 = poolmgr.allocate(32);
+    EXPECT_TRUE(ptr3);
+    EXPECT_FALSE(pool32.isInPool(ptr3));
+    EXPECT_TRUE(pool64.isInPool(ptr3));        // One block went to the next pool
+
+    EXPECT_EQ(2, pool32.getNumUsedBlocks());
+    EXPECT_EQ(1, pool64.getNumUsedBlocks());
+
+    poolmgr.deallocate(ptr2);
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    EXPECT_EQ(1, pool64.getNumUsedBlocks());
+
+    const void* ptr4 = poolmgr.allocate(64);
+    EXPECT_TRUE(ptr4);
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    EXPECT_EQ(2, pool64.getNumUsedBlocks());   // Top pool is 100% used
+
+    EXPECT_FALSE(poolmgr.allocate(64));        // No free blocks left --> NULL
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    EXPECT_EQ(2, pool64.getNumUsedBlocks());
+
+    poolmgr.deallocate(ptr3);                  // This was small chunk allocated in big pool
+    EXPECT_EQ(1, pool32.getNumUsedBlocks());
+    EXPECT_EQ(1, pool64.getNumUsedBlocks());   // Make sure it was properly deallocated
+}

libuavcan/test/linked_list.cpp

@@ -1,14 +1,36 @@
 /*
- * Copyright (C) 2013 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
 #include <gtest/gtest.h>
-#include <uavcan/linked_list.hpp>
+#include <uavcan/internal/linked_list.hpp>
 
 struct ListItem : uavcan::LinkedListNode<ListItem>
 {
     int value;
-    ListItem() : value(0) { }
+
+    ListItem(int value = 0)
+    : value(value)
+    { }
+
+    struct GreaterThanComparator
+    {
+        const int compare_with;
+
+        GreaterThanComparator(int compare_with)
+        : compare_with(compare_with)
+        { }
+
+        bool operator()(const ListItem* item) const
+        {
+            return item->value > compare_with;
+        }
+    };
+
+    void insort(uavcan::LinkedListRoot<ListItem>& root)
+    {
+        root.insertBefore(this, GreaterThanComparator(value));
+    }
 };
 
 TEST(LinkedList, Basic)
@@ -77,29 +99,27 @@ TEST(LinkedList, Basic)
     EXPECT_EQ(0, root.length());
 }
 
-struct Summator
-{
-    long sum;
-    Summator() : sum(0) { }
-    void operator()(const ListItem* item)
-    {
-        sum += item->value;
-    }
-};
-
-TEST(LinkedList, Predicate)
+TEST(LinkedList, Sorting)
 {
     uavcan::LinkedListRoot<ListItem> root;
-    ListItem items[5];
-    for (int i = 0 ; i < 5; i++)
-    {
-        EXPECT_FALSE(root.remove(items + i));  // Just to make sure that there's no such item
-        root.insert(items + i);
-        items[i].value = i;
-    }
-    EXPECT_EQ(5, root.length());
+    ListItem items[] = {0, 1, 2, 3, 4, 5};
 
-    Summator sum;
-    root.map(sum);
-    EXPECT_EQ(10, sum.sum);
+    items[2].insort(root);
+    items[3].insort(root);
+    items[0].insort(root);
+    items[4].insort(root);
+    items[1].insort(root);
+    items[5].insort(root);
+
+    EXPECT_EQ(6, root.length());
+
+    int prev_val = -100500;
+    const ListItem* item = root.get();
+    while (item)
+    {
+        //std::cout << item->value << std::endl;
+        EXPECT_LT(prev_val, item->value);
+        prev_val = item->value;
+        item = item->getNextListNode();
+    }
 }

libuavcan/test/test_main.cpp

@@ -1,5 +1,5 @@
 /*
- * Copyright (C) 2013 Pavel Kirienko <pavel.kirienko@gmail.com>
+ * Copyright (C) 2014 Pavel Kirienko <pavel.kirienko@gmail.com>
  */
 
 #include <gtest/gtest.h>