From 450871cde1d3cc3cb6eaa6665c404294d5bad765 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 22 Apr 2020 15:57:36 +1000
Subject: [PATCH] HAL_ChibiOS: added wait_pin() implementation

---
 libraries/AP_HAL_ChibiOS/GPIO.cpp | 71 ++++++++++++++++++++++++++++---
 libraries/AP_HAL_ChibiOS/GPIO.h   |  7 +++
 2 files changed, 72 insertions(+), 6 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp
index ad6326dca3..ae5e42260b 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.cpp
+++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp
@@ -30,6 +30,7 @@ static struct gpio_entry {
     AP_HAL::GPIO::irq_handler_fn_t fn; // callback for GPIO interface
     bool is_input;
     uint8_t mode;
+    thread_reference_t thd_wait;
 } _gpio_tab[] = HAL_GPIO_PINS;
 
 #define NUM_PINS ARRAY_SIZE(_gpio_tab)
@@ -219,7 +220,7 @@ bool GPIO::attach_interrupt(uint8_t pin,
     return _attach_interrupt(g->pal_line, proc, mode);
 }
 
-bool GPIO::_attach_interrupt(ioline_t line, palcallback_t cb, void *p, uint8_t mode)
+bool GPIO::_attach_interruptI(ioline_t line, palcallback_t cb, void *p, uint8_t mode)
 {
     uint32_t chmode = 0;
     switch(mode) {
@@ -239,11 +240,9 @@ bool GPIO::_attach_interrupt(ioline_t line, palcallback_t cb, void *p, uint8_t m
             break;
     }
 
-    osalSysLock();
     palevent_t *pep = pal_lld_get_line_event(line);
     if (pep->cb && p != nullptr) {
         // the pad is already being used for a callback
-        osalSysUnlock();
         return false;
     }
 
@@ -254,11 +253,18 @@ bool GPIO::_attach_interrupt(ioline_t line, palcallback_t cb, void *p, uint8_t m
     palDisableLineEventI(line);
     palSetLineCallbackI(line, cb, p);
     palEnableLineEventI(line, chmode);
-    osalSysUnlock();
 
     return true;
 }
 
+bool GPIO::_attach_interrupt(ioline_t line, palcallback_t cb, void *p, uint8_t mode)
+{
+    osalSysLock();
+    bool ret = _attach_interruptI(line, cb, p, mode);
+    osalSysUnlock();
+    return ret;
+}
+
 bool GPIO::usb_connected(void)
 {
     return _usb_connected;
@@ -288,14 +294,14 @@ void DigitalSource::toggle()
     palToggleLine(line);
 }
 
-void pal_interrupt_cb(void *arg)
+static void pal_interrupt_cb(void *arg)
 {
     if (arg != nullptr) {
         ((AP_HAL::Proc)arg)();
     }
 }
 
-void pal_interrupt_cb_functor(void *arg)
+static void pal_interrupt_cb_functor(void *arg)
 {
     const uint32_t now = AP_HAL::micros();
 
@@ -309,3 +315,56 @@ void pal_interrupt_cb_functor(void *arg)
     }
     (g->fn)(g->pin_num, palReadLine(g->pal_line), now);
 }
+
+/*
+  handle interrupt from pin change for wait_pin()
+ */
+static void pal_interrupt_wait(void *arg)
+{
+    osalSysLockFromISR();
+    struct gpio_entry *g = (gpio_entry *)arg;
+    if (g == nullptr || g->thd_wait == nullptr) {
+        osalSysUnlockFromISR();
+        return;
+    }
+    osalThreadResumeI(&g->thd_wait, MSG_OK);
+    osalSysUnlockFromISR();
+}
+
+/*
+  block waiting for a pin to change. A timeout of 0 means wait
+  forever. Return true on pin change, false on timeout
+*/
+bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_us)
+{
+    struct gpio_entry *g = gpio_by_pin_num(pin);
+    if (!g) {
+        return false;
+    }
+
+    osalSysLock();
+    if (g->thd_wait) {
+        // only allow single waiter
+        osalSysUnlock();
+        return false;
+    }
+    g->thd_wait = chThdGetSelfX();
+
+    if (!_attach_interruptI(g->pal_line,
+                           palcallback_t(pal_interrupt_wait),
+                           g,
+                           mode)) {
+        g->thd_wait = nullptr;
+        osalSysUnlock();
+        return false;
+    }
+        
+    msg_t msg = osalThreadSuspendTimeoutS(&g->thd_wait, TIME_US2I(timeout_us));
+    g->thd_wait = nullptr;
+    _attach_interruptI(g->pal_line,
+                       palcallback_t(nullptr),
+                       g,
+                       mode);
+    osalSysUnlock();
+    return msg != MSG_TIMEOUT;
+}
diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h
index 75241983e5..3e44410e3d 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.h
+++ b/libraries/AP_HAL_ChibiOS/GPIO.h
@@ -56,10 +56,17 @@ public:
     /* attach interrupt via ioline_t */
     bool _attach_interrupt(ioline_t line, AP_HAL::Proc p, uint8_t mode);
 
+    /*
+      block waiting for a pin to change. A timeout of 0 means wait
+      forever. Return true on pin change, false on timeout
+     */
+    bool wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_us) override;
+    
 private:
     bool _usb_connected;
     bool _ext_started;
 
+    bool _attach_interruptI(ioline_t line, palcallback_t cb, void *p, uint8_t mode);
     bool _attach_interrupt(ioline_t line, palcallback_t cb, void *p, uint8_t mode);
 #ifdef HAL_PIN_ALT_CONFIG
     void setup_alt_config(void);