diff --git a/libuavcan/include/uavcan/transport/can_io.hpp b/libuavcan/include/uavcan/transport/can_io.hpp index 815c3735a5..927f2c2995 100644 --- a/libuavcan/include/uavcan/transport/can_io.hpp +++ b/libuavcan/include/uavcan/transport/can_io.hpp @@ -12,6 +12,7 @@ #include #include #include +#include #include #include #include @@ -87,21 +88,15 @@ private: }; LinkedListRoot queue_; - IPoolAllocator* const allocator_; - ISystemClock* const sysclock_; + LimitedPoolAllocator allocator_; + ISystemClock& sysclock_; uint32_t rejected_frames_cnt_; void registerRejectedFrame(); public: - CanTxQueue() - : allocator_(NULL) - , sysclock_(NULL) - , rejected_frames_cnt_(0) - { } - - CanTxQueue(IPoolAllocator* allocator, ISystemClock* sysclock) - : allocator_(allocator) + CanTxQueue(IPoolAllocator& allocator, ISystemClock& sysclock, std::size_t allocator_quota) + : allocator_(allocator, allocator_quota) , sysclock_(sysclock) , rejected_frames_cnt_(0) { } @@ -151,29 +146,21 @@ class UAVCAN_EXPORT CanIOManager : Noncopyable ICanDriver& driver_; ISystemClock& sysclock_; - CanTxQueue tx_queues_[MaxCanIfaces]; + LazyConstructor tx_queues_[MaxCanIfaces]; IfaceFrameCounters counters_[MaxCanIfaces]; + const uint8_t num_ifaces_; + int sendToIface(uint8_t iface_index, const CanFrame& frame, MonotonicTime tx_deadline, CanIOFlags flags); int sendFromTxQueue(uint8_t iface_index); uint8_t makePendingTxMask() const; int callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking_deadline); public: - CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock) - : driver_(driver) - , sysclock_(sysclock) - { - assert(driver.getNumIfaces() <= MaxCanIfaces); - // We can't initialize member array with non-default constructors in C++03 - for (int i = 0; i < MaxCanIfaces; i++) - { - tx_queues_[i].~CanTxQueue(); - new (tx_queues_ + i) CanTxQueue(&allocator, &sysclock); - } - } + CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, + std::size_t mem_blocks_per_iface = 0); - uint8_t getNumIfaces() const; + uint8_t getNumIfaces() const { return num_ifaces_; } CanIfacePerfCounters getIfacePerfCounters(uint8_t iface_index) const; diff --git a/libuavcan/src/transport/uc_can_io.cpp b/libuavcan/src/transport/uc_can_io.cpp index 57f7dea8b3..990d0e4345 100644 --- a/libuavcan/src/transport/uc_can_io.cpp +++ b/libuavcan/src/transport/uc_can_io.cpp @@ -106,7 +106,7 @@ void CanTxQueue::registerRejectedFrame() void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanIOFlags flags) { - const MonotonicTime timestamp = sysclock_->getMonotonic(); + const MonotonicTime timestamp = sysclock_.getMonotonic(); if (timestamp >= tx_deadline) { @@ -115,7 +115,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, return; } - void* praw = allocator_->allocate(sizeof(Entry)); + void* praw = allocator_.allocate(sizeof(Entry)); if (praw == NULL) { UAVCAN_TRACE("CanTxQueue", "Push OOM #1, cleanup"); @@ -132,7 +132,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, } p = next; } - praw = allocator_->allocate(sizeof(Entry)); // Try again + praw = allocator_.allocate(sizeof(Entry)); // Try again } if (praw == NULL) @@ -164,7 +164,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, } UAVCAN_TRACE("CanTxQueue", "Push: Replacing %s", lowestqos->toString().c_str()); remove(lowestqos); - praw = allocator_->allocate(sizeof(Entry)); // Try again + praw = allocator_.allocate(sizeof(Entry)); // Try again } if (praw == NULL) @@ -178,7 +178,7 @@ void CanTxQueue::push(const CanFrame& frame, MonotonicTime tx_deadline, Qos qos, CanTxQueue::Entry* CanTxQueue::peek() { - const MonotonicTime timestamp = sysclock_->getMonotonic(); + const MonotonicTime timestamp = sysclock_.getMonotonic(); Entry* p = queue_.get(); while (p) { @@ -206,7 +206,7 @@ void CanTxQueue::remove(Entry*& entry) return; } queue_.remove(entry); - Entry::destroy(entry, *allocator_); + Entry::destroy(entry, allocator_); } bool CanTxQueue::topPriorityHigherOrEqual(const CanFrame& rhs_frame) const @@ -247,7 +247,7 @@ int CanIOManager::sendToIface(uint8_t iface_index, const CanFrame& frame, Monoto int CanIOManager::sendFromTxQueue(uint8_t iface_index) { assert(iface_index < MaxCanIfaces); - CanTxQueue::Entry* entry = tx_queues_[iface_index].peek(); + CanTxQueue::Entry* entry = tx_queues_[iface_index]->peek(); if (entry == NULL) { return 0; @@ -255,7 +255,7 @@ int CanIOManager::sendFromTxQueue(uint8_t iface_index) const int res = sendToIface(iface_index, entry->frame, entry->deadline, entry->flags); if (res > 0) { - tx_queues_[iface_index].remove(entry); + tx_queues_[iface_index]->remove(entry); } return res; } @@ -265,7 +265,7 @@ uint8_t CanIOManager::makePendingTxMask() const uint8_t write_mask = 0; for (uint8_t i = 0; i < getNumIfaces(); i++) { - if (!tx_queues_[i].isEmpty()) + if (!tx_queues_[i]->isEmpty()) { write_mask |= 1 << i; } @@ -286,11 +286,29 @@ int CanIOManager::callSelect(CanSelectMasks& inout_masks, MonotonicTime blocking return res; } -uint8_t CanIOManager::getNumIfaces() const +CanIOManager::CanIOManager(ICanDriver& driver, IPoolAllocator& allocator, ISystemClock& sysclock, + std::size_t mem_blocks_per_iface) + : driver_(driver) + , sysclock_(sysclock) + , num_ifaces_(driver.getNumIfaces()) { - const uint8_t num = driver_.getNumIfaces(); - assert(num > 0 && num <= MaxCanIfaces); - return std::min(std::max(num, uint8_t(0)), uint8_t(MaxCanIfaces)); + if (num_ifaces_ < 1 || num_ifaces_ > MaxCanIfaces) + { + handleFatalError("Wrong number of CAN ifaces"); + } + + if (mem_blocks_per_iface == 0) + { + mem_blocks_per_iface = allocator.getNumBlocks() / (num_ifaces_ + 1) + 1; + } + UAVCAN_TRACE("CanIOManager", "Memory blocks per iface: %u, total: %u", + unsigned(mem_blocks_per_iface), unsigned(allocator.getNumBlocks())); + + for (int i = 0; i < num_ifaces_; i++) + { + tx_queues_[i].construct + (allocator, sysclock, mem_blocks_per_iface); + } } CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) const @@ -302,7 +320,7 @@ CanIfacePerfCounters CanIOManager::getIfacePerfCounters(uint8_t iface_index) con return CanIfacePerfCounters(); } CanIfacePerfCounters cnt; - cnt.errors = iface->getErrorCount() + tx_queues_[iface_index].getRejectedFrameCount(); + cnt.errors = iface->getErrorCount() + tx_queues_[iface_index]->getRejectedFrameCount(); cnt.frames_rx = counters_[iface_index].frames_rx; cnt.frames_tx = counters_[iface_index].frames_tx; return cnt; @@ -347,7 +365,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton int res = 0; if (iface_mask & (1 << i)) { - if (tx_queues_[i].topPriorityHigherOrEqual(frame)) + if (tx_queues_[i]->topPriorityHigherOrEqual(frame)) { res = sendFromTxQueue(i); // May return 0 if nothing to transmit (e.g. expired) } @@ -384,7 +402,7 @@ int CanIOManager::send(const CanFrame& frame, MonotonicTime tx_deadline, Monoton { if (iface_mask & (1 << i)) { - tx_queues_[i].push(frame, tx_deadline, qos, flags); + tx_queues_[i]->push(frame, tx_deadline, qos, flags); } } break; diff --git a/libuavcan/test/transport/can/io.cpp b/libuavcan/test/transport/can/io.cpp index 2ccceb6f9e..84e8a0e1fb 100644 --- a/libuavcan/test/transport/can/io.cpp +++ b/libuavcan/test/transport/can/io.cpp @@ -130,7 +130,7 @@ TEST(CanIOManager, Transmission) CanDriverMock driver(2, clockmock); // IO Manager - CanIOManager iomgr(driver, poolmgr, clockmock); + CanIOManager iomgr(driver, poolmgr, clockmock, 9999); ASSERT_EQ(2, iomgr.getNumIfaces()); const int ALL_IFACES_MASK = 3; diff --git a/libuavcan/test/transport/can/tx_queue.cpp b/libuavcan/test/transport/can/tx_queue.cpp index 01cc20e009..425329b0d8 100644 --- a/libuavcan/test/transport/can/tx_queue.cpp +++ b/libuavcan/test/transport/can/tx_queue.cpp @@ -73,7 +73,7 @@ TEST(CanTxQueue, TxQueue) SystemClockMock clockmock; - CanTxQueue queue(&poolmgr, &clockmock); + CanTxQueue queue(poolmgr, clockmock, 99999); EXPECT_TRUE(queue.isEmpty()); const uavcan::CanIOFlags flags = 0;