First try at critical section support

1 files changed, 8 insertions, 1 deletions

diff --git a/ports/esp32s2/common-hal/microcontroller/__init__.c b/ports/esp32s2/common-hal/microcontroller/__init__.c
index a36d0c440..6b2e18673 100644
--- a/ports/esp32s2/common-hal/microcontroller/__init__.c
+++ b/ports/esp32s2/common-hal/microcontroller/__init__.c
@@ -39,24 +39,31 @@
 #include "supervisor/filesystem.h"
 #include "supervisor/shared/safe_mode.h"
 
+#include "freertos/FreeRTOS.h"
+
 void common_hal_mcu_delay_us(uint32_t delay) {
 
 }
 
 volatile uint32_t nesting_count = 0;
+static portMUX_TYPE cp_mutex = portMUX_INITIALIZER_UNLOCKED;
 
 void common_hal_mcu_disable_interrupts(void) {
+    if (nesting_count == 0) {
+        portENTER_CRITICAL(&cp_mutex);
+    }
     nesting_count++;
 }
 
 void common_hal_mcu_enable_interrupts(void) {
     if (nesting_count == 0) {
-
+        // Maybe log here because it's very bad.
     }
     nesting_count--;
     if (nesting_count > 0) {
         return;
     }
+    portEXIT_CRITICAL(&cp_mutex);
 }
 
 void common_hal_mcu_on_next_reset(mcu_runmode_t runmode) {