AP_HAL_Linux: allow to wakeup pollable

This allows to wakeup the thread that is sleeping on Poller::poll()
[ which in our case is an epoll_wait() call ]. This is usually achieved
by using a special signal and using the pwait() variant of the sleeping
function (or using signalfd). However integrating the signal in the
Thread class is more complex than simply use the eventfd syscall which
can serve our needs.

libraries/AP_HAL_Linux/Poller.cpp

@@ -19,6 +19,7 @@
 #include <inttypes.h>
 #include <stdio.h>
 #include <sys/epoll.h>
+#include <sys/eventfd.h>
 #include <sys/types.h>
 #include <sys/uio.h>
 #include <unistd.h>
@@ -29,12 +30,43 @@ extern const AP_HAL::HAL &hal;
 
 namespace Linux {
 
+void WakeupPollable::on_can_read()
+{
+    ssize_t r;
+    uint64_t val;
+
+    do {
+        r = read(_fd, &val, sizeof(val));
+    } while (!(r == -1 && errno == EAGAIN));
+}
+
 Poller::Poller()
 {
     _epfd = epoll_create1(EPOLL_CLOEXEC);
     if (_epfd == -1) {
         fprintf(stderr, "Failed to create epoll: %m\n");
+        return;
+    }
+
+    _wakeup._fd = eventfd(0, EFD_NONBLOCK | EFD_CLOEXEC);
+    if (_wakeup._fd == -1) {
+        fprintf(stderr, "Failed to create wakeup fd: %m\n");
+        goto fail_eventfd;
+    }
+
+    if (!register_pollable(&_wakeup, EPOLLIN)) {
+        fprintf(stderr, "Failed to add wakeup fd\n");
+        goto fail_register;
     }
+
+    return;
+
+fail_register:
+    close(_wakeup._fd);
+    _wakeup._fd = -1;
+fail_eventfd:
+    close(_epfd);
+    _epfd = -1;
 }
 
 bool Poller::register_pollable(Pollable *p, uint32_t events)
@@ -99,6 +131,20 @@ int Poller::poll() const
     return r;
 }
 
+void Poller::wakeup() const
+{
+    ssize_t r;
+    uint64_t val = 1;
+
+    do {
+        r = write(_wakeup.get_fd(), &val, sizeof(val));
+    } while (r == -1 && errno == EINTR);
+
+    if (r == -1) {
+        fprintf(stderr, "Failed to wakeup poller: %m\n");
+    }
+}
+
 Pollable::~Pollable()
 {
     /*

libraries/AP_HAL_Linux/Poller.h

@@ -60,11 +60,23 @@ protected:
     int _fd = -1;
 };
 
+/*
+ * Internal class to be used inside Poller in order to keep track of requests
+ * to wake it up
+ */
+class WakeupPollable : public Pollable {
+    friend class Poller;
+public:
+    void on_can_read() override;
+};
+
 class Poller {
 public:
     Poller();
 
     ~Poller() {
+        unregister_pollable(&_wakeup);
+
         if (_epfd >= 0) {
             close(_epfd);
         }
@@ -99,9 +111,17 @@ public:
      */
     int poll() const;
 
+    /*
+     * Wake up the thread sleeping on a poll() call if it is in fact
+     * sleeping. Otherwise a nop event is generated and handled. This is
+     * usually called from a thread different from the one calling poll().
+     */
+    void wakeup() const;
+
 private:
 
     int _epfd = -1;
+    WakeupPollable _wakeup{};
 };
 
 }