dport access: Fix nested calls to DPORT_STALL_OTHER_CPU_START()

diff --git a/components/esp32/dport_access.c b/components/esp32/dport_access.c
index 49e6032d29773770af1617d2de1e087908744f84..e1923b41221e4831116b7099e2d70c890fed7b44 100644 (file)
--- a/components/esp32/dport_access.c
+++ b/components/esp32/dport_access.c
@@ -79,11 +79,12 @@ void IRAM_ATTR esp_dport_access_stall_other_cpu_start(void)
 #ifdef DPORT_ACCESS_BENCHMARK
     ccount_start[cpu_id] = XTHAL_GET_CCOUNT();
 #endif
-    BaseType_t intLvl=portENTER_CRITICAL_NESTED();
-    oldInterruptLevel[cpu_id]=intLvl;
+    BaseType_t intLvl = portENTER_CRITICAL_NESTED();
 
     if (dport_access_ref[cpu_id] == 0) {
-        portENTER_CRITICAL_ISR(&g_dport_mux); 
+        portENTER_CRITICAL_ISR(&g_dport_mux);
+
+        oldInterruptLevel[cpu_id]=intLvl;
 
         dport_access_start[cpu_id] = 0;
         dport_access_end[cpu_id] = 0;
@@ -100,6 +101,11 @@ void IRAM_ATTR esp_dport_access_stall_other_cpu_start(void)
     }
 
     dport_access_ref[cpu_id]++;
+
+    if (dport_access_ref[cpu_id] > 1) {
+        /* Interrupts are already disabled by the parent, we're nested here. */
+        portEXIT_CRITICAL_NESTED(intLvl);
+    }
 #endif /* CONFIG_FREERTOS_UNICORE */
 }
 
@@ -124,9 +130,9 @@ void IRAM_ATTR esp_dport_access_stall_other_cpu_end(void)
         dport_access_end[cpu_id] = 1;
 
         portEXIT_CRITICAL_ISR(&g_dport_mux);
+
+        portEXIT_CRITICAL_NESTED(oldInterruptLevel[cpu_id]);
     }
-        
-    portEXIT_CRITICAL_NESTED(oldInterruptLevel[cpu_id]);
 
 #ifdef DPORT_ACCESS_BENCHMARK
     ccount_end[cpu_id] = XTHAL_GET_CCOUNT();