From 2643d26d030de635b4ab4cc19aaeab67c8598cb2 Mon Sep 17 00:00:00 2001
From: Astha Verma <astha.verma@espressif.com>
Date: Thu, 16 Jan 2025 17:01:01 +0530
Subject: [PATCH] fix(nimble): Handle nested locks when BT_NIMBLE_DEBUG enabled

---
 nimble/host/src/ble_att.c     |  3 +++
 nimble/host/src/ble_att_svr.c |  2 ++
 nimble/host/src/ble_gap.c     |  2 ++
 nimble/host/src/ble_gattc.c   |  2 ++
 nimble/host/src/ble_hs.c      | 21 ++++++++++++++++++---
 5 files changed, 27 insertions(+), 3 deletions(-)

diff --git a/nimble/host/src/ble_att.c b/nimble/host/src/ble_att.c
index c54f8136b..b34a9fb8f 100644
--- a/nimble/host/src/ble_att.c
+++ b/nimble/host/src/ble_att.c
@@ -719,7 +719,10 @@ ble_att_set_default_bearer_using_cid(uint16_t conn_handle, uint16_t cid) {
 #if MYNEWT_VAL(BLE_EATT_CHAN_NUM) > 0
     struct ble_hs_conn * conn;
 
+    ble_hs_lock();
     conn = ble_hs_conn_find(conn_handle);
+    ble_hs_unlock();
+
     if (conn == NULL) {
         return BLE_HS_ENOTCONN;
     }
diff --git a/nimble/host/src/ble_att_svr.c b/nimble/host/src/ble_att_svr.c
index b423991fe..5526edb73 100644
--- a/nimble/host/src/ble_att_svr.c
+++ b/nimble/host/src/ble_att_svr.c
@@ -371,7 +371,9 @@ ble_att_svr_check_perms(uint16_t conn_handle, int is_read,
 
     if (author) {
         /* XXX: Prompt user for authorization. */
+        ble_hs_lock();
         conn = ble_hs_conn_find(conn_handle);
+        ble_hs_unlock();
         if(!conn->bhc_sec_state.authorize){
             rc = ble_gap_authorize_event(conn_handle, entry->ha_handle_id, is_read);
             if (rc == BLE_GAP_AUTHORIZE_REJECT) {
diff --git a/nimble/host/src/ble_gap.c b/nimble/host/src/ble_gap.c
index b69e13d08..313cc812e 100644
--- a/nimble/host/src/ble_gap.c
+++ b/nimble/host/src/ble_gap.c
@@ -8020,7 +8020,9 @@ done:
 int
 ble_gap_dev_authorization(uint16_t conn_handle, bool authorized)
 {
+    ble_hs_lock();
     struct ble_hs_conn *conn = ble_hs_conn_find(conn_handle);
+    ble_hs_unlock();
 
     if (conn != NULL) {
         if (!(conn->bhc_sec_state.authenticated)) {
diff --git a/nimble/host/src/ble_gattc.c b/nimble/host/src/ble_gattc.c
index c8b292f6c..c4be7e744 100644
--- a/nimble/host/src/ble_gattc.c
+++ b/nimble/host/src/ble_gattc.c
@@ -5379,7 +5379,9 @@ ble_gattc_connection_broken(uint16_t conn_handle)
 
     ble_gattc_fail_procs(conn_handle, BLE_GATT_OP_NONE, BLE_HS_ENOTCONN);
 
+    ble_hs_lock();
     conn = ble_hs_conn_find(conn_handle);
+    ble_hs_unlock();
 
     while ((omp = STAILQ_FIRST(&conn->att_tx_q)) != NULL) {
         STAILQ_REMOVE_HEAD(&conn->att_tx_q, omp_next);
diff --git a/nimble/host/src/ble_hs.c b/nimble/host/src/ble_hs.c
index d15577279..b1ec07f71 100644
--- a/nimble/host/src/ble_hs.c
+++ b/nimble/host/src/ble_hs.c
@@ -100,7 +100,11 @@ uint16_t ble_hs_max_services;
 uint16_t ble_hs_max_client_configs;
 
 #if MYNEWT_VAL(BLE_HS_DEBUG)
+#define MAX_NESTED_LOCKS 5
+static TaskHandle_t ble_hs_task_handles[MAX_NESTED_LOCKS];
+static int ble_hs_task_handle_index = 0;
 static uint8_t ble_hs_mutex_locked;
+static uint8_t counter_lock = 0;
 static TaskHandle_t ble_hs_task_handle;
 static uint8_t ble_hs_dbg_mutex_locked;
 #endif
@@ -179,8 +183,11 @@ ble_hs_lock_nested(void)
     rc = ble_npl_mutex_pend(&ble_hs_mutex, 0xffffffff);
 
 #if MYNEWT_VAL(BLE_HS_DEBUG)
+    counter_lock++;
     ble_hs_mutex_locked = 1;
     ble_hs_task_handle = xTaskGetCurrentTaskHandle();
+    ble_hs_task_handles[ble_hs_task_handle_index] = xTaskGetCurrentTaskHandle();
+    ble_hs_task_handle_index++;
 #endif
     BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
 }
@@ -198,14 +205,22 @@ ble_hs_unlock_nested(void)
         ble_hs_dbg_mutex_locked = 0;
         return;
     }
-    if(ble_hs_task_handle == xTaskGetCurrentTaskHandle()) {
-        ble_hs_task_handle = NULL;
-        ble_hs_mutex_locked = 0;
+    if (counter_lock > 0) {
+        counter_lock--;
+        if (counter_lock == 0) {
+            ble_hs_mutex_locked = 0;
+        }
+        if (ble_hs_task_handles[ble_hs_task_handle_index - 1] == xTaskGetCurrentTaskHandle()) {
+            ble_hs_task_handle_index--;
+            ble_hs_task_handles[ble_hs_task_handle_index] = NULL;
+            ble_hs_task_handle = ble_hs_task_handles[ble_hs_task_handle_index -1];
+        }
     }
 #endif
 
     rc = ble_npl_mutex_release(&ble_hs_mutex);
     BLE_HS_DBG_ASSERT_EVAL(rc == 0 || rc == OS_NOT_STARTED);
+
 }
 
 /**