From da65a5c34981a6edf08c6846eb5fa5b020f33713 Mon Sep 17 00:00:00 2001
From: Lucas De Marchi <lucas.demarchi@intel.com>
Date: Tue, 25 Oct 2016 15:48:12 -0200
Subject: [PATCH] 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 | 46 +++++++++++++++++++++++++++++++
 libraries/AP_HAL_Linux/Poller.h   | 20 ++++++++++++++
 2 files changed, 66 insertions(+)

diff --git a/libraries/AP_HAL_Linux/Poller.cpp b/libraries/AP_HAL_Linux/Poller.cpp
index bd4aa908f0..22b52eadc9 100644
--- a/libraries/AP_HAL_Linux/Poller.cpp
+++ b/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()
 {
     /*
diff --git a/libraries/AP_HAL_Linux/Poller.h b/libraries/AP_HAL_Linux/Poller.h
index a06ddf22f4..0dc096be40 100644
--- a/libraries/AP_HAL_Linux/Poller.h
+++ b/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{};
 };
 
 }