From 8716298ea22c4425b40d160f44dc5a0546576003 Mon Sep 17 00:00:00 2001
From: Vincent Coubard <vincent.coubard@arm.com>
Date: Wed, 23 Sep 2020 17:37:56 +0100
Subject: [PATCH] BLE: Update DB entry if current entry doesn't match requested
 EDIV and RAND

---
 .../FEATURE_BLE/source/generic/SecurityDb.h   | 58 +++++++++++++++++--
 .../source/generic/SecurityManagerImpl.cpp    |  2 +-
 2 files changed, 53 insertions(+), 7 deletions(-)

diff --git a/connectivity/FEATURE_BLE/source/generic/SecurityDb.h b/connectivity/FEATURE_BLE/source/generic/SecurityDb.h
index 3cf01f35f4..8854858334 100644
--- a/connectivity/FEATURE_BLE/source/generic/SecurityDb.h
+++ b/connectivity/FEATURE_BLE/source/generic/SecurityDb.h
@@ -168,16 +168,26 @@ public:
      */
     virtual void get_entry_local_keys(
         SecurityEntryKeysDbCb_t cb,
-        entry_handle_t db_handle,
+        entry_handle_t* db_handle,
         const ediv_t &ediv,
         const rand_t &rand
     ) {
-        SecurityEntryKeys_t* keys = read_in_entry_local_keys(db_handle);
+        SecurityEntryKeys_t* keys = read_in_entry_local_keys(*db_handle);
         /* validate we have the correct key */
         if (keys && ediv == keys->ediv && rand == keys->rand) {
-            cb(db_handle, keys);
+            cb(*db_handle, keys);
         } else {
-            cb(db_handle, NULL);
+            // Maybe this isn't the correct entry, try to find one that matches
+            entry_handle_t correct_handle = find_entry_by_peer_ediv_rand(ediv, rand);
+            if (!correct_handle) {
+                cb(*db_handle, NULL);
+            }
+            // Note: keys should never be null as a matching entry has been retrieved
+            SecurityEntryKeys_t* keys = read_in_entry_local_keys(correct_handle);
+            MBED_ASSERT(keys);
+            close_entry(*db_handle, false);
+            *db_handle = correct_handle;
+            cb(*db_handle, keys);
         }
     }
 
@@ -552,17 +562,53 @@ public:
         return nullptr;
     }
 
+    /**
+     * Find a database entry based on ediv and rand.
+     *
+     * @param[in] ediv E diversifier
+     * @param[in] rand random part
+     *
+     * @return A handle to the entry.
+     */
+    virtual entry_handle_t find_entry_by_peer_ediv_rand(
+        const ediv_t &ediv,
+        const rand_t &rand
+    ) {
+        for (size_t i = 0; i < get_entry_count(); i++) {
+            entry_handle_t db_handle = get_entry_handle_by_index(i);
+            SecurityDistributionFlags_t* flags = get_distribution_flags(db_handle);
+
+            if (!flags || flags->connected) {
+                continue;
+            }
+
+            SecurityEntryKeys_t* keys = read_in_entry_local_keys(db_handle);
+            if (!keys) {
+                continue;
+            }
+
+            if (keys->ediv == ediv && keys->rand == rand) {
+                return db_handle;
+            }
+        }
+
+        return nullptr;
+    }
+
+
     /**
      * Close a connection entry.
      *
      * @param[in] db_handle this handle will be freed up from the security db.
      */
-    virtual void close_entry(entry_handle_t db_handle) {
+    virtual void close_entry(entry_handle_t db_handle, bool require_sync = true) {
         SecurityDistributionFlags_t* flags = get_distribution_flags(db_handle);
         if (flags) {
             flags->connected = false;
         }
-        sync(db_handle);
+        if (require_sync) {
+            sync(db_handle);
+        }
     }
 
     /**
diff --git a/connectivity/FEATURE_BLE/source/generic/SecurityManagerImpl.cpp b/connectivity/FEATURE_BLE/source/generic/SecurityManagerImpl.cpp
index e7a547bd4b..02db71a9ad 100644
--- a/connectivity/FEATURE_BLE/source/generic/SecurityManagerImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/SecurityManagerImpl.cpp
@@ -1935,7 +1935,7 @@ void SecurityManager::on_ltk_request(
 
     _db->get_entry_local_keys(
         mbed::callback(this, &SecurityManager::set_ltk_cb),
-        cb->db_entry,
+        &cb->db_entry,
         ediv,
         rand
     );