diff --git a/libraries/AP_HAL_ChibiOS/GPIO.cpp b/libraries/AP_HAL_ChibiOS/GPIO.cpp
index 15812c9b33fbdb..d2218a21ab2a81 100644
--- a/libraries/AP_HAL_ChibiOS/GPIO.cpp
+++ b/libraries/AP_HAL_ChibiOS/GPIO.cpp
@@ -241,17 +241,16 @@ uint8_t GPIO::read(uint8_t pin)
     if (g) {
         return palReadLine(g->pal_line);
     }
+#if HAL_WITH_IO_MCU
+    if (AP_BoardConfig::io_enabled() && iomcu.valid_GPIO_pin(pin)) {
+        return iomcu.read_virtual_GPIO(pin);
+    }
+#endif
     return 0;
 }
 
 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 +262,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)
 {
+    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);
-        return;
     }
 #endif
-    struct gpio_entry *g = gpio_by_pin_num(pin);
-    if (g) {
-        palToggleLine(g->pal_line);
-    }
 }
 
 /* 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 +531,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
diff --git a/libraries/AP_HAL_ChibiOS/RCOutput.cpp b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
index 0fcc96e04c4c05..da2b34fa3621b8 100644
--- a/libraries/AP_HAL_ChibiOS/RCOutput.cpp
+++ b/libraries/AP_HAL_ChibiOS/RCOutput.cpp
@@ -1263,9 +1263,13 @@ bool RCOutput::get_output_mode_banner(char banner_msg[], uint8_t banner_msg_len)
     if (iomcu_enabled) {
         uint8_t iomcu_mask;
         const output_mode iomcu_mode = iomcu.get_output_mode(iomcu_mask);
+        const uint8_t gpio_mask = iomcu.get_GPIO_mask();
         for (uint8_t i = 0; i < chan_offset; i++ ) {
-            if (iomcu_mask & 1U<<i) {
+            const uint8_t chan_bit = 1U<<i;
+            if (iomcu_mask & chan_bit) {
                 ch_mode[i] = iomcu_mode;
+            } else if (gpio_mask & chan_bit) {
+                ch_mode[i] = MODE_PWM_NONE;
             } else {
                 ch_mode[i] = MODE_PWM_NORMAL;
             }
diff --git a/libraries/AP_IOMCU/AP_IOMCU.cpp b/libraries/AP_IOMCU/AP_IOMCU.cpp
index 3507db08fad701..a228c977ae077e 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.cpp
+++ b/libraries/AP_IOMCU/AP_IOMCU.cpp
@@ -1369,6 +1369,12 @@ void AP_IOMCU::set_GPIO_mask(uint8_t mask)
     trigger_event(IOEVENT_GPIO);
 }
 
+// Get GPIO mask of channels setup for output
+uint8_t AP_IOMCU::get_GPIO_mask() const
+{
+    return GPIO.channel_mask;
+}
+
 // write to a output pin
 void AP_IOMCU::write_GPIO(uint8_t pin, bool value)
 {
@@ -1386,6 +1392,17 @@ void AP_IOMCU::write_GPIO(uint8_t pin, bool value)
     trigger_event(IOEVENT_GPIO);
 }
 
+// Read the last output value send to the GPIO pin
+// This is not a real read of the actual pin
+// This allows callers to check for state change
+uint8_t AP_IOMCU::read_virtual_GPIO(uint8_t pin) const
+{
+    if (!convert_pin_number(pin)) {
+        return 0;
+    }
+    return (GPIO.output_mask & (1U << pin)) != 0;
+}
+
 // toggle a output pin
 void AP_IOMCU::toggle_GPIO(uint8_t pin)
 {
diff --git a/libraries/AP_IOMCU/AP_IOMCU.h b/libraries/AP_IOMCU/AP_IOMCU.h
index 5199c5fb3dbdb8..5092bf26fa0ddf 100644
--- a/libraries/AP_IOMCU/AP_IOMCU.h
+++ b/libraries/AP_IOMCU/AP_IOMCU.h
@@ -157,9 +157,17 @@ class AP_IOMCU
     // set GPIO mask of channels setup for output
     void set_GPIO_mask(uint8_t mask);
 
+    // Get GPIO mask of channels setup for output
+    uint8_t get_GPIO_mask() const;
+
     // write to a output pin
     void write_GPIO(uint8_t pin, bool value);
 
+    // Read the last output value send to the GPIO pin
+    // This is not a real read of the actual pin
+    // This allows callers to check for state change
+    uint8_t read_virtual_GPIO(uint8_t pin) const;
+
     // toggle a output pin
     void toggle_GPIO(uint8_t pin);