diff --git a/src/platform/PICO/exti_pico.c b/src/platform/PICO/exti_pico.c
index 54be2ba5f9..17aeff8b74 100644
--- a/src/platform/PICO/exti_pico.c
+++ b/src/platform/PICO/exti_pico.c
@@ -19,22 +19,70 @@
* If not, see .
*/
+#include
#include "drivers/exti.h"
#include "common/utils.h"
+#include "drivers/io_impl.h"
+
+#include "hardware/gpio.h"
+
+typedef struct {
+ extiCallbackRec_t* handler;
+} extiChannelRec_t;
+
+static extiChannelRec_t extiChannelRecs[DEFIO_USED_COUNT];
+static uint32_t extiEventMask[DEFIO_USED_COUNT];
void EXTIConfig(IO_t io, extiCallbackRec_t *cb, int irqPriority, ioConfig_t config, extiTrigger_t trigger)
{
- UNUSED(io);
- UNUSED(cb);
- UNUSED(irqPriority);
- UNUSED(config);
- UNUSED(trigger);
+ uint32_t gpio = IO_Pin(io);
+
+ UNUSED(irqPriority); // Just stick with default GPIO irq priority for now
+ UNUSED(config); // TODO consider pullup/pulldown etc. Needs fixing first in platform.h
+
+ // Ensure the GPIO is initialised and not being used for some other function
+ gpio_init(gpio);
+
+ extiChannelRec_t *rec = &extiChannelRecs[gpio];
+ rec->handler = cb;
+
+ switch(trigger) {
+ case BETAFLIGHT_EXTI_TRIGGER_RISING:
+ default:
+ extiEventMask[gpio] = GPIO_IRQ_EDGE_RISE;
+ break;
+
+ case BETAFLIGHT_EXTI_TRIGGER_FALLING:
+ extiEventMask[gpio] = GPIO_IRQ_EDGE_FALL;
+ break;
+
+ case BETAFLIGHT_EXTI_TRIGGER_BOTH:
+ extiEventMask[gpio] = GPIO_IRQ_EDGE_RISE | GPIO_IRQ_EDGE_FALL;
+ break;
+ }
+}
+
+static void EXTI_IRQHandler(uint gpio, uint32_t event_mask)
+{
+ // Call the registered handler for this GPIO
+ if (extiChannelRecs[gpio].handler) {
+ extiChannelRecs[gpio].handler->fn(extiChannelRecs[gpio].handler);
+ }
+
+ // Acknowledge the interrupt
+ gpio_acknowledge_irq(gpio, event_mask);
}
void EXTIInit(void)
{
- //TODO: implement
- // NOOP
+ // Clear all the callbacks
+ memset(extiChannelRecs, 0, sizeof(extiChannelRecs));
+
+ // Register the shared handler for GPIO interrupts
+ gpio_set_irq_callback(EXTI_IRQHandler);
+
+ // Enable the interrupt
+ irq_set_enabled(IO_IRQ_BANK0, true);
}
void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn)
@@ -44,6 +92,10 @@ void EXTIHandlerInit(extiCallbackRec_t *self, extiHandlerCallback *fn)
void EXTIEnable(IO_t io)
{
- //TODO: implement
- UNUSED(io);
+ gpio_set_irq_enabled(IO_Pin(io), extiEventMask[IO_Pin(io)], true);
+}
+
+void EXTIDisable(IO_t io)
+{
+ gpio_set_irq_enabled(IO_Pin(io), 0, false);
}
diff --git a/src/platform/PICO/target/RP2350B/target.h b/src/platform/PICO/target/RP2350B/target.h
index 202c60e637..5ccfa41420 100644
--- a/src/platform/PICO/target/RP2350B/target.h
+++ b/src/platform/PICO/target/RP2350B/target.h
@@ -45,7 +45,7 @@
#define USBD_PRODUCT_STRING "Betaflight RP2350B"
#endif
-#define USE_MULTICORE
+//#define USE_MULTICORE
#define USE_IO
#define USE_UART0
#define USE_UART1