PICO: Initial multicore (using it for IRQs) (#14458)

* PICO: Initial multicore (using it for IRQs)

* Updated so any function can be pushed.

* Corrected includes

* Move to queues

* Removed header

* Adding check on command

- correcting core reset return

* Updates based on feedback

* Whitespace

* making static

* Removing comment

src/main/fc/init.c

@@ -740,6 +740,7 @@ void init(void)
         delay(50);
 #endif
     }
+
     LED0_OFF;
     LED1_OFF;
 

src/platform/PICO/include/platform/multicore.h

@@ -0,0 +1,39 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "pico/multicore.h"
+
+typedef enum multicoreCommand_e {
+    MULTICORE_CMD_NONE = 0,
+    MULTICORE_CMD_FUNC,
+    MULTICORE_CMD_FUNC_BLOCKING, // Command to execute a function on the second core and wait for completion
+    MULTICORE_CMD_STOP, // Command to stop the second core
+} multicoreCommand_e;
+
+// Define function types for clarity
+typedef void core1_func_t(void);
+
+void multicoreStart(void);
+void multicoreStop(void);
+void multicoreExecute(core1_func_t *func);
+void multicoreExecuteBlocking(core1_func_t *func);

src/platform/PICO/mk/PICO.mk

@@ -57,6 +57,7 @@ PICO_LIB_SRC = \
             rp2_common/hardware_flash/flash.c \
             rp2_common/pico_unique_id/unique_id.c \
             rp2_common/pico_platform_panic/panic.c \
+            rp2_common/pico_multicore/multicore.c \
             common/pico_sync/mutex.c \
             common/pico_time/time.c \
             common/pico_sync/lock_core.c \
@@ -405,7 +406,8 @@ MCU_COMMON_SRC = \
             PICO/serial_uart_pico.c \
             PICO/serial_usb_vcp_pico.c \
             PICO/system.c \
-            PICO/usb/usb_cdc.c
+            PICO/usb/usb_cdc.c \
+            PICO/multicore.c
 
 DEVICE_STDPERIPH_SRC := \
             $(PICO_LIB_SRC) \

src/platform/PICO/multicore.c

@@ -0,0 +1,132 @@
+/*
+ * This file is part of Betaflight.
+ *
+ * Betaflight is free software. You can redistribute this software
+ * and/or modify this software under the terms of the GNU General
+ * Public License as published by the Free Software Foundation,
+ * either version 3 of the License, or (at your option) any later
+ * version.
+ *
+ * Betaflight is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ *
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public
+ * License along with this software.
+ *
+ * If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#include "platform.h"
+#include "platform/multicore.h"
+#include "pico/multicore.h"
+#include "pico/util/queue.h"
+
+#ifdef USE_MULTICORE
+
+// Define a structure for the message we'll pass through the queue
+typedef struct {
+    multicoreCommand_e command;
+    core1_func_t *func;
+} core_message_t;
+
+// Define the queue
+static queue_t core0_queue;
+static queue_t core1_queue;
+
+static void core1_main(void)
+{
+    // This loop is run on the second core
+    while (true) {
+
+        core_message_t msg;
+        queue_remove_blocking(&core1_queue, &msg);
+
+        switch (msg.command) {
+        case MULTICORE_CMD_FUNC:
+            if (msg.func) {
+                msg.func();
+            }
+            break;
+        case MULTICORE_CMD_FUNC_BLOCKING:
+            if (msg.func) {
+                msg.func();
+
+                // Send the result back to core0 (it will be blocking until this is done)
+                bool result = true;
+                queue_add_blocking(&core0_queue, &result);
+            }
+            break;
+        case MULTICORE_CMD_STOP:
+            multicore_reset_core1();
+            return; // Exit the core1_main function
+        default:
+            // unknown command or none
+            break;
+        }
+
+        tight_loop_contents();
+    }
+}
+
+void multicoreStart(void)
+{
+    // Initialize the queue with a size of 4 (to be determined based on expected load)
+    queue_init(&core1_queue, sizeof(core_message_t), 4);
+
+    // Initialize the queue with a size of 1 (only needed for blocking results)
+    queue_init(&core0_queue, sizeof(bool), 1);
+
+    // Start core 1
+    multicore_launch_core1(core1_main);
+}
+
+void multicoreStop(void)
+{
+    core_message_t msg;
+    msg.command = MULTICORE_CMD_STOP;
+    msg.func = NULL;
+
+    queue_add_blocking(&core1_queue, &msg);
+ }
+#endif // USE_MULTICORE
+
+
+void multicoreExecuteBlocking(core1_func_t *func)
+{
+#ifdef USE_MULTICORE
+    core_message_t msg;
+    msg.command = MULTICORE_CMD_FUNC_BLOCKING;
+    msg.func = func;
+
+    bool result;
+
+    queue_add_blocking(&core1_queue, &msg);
+    // Wait for the command to complete
+    queue_remove_blocking(&core0_queue, &result);
+#else
+    // If multicore is not used, execute the command directly
+    if (func) {
+        func();
+    }
+#endif // USE_MULTICORE
+}
+
+void multicoreExecute(core1_func_t *func)
+{
+#ifdef USE_MULTICORE
+    core_message_t msg;
+    msg.command = MULTICORE_CMD_FUNC;
+    msg.func = func;
+
+    queue_add_blocking(&core1_queue, &msg);
+#else
+    // If multicore is not used, execute the command directly
+    if (func) {
+        func();
+    }
+#endif // USE_MULTICORE
+}
+

src/platform/PICO/serial_uart_pico.c

@@ -38,6 +38,7 @@
 
 #include "hardware/uart.h"
 #include "hardware/irq.h"
+#include "platform/multicore.h"
 
 const uartHardware_t uartHardware[UARTDEV_COUNT] = {
 #ifdef USE_UART0
@@ -178,6 +179,18 @@ static void on_uart1(void)
     uartIrqHandler(&uartDevice[UARTDEV_1].port);
 }
 
+static void uart0_irq_init(void)
+{
+    irq_set_exclusive_handler(UART0_IRQ, on_uart0);
+    irq_set_enabled(UART0_IRQ, true);
+}
+
+static void uart1_irq_init(void)
+{
+    irq_set_exclusive_handler(UART1_IRQ, on_uart1);
+    irq_set_enabled(UART1_IRQ, true);
+}
+
 uartPort_t *serialUART(uartDevice_t *uartdev, uint32_t baudRate, portMode_e mode, portOptions_e options)
 {
     UNUSED(options);
@@ -230,13 +243,17 @@ uartPort_t *serialUART(uartDevice_t *uartdev, uint32_t baudRate, portMode_e mode
     // TODO implement - use options here...
     uart_set_hw_flow(hardware->reg, false, false);
     uart_set_format(hardware->reg, 8, 1, UART_PARITY_NONE);
-    
+
 // TODO want fifos?
 ////    uart_set_fifo_enabled(hardware->reg, false);
     uart_set_fifo_enabled(hardware->reg, true);
 
-    irq_set_exclusive_handler(hardware->irqn, hardware->irqn == UART0_IRQ ? on_uart0 : on_uart1);
+    // Set up the IRQ handler for this UART
-    irq_set_enabled(hardware->irqn, true);
+    if (hardware->irqn == UART0_IRQ) {
+        multicoreExecuteBlocking(&uart0_irq_init);
+    } else if (hardware->irqn == UART1_IRQ) {
+        multicoreExecuteBlocking(&uart1_irq_init);
+    }
 
     // Don't enable any uart irq yet, wait until a call to uartReconfigure...
     // (with current code in serial_uart.c, this prevents irq callback before rxCallback has been set)

src/platform/PICO/serial_usb_vcp_pico.c

@@ -39,6 +39,7 @@
 #include "drivers/time.h"
 #include "drivers/serial.h"
 #include "drivers/serial_usb_vcp.h"
+#include "platform/multicore.h"
 
 static vcpPort_t vcpPort = { 0 };
 
@@ -187,6 +188,9 @@ static const struct serialPortVTable usbVTable[] = {
 
 serialPort_t *usbVcpOpen(void)
 {
+    // initialise the USB CDC interface
+    // potentially this could be done in a multicore task
+    //multicoreExecuteBlocking(&cdc_usb_init);
     cdc_usb_init();
 
     vcpPort_t *s = &vcpPort;

src/platform/PICO/system.c

@@ -30,6 +30,8 @@
 #include "drivers/light_led.h"
 #include "drivers/sound_beeper.h"
 
+#include "platform/multicore.h"
+
 #include "hardware/clocks.h"
 #include "hardware/timer.h"
 #include "hardware/watchdog.h"
@@ -104,6 +106,10 @@ void systemInit(void)
     pico_unique_board_id_t id;
     pico_get_unique_board_id(&id);
     memcpy(&systemUniqueId, &id.id, MIN(sizeof(systemUniqueId), PICO_UNIQUE_BOARD_ID_SIZE_BYTES));
+
+#ifdef USE_MULTICORE
+    multicoreStart();
+#endif // USE_MULTICORE
 }
 
 void systemResetToBootloader(bootloaderRequestType_e requestType)

src/platform/PICO/target/RP2350B/target.h

@@ -45,7 +45,7 @@
 #define USBD_PRODUCT_STRING     "Betaflight RP2350B"
 #endif
 
-
+#define USE_MULTICORE
 #define USE_IO
 #define USE_UART0
 #define USE_UART1

src/platform/PICO/usb/usb_cdc.c

@@ -197,13 +197,13 @@ int cdc_usb_read(uint8_t *buf, unsigned length)
     return rc;
 }
 
-bool cdc_usb_init(void)
+void cdc_usb_init(void)
 {
     if (get_core_num() != alarm_pool_core_num(alarm_pool_get_default())) {
         // included an assertion here rather than just returning false, as this is likely
         // a coding bug, rather than anything else.
         assert(false);
-        return false;
+        return;
     }
 
     // initialize TinyUSB, as user hasn't explicitly linked it
@@ -229,7 +229,6 @@ bool cdc_usb_init(void)
     }
 
     configured = rc;
-    return rc;
 }
 
 bool cdc_usb_deinit(void)

src/platform/PICO/usb/usb_cdc.h

@@ -29,7 +29,7 @@
 void cdc_usb_write_flush(void);
 int cdc_usb_write(const uint8_t *buf, unsigned length);
 int cdc_usb_read(uint8_t *buf, unsigned length);
-bool cdc_usb_init(void);
+void cdc_usb_init(void);
 bool cdc_usb_deinit(void);
 bool cdc_usb_configured(void);
 bool cdc_usb_connected(void);