Use real FreeRTOS tasks to idle core during flash (#1444)

Fixes #1439

Use a real FreeRTOS task, at the highest priority, to idle the other core
while doing flash accesses.

The USB stack seems to have some timing dependent bits which get broken
if FreeRTOS switches out the current task when it's running.  Avoid any
issue by disabling preemption on the core for all tud_task calls.

The PendSV handler disable flag can live completely inside the FreeRTOS
variant port, so remove any reference to it in the main core.

Tested using Multicode-FreeRTOS, #1402 and #1441

The USB FIFO interrupts were still being serviced even when the core was
frozen, leading to crashes.  Explicitly shut off IRQs on both the victim
and the initiator core when freezing.

Removed the need for hack __holdUpPendSV flag

cores/rp2040/RP2040Support.h

@@ -91,15 +91,14 @@ public:
         if (!_multicore) {
             return;
         }
-        __holdUpPendSV = 1;
         if (__isFreeRTOS) {
-            vTaskPreemptionDisable(nullptr);
-            vTaskSuspendAll();
+            __freertos_idle_other_core();
+        } else {
+            mutex_enter_blocking(&_idleMutex);
+            __otherCoreIdled = false;
+            multicore_fifo_push_blocking(_GOTOSLEEP);
+            while (!__otherCoreIdled) { /* noop */ }
         }
-        mutex_enter_blocking(&_idleMutex);
-        __otherCoreIdled = false;
-        multicore_fifo_push_blocking(_GOTOSLEEP);
-        while (!__otherCoreIdled) { /* noop */ }
     }
 
     void resumeOtherCore() {
@@ -109,10 +108,8 @@ public:
         mutex_exit(&_idleMutex);
         __otherCoreIdled = false;
         if (__isFreeRTOS) {
-            xTaskResumeAll();
-            vTaskPreemptionEnable(nullptr);
+            __freertos_resume_other_core();
         }
-        __holdUpPendSV = 0;
 
         // Other core will exit busy-loop and return to operation
         // once __otherCoreIdled == false.

cores/rp2040/SerialUSB.cpp

@@ -177,8 +177,10 @@ static bool _rts = false;
 static int _bps = 115200;
 static bool _rebooting = false;
 static void CheckSerialReset() {
-    __holdUpPendSV = 1; // Ensure we don't get swapped out by FreeRTOS
     if (!_rebooting && (_bps == 1200) && (!_dtr)) {
+        if (__isFreeRTOS) {
+            vTaskPreemptionDisable(nullptr);
+        }
         _rebooting = true;
         // Disable NVIC IRQ, so that we don't get bothered anymore
         irq_set_enabled(USBCTRL_IRQ, false);
@@ -190,7 +192,6 @@ static void CheckSerialReset() {
         reset_usb_boot(0, 0);
         while (1); // WDT will fire here
     }
-    __holdUpPendSV = 0;
 }
 
 extern "C" void tud_cdc_line_state_cb(uint8_t itf, bool dtr, bool rts) {

cores/rp2040/_freertos.h

@@ -59,8 +59,7 @@ extern "C" {
     extern void vTaskPreemptionEnable(TaskHandle_t p) __attribute__((weak));
 #endif
 
+    extern void __freertos_idle_other_core() __attribute__((weak));
+    extern void __freertos_resume_other_core() __attribute__((weak));
 }
 extern SemaphoreHandle_t __get_freertos_mutex_for_ptr(mutex_t *m, bool recursive = false);
-
-// Halt the FreeRTOS PendSV task switching magic
-extern "C" int __holdUpPendSV;

cores/rp2040/main.cpp

@@ -27,7 +27,6 @@
 RP2040 rp2040;
 extern "C" {
     volatile bool __otherCoreIdled = false;
-    int __holdUpPendSV = 0;
 };
 
 mutex_t _pioMutex;

libraries/FreeRTOS/src/variantHooks.cpp

@@ -40,7 +40,54 @@
 #include <pico.h>
 #include <pico/time.h>
 
-#include "_freertos.h"
+#include <_freertos.h>
+
+// Interfaces for the main core to use FreeRTOS mutexes
+extern "C" {
+    extern volatile bool __otherCoreIdled;
+    int __holdUpPendSV = 0; // TOTO - remove from FreeRTOS lib
+
+    SemaphoreHandle_t __freertos_mutex_create() {
+        return xSemaphoreCreateMutex();
+    }
+
+    SemaphoreHandle_t _freertos_recursive_mutex_create() {
+        return xSemaphoreCreateRecursiveMutex();
+    }
+
+    void __freertos_mutex_take(SemaphoreHandle_t mtx) {
+        xSemaphoreTake(mtx, portMAX_DELAY);
+    }
+
+    void __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) {
+        xSemaphoreTakeFromISR(mtx, NULL);
+    }
+
+    int __freertos_mutex_try_take(SemaphoreHandle_t mtx) {
+        return xSemaphoreTake(mtx, 0);
+    }
+
+    void __freertos_mutex_give(SemaphoreHandle_t mtx) {
+        xSemaphoreGive(mtx);
+    }
+
+    void __freertos_mutex_give_from_isr(SemaphoreHandle_t mtx) {
+        xSemaphoreGiveFromISR(mtx, NULL);
+    }
+
+    void __freertos_recursive_mutex_take(SemaphoreHandle_t mtx) {
+        xSemaphoreTakeRecursive(mtx, portMAX_DELAY);
+    }
+
+    int __freertos_recursive_mutex_try_take(SemaphoreHandle_t mtx) {
+        return xSemaphoreTakeRecursive(mtx, 0);
+    }
+
+    void __freertos_recursive_mutex_give(SemaphoreHandle_t mtx) {
+        xSemaphoreGiveRecursive(mtx);
+    }
+}
+
 
 /*-----------------------------------------------------------*/
 
@@ -108,6 +155,40 @@ extern "C" void yield() {
     taskYIELD();
 }
 
+static TaskHandle_t __idleCoreTask[2];
+static void __no_inline_not_in_flash_func(IdleThisCore)(void *param) {
+    (void) param;
+    while (true) {
+        ulTaskNotifyTake(pdTRUE, portMAX_DELAY);
+        vTaskPreemptionDisable(nullptr);
+        portDISABLE_INTERRUPTS();
+        __otherCoreIdled = true;
+        while (__otherCoreIdled) {
+            /* noop */
+        }
+        portENABLE_INTERRUPTS();
+        vTaskPreemptionEnable(nullptr);
+    }
+}
+
+extern "C" void __no_inline_not_in_flash_func(__freertos_idle_other_core)() {
+    vTaskPreemptionDisable(nullptr);
+    xTaskNotifyGive(__idleCoreTask[ 1 ^ sio_hw->cpuid ]);
+    while (!__otherCoreIdled) {
+        /* noop */
+    }
+    portDISABLE_INTERRUPTS();
+    vTaskSuspendAll();
+}
+
+extern "C" void __no_inline_not_in_flash_func(__freertos_resume_other_core)() {
+    __otherCoreIdled = false;
+    portENABLE_INTERRUPTS();
+    xTaskResumeAll();
+    vTaskPreemptionEnable(nullptr);
+}
+
+
 extern mutex_t __usb_mutex;
 static TaskHandle_t __usbTask;
 static void __usb(void *param);
@@ -124,6 +205,12 @@ void startFreeRTOS(void) {
         vTaskCoreAffinitySet(c1, 1 << 1);
     }
 
+    // Create the idle-other-core tasks (for when flash is being written)
+    xTaskCreate(IdleThisCore, "IdleCore0", 128, 0, configMAX_PRIORITIES - 1, __idleCoreTask + 0);
+    vTaskCoreAffinitySet(__idleCoreTask[0], 1 << 0);
+    xTaskCreate(IdleThisCore, "IdleCore1", 128, 0, configMAX_PRIORITIES - 1, __idleCoreTask + 1);
+    vTaskCoreAffinitySet(__idleCoreTask[1], 1 << 1);
+
     // Initialise and run the freeRTOS scheduler. Execution should never return here.
     __freeRTOSinitted = true;
     vTaskStartScheduler();
@@ -398,54 +485,9 @@ void __USBStart() {
     __SetupDescHIDReport();
     __SetupUSBDescriptor();
 
-    // Make highest prio and locked to core 0
-    xTaskCreate(__usb, "USB", 256, 0, configMAX_PRIORITIES - 1, &__usbTask);
+    // Make high prio and locked to core 0
+    xTaskCreate(__usb, "USB", 256, 0, configMAX_PRIORITIES - 2, &__usbTask);
     vTaskCoreAffinitySet(__usbTask, 1 << 0);
 }
 
 
-// Interfaces for the main core to use FreeRTOS mutexes
-extern "C" {
-
-    SemaphoreHandle_t __freertos_mutex_create() {
-        return xSemaphoreCreateMutex();
-    }
-
-    SemaphoreHandle_t _freertos_recursive_mutex_create() {
-        return xSemaphoreCreateRecursiveMutex();
-    }
-
-    void __freertos_mutex_take(SemaphoreHandle_t mtx) {
-        xSemaphoreTake(mtx, portMAX_DELAY);
-    }
-
-    void __freertos_mutex_take_from_isr(SemaphoreHandle_t mtx) {
-        xSemaphoreTakeFromISR(mtx, NULL);
-    }
-
-    int __freertos_mutex_try_take(SemaphoreHandle_t mtx) {
-        return xSemaphoreTake(mtx, 0);
-    }
-
-    void __freertos_mutex_give(SemaphoreHandle_t mtx) {
-        xSemaphoreGive(mtx);
-    }
-
-    void __freertos_mutex_give_from_isr(SemaphoreHandle_t mtx) {
-        xSemaphoreGiveFromISR(mtx, NULL);
-    }
-
-    void __freertos_recursive_mutex_take(SemaphoreHandle_t mtx) {
-        xSemaphoreTakeRecursive(mtx, portMAX_DELAY);
-    }
-
-    int __freertos_recursive_mutex_try_take(SemaphoreHandle_t mtx) {
-        return xSemaphoreTakeRecursive(mtx, 0);
-    }
-
-    void __freertos_recursive_mutex_give(SemaphoreHandle_t mtx) {
-        xSemaphoreGiveRecursive(mtx);
-    }
-
-}
-