From c031e1441ca38cb89833ad2be7abd6c836ecea22 Mon Sep 17 00:00:00 2001
From: Rahul Tank <rahul.tank@espressif.com>
Date: Fri, 3 Jan 2025 16:35:28 +0530
Subject: [PATCH] fix(nimble): Corrected storing method of csfc to nvs

---
 nimble/host/src/ble_gatts.c                   |  4 ++
 .../host/store/config/src/ble_store_config.c  | 40 +++++++++----------
 .../store/config/src/ble_store_config_conf.c  |  4 +-
 3 files changed, 26 insertions(+), 22 deletions(-)

diff --git a/nimble/host/src/ble_gatts.c b/nimble/host/src/ble_gatts.c
index 876bf57de..05096f52a 100644
--- a/nimble/host/src/ble_gatts.c
+++ b/nimble/host/src/ble_gatts.c
@@ -2257,6 +2257,7 @@ ble_gatts_peer_cl_sup_feat_update(uint16_t conn_handle, struct os_mbuf *om)
             goto done;
         }
 
+        memset(&value_csfc, 0, sizeof value_csfc);
         value_csfc.peer_addr = conn->bhc_peer_addr;
         memcpy(value_csfc.csfc, feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE));
 
@@ -2426,6 +2427,9 @@ ble_gatts_bonding_established(uint16_t conn_handle)
     }
 
     csfc.peer_addr = conn->bhc_peer_addr;
+    csfc.peer_addr.type =
+	    ble_hs_misc_peer_addr_type_to_id(conn->bhc_peer_addr.type);
+
     memcpy(csfc.csfc, conn->bhc_gatt_svr.peer_cl_sup_feat, MYNEWT_VAL(BLE_GATT_CSFC_SIZE));
 
     ble_hs_unlock();
diff --git a/nimble/host/store/config/src/ble_store_config.c b/nimble/host/store/config/src/ble_store_config.c
index 4900fe960..de1739866 100644
--- a/nimble/host/store/config/src/ble_store_config.c
+++ b/nimble/host/store/config/src/ble_store_config.c
@@ -52,8 +52,8 @@ int ble_store_config_num_local_irks;
 #if MYNEWT_VAL(BLE_STORE_MAX_CSFCS)
 struct ble_store_value_csfc
     ble_store_config_csfcs[MYNEWT_VAL(BLE_STORE_MAX_CSFCS)];
-int ble_store_config_num_csfcs;
 #endif
+int ble_store_config_num_csfcs;
 
 #if MYNEWT_VAL(ENC_ADV_DATA)
 struct ble_store_value_ead
@@ -873,28 +873,25 @@ ble_store_config_write_local_irk(const struct ble_store_value_local_irk *value_i
  *****************************************************************************/
 
 static int
-ble_store_config_find_csfc(const struct ble_store_key_csfc *key)
+ble_store_config_find_csfc(const struct ble_store_key_csfc *key,
+                           const struct ble_store_value_csfc *value_csfc,
+                           int num_value_csfc)
 {
-    struct ble_store_value_csfc *csfc;
-    int skipped;
+    const struct ble_store_value_csfc *cur;
     int i;
 
-    skipped = 0;
-    for (i = 0; i < ble_store_config_num_csfcs; i++) {
-        csfc = ble_store_config_csfcs + i;
+    if (!ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) {
+        if (key->idx < num_value_csfc) {
+            return key->idx;
+        }
+    } else if (key->idx == 0) {
+        for (i = 0; i < num_value_csfc; i++) {
+            cur = &value_csfc[i];
 
-        if (ble_addr_cmp(&key->peer_addr, BLE_ADDR_ANY)) {
-            if (ble_addr_cmp(&csfc->peer_addr, &key->peer_addr)) {
-                continue;
+            if (!ble_addr_cmp(&cur->peer_addr, &key->peer_addr)) {
+                return i;
             }
         }
-
-        if (key->idx > skipped) {
-            skipped++;
-            continue;
-        }
-
-        return i;
     }
 
     return -1;
@@ -906,7 +903,8 @@ ble_store_config_delete_csfc(const struct ble_store_key_csfc *key_csfc)
     int idx;
     int rc;
 
-    idx = ble_store_config_find_csfc(key_csfc);
+    idx = ble_store_config_find_csfc(key_csfc, ble_store_config_csfcs,
+                                     ble_store_config_num_csfcs);
     if (idx == -1) {
         return BLE_HS_ENOENT;
     }
@@ -933,7 +931,8 @@ ble_store_config_read_csfc(const struct ble_store_key_csfc *key_csfc,
 {
     int idx;
 
-    idx = ble_store_config_find_csfc(key_csfc);
+    idx = ble_store_config_find_csfc(key_csfc, ble_store_config_csfcs,
+                                    ble_store_config_num_csfcs);
     if (idx == -1) {
         return BLE_HS_ENOENT;
     }
@@ -950,7 +949,8 @@ ble_store_config_write_csfc(const struct ble_store_value_csfc *value_csfc)
     int rc;
 
     ble_store_key_from_value_csfc(&key_csfc, value_csfc);
-    idx = ble_store_config_find_csfc(&key_csfc);
+    idx = ble_store_config_find_csfc(&key_csfc, ble_store_config_csfcs,
+                                     ble_store_config_num_csfcs);
     if (idx == -1) {
         if (ble_store_config_num_csfcs >= MYNEWT_VAL(BLE_STORE_MAX_CSFCS)) {
             BLE_HS_LOG(DEBUG, "error persisting csfc; too many entries (%d)\n",
diff --git a/nimble/host/store/config/src/ble_store_config_conf.c b/nimble/host/store/config/src/ble_store_config_conf.c
index 93d36f618..969d47e19 100644
--- a/nimble/host/store/config/src/ble_store_config_conf.c
+++ b/nimble/host/store/config/src/ble_store_config_conf.c
@@ -66,8 +66,8 @@ static struct conf_handler ble_store_config_conf_handler = {
 #define BLE_STORE_CONFIG_CSFC_ENCODE_SZ     \
     BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_csfc))
 
-#define BLE_STORE_CONFIG_CSFC_ENCODE_SZ     \
-    BASE64_ENCODE_SIZE(sizeof (struct ble_store_value_csfc))
+#define BLE_STORE_CONFIG_CSFC_SET_ENCODE_SZ     \
+    (MYNEWT_VAL(BLE_STORE_MAX_CSFCS) * BLE_STORE_CONFIG_CSFC_ENCODE_SZ + 1)
 
 #if MYNEWT_VAL(ENC_ADV_DATA)
 #define BLE_STORE_CONFIG_EAD_ENCODE_SZ     \