From 606190d9b311a440ce9b034d562624cdad799c2f Mon Sep 17 00:00:00 2001
From: Tiago Medicci Serrano <tiago.medicci@espressif.com>
Date: Mon, 9 Oct 2023 17:10:18 -0300
Subject: [PATCH] esp32/ble: fix saving/restoring the interrupt status flags

Whenever we enter/leave a critical section, the interrupt status is
saved and, then, restored. However, for the ESP32's BLE adapter,
entering/leaving a critical section is done on separate functions
that need to be registered as a callback.

The status flag was being saved as a global variable. However,
calling nested enter_critical_section would overwrite this global
variable that was storing the previous flag and, when leaving the
last critical section, the restored status would be different from
the one expected. The proposed solution for this issue is to create
a global array to store the interrupt status flags for nested calls.
---
 arch/xtensa/src/esp32/Kconfig             |  8 +++-
 arch/xtensa/src/esp32/esp32_ble_adapter.c | 51 +++++++++++++++++++++--
 2 files changed, 54 insertions(+), 5 deletions(-)

diff --git a/arch/xtensa/src/esp32/Kconfig b/arch/xtensa/src/esp32/Kconfig
index 24c6605ee5..bb273a1686 100644
--- a/arch/xtensa/src/esp32/Kconfig
+++ b/arch/xtensa/src/esp32/Kconfig
@@ -2093,7 +2093,7 @@ choice EXAMPLE_POWER_SAVE_MODE
 
 config EXAMPLE_POWER_SAVE_NONE
 	bool "none"
-	
+
 config EXAMPLE_POWER_SAVE_MIN_MODEM
 	bool "minimum modem"
 
@@ -2130,6 +2130,12 @@ config ESP32_BLE_TASK_PRIORITY
 	int "Controller task priority"
 	default 110
 
+config ESP32_BLE_INTERRUPT_SAVE_STATUS
+	int "Number of interrupt save status"
+	default 3
+	---help---
+		Number of interrupt save status variables to keep track. Increase it if any related bug is found.
+
 config ESP32_BLE_MAX_CONN
 	int "Maximum BLE simultaneous connections"
 	range 1 3
diff --git a/arch/xtensa/src/esp32/esp32_ble_adapter.c b/arch/xtensa/src/esp32/esp32_ble_adapter.c
index 22de775e74..c7050db08e 100644
--- a/arch/xtensa/src/esp32/esp32_ble_adapter.c
+++ b/arch/xtensa/src/esp32/esp32_ble_adapter.c
@@ -104,7 +104,11 @@ do {\
 
 #define ESP_BT_CONTROLLER_CONFIG_MAGIC_VAL  0x20200622
 
-extern void btdm_controller_set_sleep_mode(uint8_t mode);
+#ifdef CONFIG_ESP32_BLE_INTERRUPT_SAVE_STATUS
+#  define NR_IRQSTATE_FLAGS   CONFIG_ESP32_BLE_INTERRUPT_SAVE_STATUS
+#else
+#  define NR_IRQSTATE_FLAGS   3
+#endif
 
 /****************************************************************************
  * Private Types
@@ -305,6 +309,14 @@ struct osi_funcs_s
     uint32_t _magic;
 };
 
+/* List of nested IRQ status flags */
+
+struct irqstate_list_s
+{
+  struct irqstate_list_s *flink;
+  irqstate_t flags;
+};
+
 /****************************************************************************
  * Private Function
  ****************************************************************************/
@@ -503,7 +515,11 @@ static DRAM_ATTR esp_timer_handle_t g_btdm_slp_tmr;
 
 /* BT interrupt private data */
 
-static irqstate_t g_inter_flags;
+static sq_queue_t g_int_flags_free;
+
+static sq_queue_t g_int_flags_used;
+
+static struct irqstate_list_s g_int_flags[NR_IRQSTATE_FLAGS];
 
 /****************************************************************************
  * Public Data
@@ -625,6 +641,8 @@ static btdm_dram_available_region_t btdm_dram_available_region[] =
   },
 };
 
+extern void btdm_controller_set_sleep_mode(uint8_t mode);
+
 /****************************************************************************
  * Private Functions and Public Functions only used by libraries
  ****************************************************************************/
@@ -1089,7 +1107,15 @@ static int esp_int_adpt_cb(int irq, void *context, void *arg)
 
 static void IRAM_ATTR interrupt_disable(void)
 {
-  g_inter_flags = enter_critical_section();
+  struct irqstate_list_s *irqstate;
+
+  irqstate = (struct irqstate_list_s *)sq_remlast(&g_int_flags_free);
+
+  DEBUGASSERT(irqstate != NULL);
+
+  irqstate->flags = enter_critical_section();
+
+  sq_addlast((sq_entry_t *)irqstate, &g_int_flags_used);
 }
 
 /****************************************************************************
@@ -1109,7 +1135,15 @@ static void IRAM_ATTR interrupt_disable(void)
 
 static void IRAM_ATTR interrupt_restore(void)
 {
-  leave_critical_section(g_inter_flags);
+  struct irqstate_list_s *irqstate;
+
+  irqstate = (struct irqstate_list_s *)sq_remlast(&g_int_flags_used);
+
+  DEBUGASSERT(irqstate != NULL);
+
+  leave_critical_section(irqstate->flags);
+
+  sq_addlast((sq_entry_t *)irqstate, &g_int_flags_free);
 }
 
 /****************************************************************************
@@ -2370,6 +2404,15 @@ int esp32_bt_controller_init(void)
   esp_bt_controller_config_t *cfg = &bt_cfg;
   int err;
   uint32_t btdm_cfg_mask = 0;
+  int i;
+
+  sq_init(&g_int_flags_free);
+  sq_init(&g_int_flags_used);
+
+  for (i = 0; i < NR_IRQSTATE_FLAGS; i++)
+    {
+      sq_addlast((sq_entry_t *)&g_int_flags[i], &g_int_flags_free);
+    }
 
   if (btdm_controller_status != ESP_BT_CONTROLLER_STATUS_IDLE)
     {