Add ISR check to critical section

Author: ldursw

libraries/FreeRTOS/src/variantHooks.cpp

@@ -45,6 +45,7 @@
 // Interfaces for the main core to use FreeRTOS mutexes
 extern "C" {
     extern volatile bool __otherCoreIdled;
+    static UBaseType_t __savedIrqs[configNUMBER_OF_CORES];
 
     SemaphoreHandle_t __freertos_mutex_create() {
         return xSemaphoreCreateMutex();
@@ -93,11 +94,19 @@ extern "C" {
     }
 
     void __freertos_task_exit_critical() {
-        taskEXIT_CRITICAL();
+        if (portGET_CRITICAL_NESTING_COUNT() == 1U && portCHECK_IF_IN_ISR()) {
+            taskEXIT_CRITICAL_FROM_ISR(__savedIrqs[portGET_CORE_ID()]);
+        } else {
+            taskEXIT_CRITICAL();
+        }
     }
 
     void __freertos_task_enter_critical() {
-        taskENTER_CRITICAL();
+        if (portGET_CRITICAL_NESTING_COUNT() == 0U && portCHECK_IF_IN_ISR()) {
+            __savedIrqs[portGET_CORE_ID()] = taskENTER_CRITICAL_FROM_ISR();
+        } else {
+            taskENTER_CRITICAL();
+        }
     }
 }