From 60d579dc889ef56cf32aac883f16610f952ed9cf Mon Sep 17 00:00:00 2001
From: Iampete1 <iampete@hotmail.co.uk>
Date: Sun, 28 Jul 2024 17:37:47 +0100
Subject: [PATCH] AP_HAL_ChibiOS: GPIO: Check IOMCU after local pins for speed

---
 libraries/AP_HAL_ChibiOS/GPIO.cpp | 39 +++++++++++++++++--------------
 1 file changed, 21 insertions(+), 18 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp
index 15812c9b33..a210420951 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.cpp
+++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp
@@ -246,12 +246,6 @@ uint8_t GPIO::read(uint8_t pin)
 
 void GPIO::write(uint8_t pin, uint8_t value)
 {
-#if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
-        iomcu.write_GPIO(pin, value);
-        return;
-    }
-#endif
     struct gpio_entry *g = gpio_by_pin_num(pin);
     if (g) {
         if (g->is_input) {
@@ -263,36 +257,42 @@ void GPIO::write(uint8_t pin, uint8_t value)
         } else {
             palSetLine(g->pal_line);
         }
+        return;
     }
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
+        iomcu.write_GPIO(pin, value);
+    }
+#endif
 }
 
 void GPIO::toggle(uint8_t pin)
 {
-#if HAL_WITH_IO_MCU
-    if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
-        iomcu.toggle_GPIO(pin);
-        return;
-    }
-#endif
     struct gpio_entry *g = gpio_by_pin_num(pin);
     if (g) {
         palToggleLine(g->pal_line);
+        return;
     }
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
+        iomcu.toggle_GPIO(pin);
+    }
+#endif
 }
 
 /* Alternative interface: */
 AP_HAL::DigitalSource* GPIO::channel(uint16_t pin)
 {
+    struct gpio_entry *g = gpio_by_pin_num(pin);
+    if (g != nullptr) {
+        return NEW_NOTHROW DigitalSource(g->pal_line);
+    }
 #if HAL_WITH_IO_MCU
     if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
         return NEW_NOTHROW IOMCU_DigitalSource(pin);
     }
 #endif
-    struct gpio_entry *g = gpio_by_pin_num(pin);
-    if (!g) {
-        return nullptr;
-    }
-    return NEW_NOTHROW DigitalSource(g->pal_line);
+    return nullptr;
 }
 
 extern const AP_HAL::HAL& hal;
@@ -526,12 +526,15 @@ bool GPIO::wait_pin(uint8_t pin, INTERRUPT_TRIGGER_TYPE mode, uint32_t timeout_u
 // check if a pin number is valid
 bool GPIO::valid_pin(uint8_t pin) const
 {
+    if (gpio_by_pin_num(pin) != nullptr) {
+        return true;
+    }
 #if HAL_WITH_IO_MCU
     if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
         return true;
     }
 #endif
-    return gpio_by_pin_num(pin) != nullptr;
+    return false;
 }
 
 // return servo channel associated with GPIO pin.  Returns true on success and fills in servo_ch argument