diff --git a/libuavcan/CMakeLists.txt b/libuavcan/CMakeLists.txt index d40f34abee..35da2b8f79 100644 --- a/libuavcan/CMakeLists.txt +++ b/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" diff --git a/libuavcan/include/uavcan/can_driver.hpp b/libuavcan/include/uavcan/can_driver.hpp index 97db0a14bb..ccc47c91c7 100644 --- a/libuavcan/include/uavcan/can_driver.hpp +++ b/libuavcan/include/uavcan/can_driver.hpp @@ -1,19 +1,21 @@ /* * CAN bus driver interface. - * Copyright (C) 2013 Pavel Kirienko + * Copyright (C) 2014 Pavel Kirienko */ #pragma once #include -#include +#include #include +#include 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. diff --git a/libuavcan/include/uavcan/internal/can_io.hpp b/libuavcan/include/uavcan/internal/can_io.hpp new file mode 100644 index 0000000000..c56c3fd69c --- /dev/null +++ b/libuavcan/include/uavcan/internal/can_io.hpp @@ -0,0 +1,161 @@ +/* + * CAN bus IO logic. + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +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 + { + 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 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); +}; + +} diff --git a/libuavcan/include/uavcan/internal/debug.hpp b/libuavcan/include/uavcan/internal/debug.hpp new file mode 100644 index 0000000000..ad77034935 --- /dev/null +++ b/libuavcan/include/uavcan/internal/debug.hpp @@ -0,0 +1,30 @@ +/* + * Debug stuff, should only be used for library development. + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#if UAVCAN_DEBUG + +#include +#include + +#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 diff --git a/libuavcan/include/uavcan/internal/dynamic_memory.hpp b/libuavcan/include/uavcan/internal/dynamic_memory.hpp new file mode 100644 index 0000000000..2188a52f7a --- /dev/null +++ b/libuavcan/include/uavcan/internal/dynamic_memory.hpp @@ -0,0 +1,166 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include + +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 +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 +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(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(const_cast(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(); + } +}; + +} diff --git a/libuavcan/include/uavcan/linked_list.hpp b/libuavcan/include/uavcan/internal/linked_list.hpp similarity index 67% rename from libuavcan/include/uavcan/linked_list.hpp rename to libuavcan/include/uavcan/internal/linked_list.hpp index 6390067229..ce7b1cbc9c 100644 --- a/libuavcan/include/uavcan/linked_list.hpp +++ b/libuavcan/include/uavcan/internal/linked_list.hpp @@ -1,6 +1,6 @@ /* * Singly-linked list. - * Copyright (C) 2013 Pavel Kirienko + * Copyright (C) 2014 Pavel Kirienko */ #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 + 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 - void map(Fun& fun) - { - T* node = root_; - while (node) - { - fun(node); - node = node->getNextListNode(); - } - } }; } diff --git a/libuavcan/include/uavcan/system_clock.hpp b/libuavcan/include/uavcan/system_clock.hpp index 1775ded228..c5fb62db28 100644 --- a/libuavcan/include/uavcan/system_clock.hpp +++ b/libuavcan/include/uavcan/system_clock.hpp @@ -1,11 +1,11 @@ /* * System clock driver interface. - * Copyright (C) 2013 Pavel Kirienko + * Copyright (C) 2014 Pavel Kirienko */ #pragma once -#include +#include 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. diff --git a/libuavcan/src/can_driver.cpp b/libuavcan/src/can_driver.cpp new file mode 100644 index 0000000000..02389615e9 --- /dev/null +++ b/libuavcan/src/can_driver.cpp @@ -0,0 +1,60 @@ +/* + * CAN bus driver interface. + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +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); +} + +} diff --git a/libuavcan/src/can_io.cpp b/libuavcan/src/can_io.cpp new file mode 100644 index 0000000000..7bc08d0001 --- /dev/null +++ b/libuavcan/src/can_io.cpp @@ -0,0 +1,367 @@ +/* + * CAN bus IO logic. + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#include + +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 = " "; + break; + case PERSISTENT: + str_qos = " "; + break; + default: + assert(0); + str_qos = " "; + } + 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::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::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; +} + +} diff --git a/libuavcan/test/can/common.hpp b/libuavcan/test/can/common.hpp new file mode 100644 index 0000000000..1808f3a279 --- /dev/null +++ b/libuavcan/test/can/common.hpp @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include + +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(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; +} + +} diff --git a/libuavcan/test/can/frame.cpp b/libuavcan/test/can/frame.cpp new file mode 100644 index 0000000000..22ce460da6 --- /dev/null +++ b/libuavcan/test/can/frame.cpp @@ -0,0 +1,41 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#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()); +} diff --git a/libuavcan/test/can/io.cpp b/libuavcan/test/can/io.cpp new file mode 100644 index 0000000000..ad36fe9d68 --- /dev/null +++ b/libuavcan/test/can/io.cpp @@ -0,0 +1,446 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include +#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 tx; ///< Queue of outgoing frames (bus <-- library) + std::queue 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 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 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 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 +} diff --git a/libuavcan/test/can/tx_queue.cpp b/libuavcan/test/can/tx_queue.cpp new file mode 100644 index 0000000000..6386dcad86 --- /dev/null +++ b/libuavcan/test/can/tx_queue.cpp @@ -0,0 +1,178 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#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)); +} diff --git a/libuavcan/test/dynamic_memory.cpp b/libuavcan/test/dynamic_memory.cpp new file mode 100644 index 0000000000..3ff4008ca0 --- /dev/null +++ b/libuavcan/test/dynamic_memory.cpp @@ -0,0 +1,94 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include + +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 +} diff --git a/libuavcan/test/linked_list.cpp b/libuavcan/test/linked_list.cpp index d802f95ec5..a6a59c791d 100644 --- a/libuavcan/test/linked_list.cpp +++ b/libuavcan/test/linked_list.cpp @@ -1,14 +1,36 @@ /* - * Copyright (C) 2013 Pavel Kirienko + * Copyright (C) 2014 Pavel Kirienko */ #include -#include +#include struct ListItem : uavcan::LinkedListNode { 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& 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 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(); + } } diff --git a/libuavcan/test/test_main.cpp b/libuavcan/test/test_main.cpp index fc18353571..cd16e1fb59 100644 --- a/libuavcan/test/test_main.cpp +++ b/libuavcan/test/test_main.cpp @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Pavel Kirienko + * Copyright (C) 2014 Pavel Kirienko */ #include