From eeb8b84215aa9d00fc8db958660e5c22e1b332b4 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Fri, 9 May 2014 02:17:27 +0400 Subject: [PATCH] STM32: Refactored driver; NuttX CPU ussage dropped to 7% --- .../stm32/driver/include/uavcan_stm32/can.hpp | 13 +-- .../driver/include/uavcan_stm32/thread.hpp | 22 +++-- .../stm32/driver/src/uc_stm32_thread.cpp | 98 +++++++++---------- 3 files changed, 66 insertions(+), 67 deletions(-) diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp index f1ce342917..8ce479eee6 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/can.hpp @@ -93,7 +93,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable RxQueue rx_queue_; bxcan::CanType* const can_; uavcan::uint64_t error_cnt_; - Event& update_event_; + BusEvent& update_event_; TxItem pending_tx_[NumTxMailboxes]; uavcan::uint8_t last_hw_error_code_; const uavcan::uint8_t self_index_; @@ -121,7 +121,7 @@ class CanIface : public uavcan::ICanIface, uavcan::Noncopyable public: enum { MaxRxQueueCapacity = 254 }; - CanIface(bxcan::CanType* can, Event& update_event, uavcan::uint8_t self_index, + CanIface(bxcan::CanType* can, BusEvent& update_event, uavcan::uint8_t self_index, CanRxItem* rx_queue_buffer, uavcan::uint8_t rx_queue_capacity) : rx_queue_(rx_queue_buffer, rx_queue_capacity) , can_(can) @@ -181,20 +181,19 @@ public: */ class CanDriver : public uavcan::ICanDriver, uavcan::Noncopyable { - Event update_event_; + BusEvent update_event_; CanIface if0_; #if UAVCAN_STM32_NUM_IFACES > 1 CanIface if1_; #endif - uavcan::CanSelectMasks makeSelectMasks() const; - virtual uavcan::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline); public: template CanDriver(CanRxItem (&rx_queue_storage)[UAVCAN_STM32_NUM_IFACES][RxQueueCapacity]) - : if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) + : update_event_(*this) + , if0_(bxcan::Can[0], update_event_, 0, rx_queue_storage[0], RxQueueCapacity) #if UAVCAN_STM32_NUM_IFACES > 1 , if1_(bxcan::Can[1], update_event_, 1, rx_queue_storage[1], RxQueueCapacity) #endif @@ -202,6 +201,8 @@ public: uavcan::StaticAssert<(RxQueueCapacity <= CanIface::MaxRxQueueCapacity)>::check(); } + uavcan::CanSelectMasks makeSelectMasks() const; + int init(uavcan::uint32_t bitrate); virtual CanIface* getIface(uavcan::uint8_t iface_index); diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index bd4f47787b..eb1c6358e8 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -23,14 +23,20 @@ namespace uavcan_stm32 { +class CanDriver; + #if UAVCAN_STM32_CHIBIOS -class Event +class BusEvent { chibios_rt::CounterSemaphore sem_; public: - Event() : sem_(0) { } + BusEvent(CanDriver& can_driver) + : sem_(0) + { + (void)can_driver; + } bool wait(uavcan::MonotonicDuration duration); @@ -50,33 +56,33 @@ public: #elif UAVCAN_STM32_NUTTX -class Event : uavcan::Noncopyable +class BusEvent : uavcan::Noncopyable { static const unsigned MaxPollWaiters = 8; - static const unsigned PollEvents = POLLIN | POLLOUT; ::file_operations file_ops_; ::pollfd* pollset_[MaxPollWaiters]; + CanDriver& can_driver_; bool signal_; static int openTrampoline(::file* filp); static int closeTrampoline(::file* filp); static int pollTrampoline(::file* filp, ::pollfd* fds, bool setup); - ::timespec computeDeadline(uavcan::MonotonicDuration duration); - int open(::file* filp); int close(::file* filp); int poll(::file* filp, ::pollfd* fds, bool setup); + unsigned makePollMask() const; + int addPollWaiter(::pollfd* fds); int removePollWaiter(::pollfd* fds); public: static const char* const DevName; - Event(); - ~Event(); + BusEvent(CanDriver& can_driver); + ~BusEvent(); bool wait(uavcan::MonotonicDuration duration); diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index a4f73b0048..c24885968d 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -4,6 +4,7 @@ #include #include +#include #include "internal.hpp" namespace uavcan_stm32 @@ -11,9 +12,9 @@ namespace uavcan_stm32 #if UAVCAN_STM32_CHIBIOS /* - * Event + * BusEvent */ -bool Event::wait(uavcan::MonotonicDuration duration) +bool BusEvent::wait(uavcan::MonotonicDuration duration) { static const uavcan::int64_t MaxDelayMSec = 0x000FFFFF; @@ -31,12 +32,12 @@ bool Event::wait(uavcan::MonotonicDuration duration) return ret == RDY_OK; } -void Event::signal() +void BusEvent::signal() { sem_.signal(); } -void Event::signalFromInterrupt() +void BusEvent::signalFromInterrupt() { chSysLockFromIsr(); sem_.signalI(); @@ -58,58 +59,37 @@ void Mutex::unlock() #elif UAVCAN_STM32_NUTTX -const unsigned Event::MaxPollWaiters; -const unsigned Event::PollEvents; -const char* const Event::DevName = "/dev/uavcan/busevent"; +const unsigned BusEvent::MaxPollWaiters; +const char* const BusEvent::DevName = "/dev/uavcan/busevent"; -int Event::openTrampoline(::file* filp) +int BusEvent::openTrampoline(::file* filp) { - return static_cast(filp->f_inode->i_private)->open(filp); + return static_cast(filp->f_inode->i_private)->open(filp); } -int Event::closeTrampoline(::file* filp) +int BusEvent::closeTrampoline(::file* filp) { - return static_cast(filp->f_inode->i_private)->close(filp); + return static_cast(filp->f_inode->i_private)->close(filp); } -int Event::pollTrampoline(::file* filp, ::pollfd* fds, bool setup) +int BusEvent::pollTrampoline(::file* filp, ::pollfd* fds, bool setup) { - return static_cast(filp->f_inode->i_private)->poll(filp, fds, setup); + return static_cast(filp->f_inode->i_private)->poll(filp, fds, setup); } -::timespec Event::computeDeadline(uavcan::MonotonicDuration duration) -{ - if (duration.isNegative()) - { - duration = uavcan::MonotonicDuration(); - } - ::timespec deadline = ::timespec(); - if (::clock_gettime(CLOCK_REALTIME, &deadline) >= 0) - { - deadline.tv_sec += duration.toUSec() / 1000000; - deadline.tv_nsec += (duration.toUSec() % 1000000) * 1000; - if (deadline.tv_nsec >= 1000000000L) - { - deadline.tv_sec += 1; - deadline.tv_nsec -= 1000000000L; - } - } - return deadline; -} - -int Event::open(::file* filp) +int BusEvent::open(::file* filp) { (void)filp; return 0; } -int Event::close(::file* filp) +int BusEvent::close(::file* filp) { (void)filp; return 0; } -int Event::poll(::file* filp, ::pollfd* fds, bool setup) +int BusEvent::poll(::file* filp, ::pollfd* fds, bool setup) { CriticalSectionLocker locker; int ret = -1; @@ -119,11 +99,7 @@ int Event::poll(::file* filp, ::pollfd* fds, bool setup) ret = addPollWaiter(fds); if (ret == 0) { - const unsigned poll_state = signal_ ? PollEvents : 0; - signal_ = false; - - // Update fds and signal immediately if needed - fds->revents |= fds->events & poll_state; + fds->revents |= fds->events & makePollMask(); if (fds->revents != 0) { (void)sem_post(fds->sem); @@ -138,7 +114,22 @@ int Event::poll(::file* filp, ::pollfd* fds, bool setup) return ret; } -int Event::addPollWaiter(::pollfd* fds) +unsigned BusEvent::makePollMask() const +{ + const uavcan::CanSelectMasks select_masks = can_driver_.makeSelectMasks(); + unsigned poll_mask = 0; + if (select_masks.read != 0) + { + poll_mask |= POLLIN; + } + if (select_masks.write != 0) + { + poll_mask |= POLLOUT; + } + return poll_mask; +} + +int BusEvent::addPollWaiter(::pollfd* fds) { for (unsigned i = 0; i < MaxPollWaiters; i++) { @@ -151,7 +142,7 @@ int Event::addPollWaiter(::pollfd* fds) return -ENOMEM; } -int Event::removePollWaiter(::pollfd* fds) +int BusEvent::removePollWaiter(::pollfd* fds) { for (unsigned i = 0; i < MaxPollWaiters; i++) { @@ -164,14 +155,15 @@ int Event::removePollWaiter(::pollfd* fds) return -EINVAL; } -Event::Event() - : signal_(false) +BusEvent::BusEvent(CanDriver& can_driver) + : can_driver_(can_driver) + , signal_(false) { std::memset(&file_ops_, 0, sizeof(file_ops_)); std::memset(pollset_, 0, sizeof(pollset_)); - file_ops_.open = &Event::openTrampoline; - file_ops_.close = &Event::closeTrampoline; - file_ops_.poll = &Event::pollTrampoline; + file_ops_.open = &BusEvent::openTrampoline; + file_ops_.close = &BusEvent::closeTrampoline; + file_ops_.poll = &BusEvent::pollTrampoline; // TODO: move to init(), add proper error handling if (register_driver(DevName, &file_ops_, 0666, static_cast(this)) != 0) { @@ -179,12 +171,12 @@ Event::Event() } } -Event::~Event() +BusEvent::~BusEvent() { (void)unregister_driver(DevName); } -bool Event::wait(uavcan::MonotonicDuration duration) +bool BusEvent::wait(uavcan::MonotonicDuration duration) { // TODO blocking wait const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration; @@ -203,15 +195,15 @@ bool Event::wait(uavcan::MonotonicDuration duration) return false; } -void Event::signalFromInterrupt() +void BusEvent::signalFromInterrupt() { - signal_ = true; + signal_ = true; // HACK for (unsigned i = 0; i < MaxPollWaiters; i++) { ::pollfd* const fd = pollset_[i]; if (fd != nullptr) { - fd->revents = fd->events & PollEvents; + fd->revents = fd->events & makePollMask(); if ((fd->revents != 0) && (fd->sem->semcount <= 0)) { (void)sem_post(fd->sem);