From 0455a0278803ceba4e085c395e76d096eb25b4b2 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Tue, 25 Jun 2024 17:10:56 +1000
Subject: [PATCH] AP_HAL_ChibiOS: add gpio attach interrupt with 64bit micros
 time in the callback

---
 libraries/AP_HAL_ChibiOS/GPIO.cpp | 47 ++++++++++++++++++++++++++-----
 libraries/AP_HAL_ChibiOS/GPIO.h   |  4 +++
 2 files changed, 44 insertions(+), 7 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp
index f62bd73c27bfd2..166283f9f10228 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.cpp
+++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp
@@ -44,6 +44,7 @@ struct gpio_entry {
     uint8_t pwm_num;
     ioline_t pal_line;
     AP_HAL::GPIO::irq_handler_fn_t fn; // callback for GPIO interface
+    AP_HAL::GPIO::irq_handler_64_fn_t fn_64; // callback for GPIO interface
     thread_reference_t thd_wait;
     bool is_input;
     uint8_t mode;
@@ -318,6 +319,29 @@ bool GPIO::attach_interrupt(uint8_t pin,
         return false;
     }
     g->fn = fn;
+    g->fn_64 = nullptr;
+    g->isr_mode = mode;
+    return true;
+}
+
+bool GPIO::attach_interrupt(uint8_t pin,
+                            irq_handler_64_fn_t fn_64,
+                            INTERRUPT_TRIGGER_TYPE mode)
+{
+    struct gpio_entry *g = gpio_by_pin_num(pin, false);
+    if (!g) {
+        return false;
+    }
+    g->isr_disabled_ticks = 0;
+    g->isr_quota = 0;
+    if (!_attach_interrupt(g->pal_line,
+                           palcallback_t(fn_64?pal_interrupt_cb_functor:nullptr),
+                           g,
+                           mode)) {
+        return false;
+    }
+    g->fn = nullptr;
+    g->fn_64 = fn_64;
     g->isr_mode = mode;
     return true;
 }
@@ -442,14 +466,19 @@ static void pal_interrupt_cb(void *arg)
 
 static void pal_interrupt_cb_functor(void *arg)
 {
-    const uint32_t now = AP_HAL::micros();
-
     struct gpio_entry *g = (gpio_entry *)arg;
     if (g == nullptr) {
         // what?
         return;
     }
-    if (!(g->fn)) {
+    uint64_t now;
+    if (g->fn_64) {
+        now = AP_HAL::micros64();
+    } else {
+        now = AP_HAL::micros();
+    }
+
+    if (!(g->fn) && !(g->fn_64)) {
         return;
     }
     if (g->isr_quota >= 1) {
@@ -467,7 +496,11 @@ static void pal_interrupt_cb_functor(void *arg)
         }
         g->isr_quota--;
     }
-    (g->fn)(g->pin_num, palReadLine(g->pal_line), now);
+    if (g->fn) {
+        (g->fn)(g->pin_num, palReadLine(g->pal_line), now);
+    } else if (g->fn_64) {
+        (g->fn_64)(g->pin_num, palReadLine(g->pal_line), now);
+    }
 }
 
 /*
@@ -634,15 +667,15 @@ void GPIO::timer_tick()
 
         // 100 * 100ms = 10 seconds
         const uint8_t ISR_retry_ticks = 100U;
-        if ((_gpio_tab[i].isr_disabled_ticks > ISR_retry_ticks) && (_gpio_tab[i].fn != nullptr)) {
+        if ((_gpio_tab[i].isr_disabled_ticks > ISR_retry_ticks) && (_gpio_tab[i].fn != nullptr && _gpio_tab[i].fn_64 != nullptr)) {
             // Try re-enabling
 #ifndef HAL_NO_UARTDRIVER
             GCS_SEND_TEXT(MAV_SEVERITY_NOTICE, "Retrying pin %d after ISR flood", _gpio_tab[i].pin_num);
 #endif
-            if (attach_interrupt(_gpio_tab[i].pin_num, _gpio_tab[i].fn, _gpio_tab[i].isr_mode)) {
+            if ((_gpio_tab[i].fn != nullptr) && attach_interrupt(_gpio_tab[i].pin_num, _gpio_tab[i].fn, _gpio_tab[i].isr_mode)) {
                 // Success, reset quota
                 _gpio_tab[i].isr_quota = quota;
-            } else {
+            } else if ((_gpio_tab[i].fn_64 != nullptr) && attach_interrupt(_gpio_tab[i].pin_num, _gpio_tab[i].fn_64, _gpio_tab[i].isr_mode)) {
                 // Failed, reset disabled count to try again later
                 _gpio_tab[i].isr_disabled_ticks = 1;
             }
diff --git a/libraries/AP_HAL_ChibiOS/GPIO.h b/libraries/AP_HAL_ChibiOS/GPIO.h
index cb978cb1b62ed4..d03baf07b7c514 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.h
+++ b/libraries/AP_HAL_ChibiOS/GPIO.h
@@ -52,6 +52,10 @@ class ChibiOS::GPIO : public AP_HAL::GPIO {
                              irq_handler_fn_t fn,
                              INTERRUPT_TRIGGER_TYPE mode) override;
 
+    bool    attach_interrupt(uint8_t pin,
+                             irq_handler_64_fn_t fn,
+                             INTERRUPT_TRIGGER_TYPE mode) override;
+
     /* return true if USB cable is connected */
     bool    usb_connected(void) override;