diff --git a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp index 58c9fae21d..bd4f47787b 100644 --- a/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp +++ b/libuavcan_drivers/stm32/driver/include/uavcan_stm32/thread.hpp @@ -7,8 +7,13 @@ #if UAVCAN_STM32_CHIBIOS # include #elif UAVCAN_STM32_NUTTX -# include -# include +# include +# include +# include +# include +# include +# include +# include #else # error "Unknown OS" #endif @@ -47,16 +52,34 @@ public: class Event : uavcan::Noncopyable { - sem_t sem_; + static const unsigned MaxPollWaiters = 8; + static const unsigned PollEvents = POLLIN | POLLOUT; + + ::file_operations file_ops_; + ::pollfd* pollset_[MaxPollWaiters]; + 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); + + int addPollWaiter(::pollfd* fds); + int removePollWaiter(::pollfd* fds); public: + static const char* const DevName; + Event(); ~Event(); bool wait(uavcan::MonotonicDuration duration); - void signal(); - void signalFromInterrupt(); }; diff --git a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp index 5d7a3382ff..a4f73b0048 100644 --- a/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp +++ b/libuavcan_drivers/stm32/driver/src/uc_stm32_thread.cpp @@ -3,6 +3,8 @@ */ #include +#include +#include "internal.hpp" namespace uavcan_stm32 { @@ -56,50 +58,166 @@ void Mutex::unlock() #elif UAVCAN_STM32_NUTTX -Event::Event() - : sem_() +const unsigned Event::MaxPollWaiters; +const unsigned Event::PollEvents; +const char* const Event::DevName = "/dev/uavcan/busevent"; + +int Event::openTrampoline(::file* filp) { - (void)sem_init(&sem_, 0, 0); + return static_cast(filp->f_inode->i_private)->open(filp); } -Event::~Event() +int Event::closeTrampoline(::file* filp) { - (void)sem_destroy(&sem_); + return static_cast(filp->f_inode->i_private)->close(filp); } -bool Event::wait(uavcan::MonotonicDuration duration) +int Event::pollTrampoline(::file* filp, ::pollfd* fds, bool 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) + ::timespec deadline = ::timespec(); + if (::clock_gettime(CLOCK_REALTIME, &deadline) >= 0) { - ASSERT(0); - return false; + 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; + } } - 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; - } - - const int result = sem_timedwait(&sem_, &deadline); - return result >= 0; + return deadline; } -void Event::signal() +int Event::open(::file* filp) { - (void)sem_post(&sem_); + (void)filp; + return 0; +} + +int Event::close(::file* filp) +{ + (void)filp; + return 0; +} + +int Event::poll(::file* filp, ::pollfd* fds, bool setup) +{ + CriticalSectionLocker locker; + int ret = -1; + + if (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; + if (fds->revents != 0) + { + (void)sem_post(fds->sem); + } + } + } + else + { + ret = removePollWaiter(fds); + } + + return ret; +} + +int Event::addPollWaiter(::pollfd* fds) +{ + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + if (pollset_[i] == nullptr) + { + pollset_[i] = fds; + return 0; + } + } + return -ENOMEM; +} + +int Event::removePollWaiter(::pollfd* fds) +{ + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + if (fds == pollset_[i]) + { + pollset_[i] = nullptr; + return 0; + } + } + return -EINVAL; +} + +Event::Event() + : 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; + // TODO: move to init(), add proper error handling + if (register_driver(DevName, &file_ops_, 0666, static_cast(this)) != 0) + { + std::abort(); + } +} + +Event::~Event() +{ + (void)unregister_driver(DevName); +} + +bool Event::wait(uavcan::MonotonicDuration duration) +{ + // TODO blocking wait + const uavcan::MonotonicTime deadline = clock::getMonotonic() + duration; + while (clock::getMonotonic() < deadline) + { + { + CriticalSectionLocker locker; + if (signal_) + { + signal_ = false; + return true; + } + } + ::usleep(1000); + } + return false; } void Event::signalFromInterrupt() { - (void)sem_post(&sem_); // Can be called from ISR http://nuttx.org/Documentation/NuttxUserGuide.html#sempost + signal_ = true; + for (unsigned i = 0; i < MaxPollWaiters; i++) + { + ::pollfd* const fd = pollset_[i]; + if (fd != nullptr) + { + fd->revents = fd->events & PollEvents; + if ((fd->revents != 0) && (fd->sem->semcount <= 0)) + { + (void)sem_post(fd->sem); + } + } + } } #endif