STM32: Implemented event device /dev/uavcan/busevent for multiplexing via poll()

2026-10-13 04:33:33 +08:00 · 2014-05-08 16:45:49 +04:00
parent 09ac68bffd
commit 6be5246101
2 changed files with 170 additions and 29 deletions
@@ -7,8 +7,13 @@
 #if UAVCAN_STM32_CHIBIOS
 # include <ch.hpp>
 #elif UAVCAN_STM32_NUTTX
-# include <semaphore.h>
-# include <time.h>
+# include <nuttx/config.h>
+# include <nuttx/fs/fs.h>
+# include <poll.h>
+# include <errno.h>
+# include <cstdio>
+# include <ctime>
+# include <cstring>
 #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();
 };

@@ -3,6 +3,8 @@
 */

 #include <uavcan_stm32/thread.hpp>
+#include <uavcan_stm32/clock.hpp>
+#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<Event*>(filp->f_inode->i_private)->open(filp);
 }

-Event::~Event()
+int Event::closeTrampoline(::file* filp)
 {
-    (void)sem_destroy(&sem_);
+    return static_cast<Event*>(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<Event*>(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<void*>(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