Merge pull request #13660 from pan-/host-privacy-fixes

BLE: Retrieve correct DB entry when RPA used

Author: pan-

connectivity/FEATURE_BLE/source/generic/GapImpl.cpp

@@ -1260,43 +1260,24 @@ void Gap::on_connection_complete(const GapConnectionCompleteEvent &e)
     }
 #endif // BLE_ROLE_PERIPHERAL
 
-    ble::address_t address;
-    if (_address_type == own_address_type_t::PUBLIC) {
-        address = _pal_gap.get_device_address();
-    } else {
-        address = _pal_gap.get_random_address();
-    }
-
-    // signal internal stack
-    if (_connection_event_handler) {
-        _connection_event_handler->on_connected(
-            e.connection_handle,
-            e.role,
-            e.peer_address_type,
-            e.peer_address,
-            _address_type,
-            address
-        );
-    }
+    ConnectionCompleteEvent event(
+        BLE_ERROR_NONE,
+        e.connection_handle,
+        e.role,
+        e.peer_address_type,
+        e.peer_address,
+        e.local_resolvable_private_address,
+        e.peer_resolvable_private_address,
+        conn_interval_t(e.connection_interval),
+        e.connection_latency,
+        supervision_timeout_t(e.supervision_timeout),
+        /* default master clock accuracy */ ble::clock_accuracy_t::PPM_500
+    );
 
-    // signal application
     if (_event_handler) {
-        ConnectionCompleteEvent event(
-            BLE_ERROR_NONE,
-            e.connection_handle,
-            e.role,
-            e.peer_address_type,
-            e.peer_address,
-            e.local_resolvable_private_address,
-            e.peer_resolvable_private_address,
-            conn_interval_t(e.connection_interval),
-            e.connection_latency,
-            supervision_timeout_t(e.supervision_timeout),
-            /* default master clock accuracy */ ble::clock_accuracy_t::PPM_500
-        );
-        signal_connection_complete(
-            event
-        );
+        signal_connection_complete(event);
+    } else {
+        report_internal_connection_complete(event);
     }
 }
 
@@ -2204,6 +2185,30 @@ void Gap::on_extended_advertising_report(
 }
 
 #if BLE_FEATURE_CONNECTABLE
+void Gap::report_internal_connection_complete(const ConnectionCompleteEvent& event)
+{
+    if (!_connection_event_handler || event.getStatus() != BLE_ERROR_NONE) {
+        return;
+    }
+
+    ble::address_t address;
+    if (_address_type == own_address_type_t::PUBLIC) {
+        address = _pal_gap.get_device_address();
+    } else {
+        address = _pal_gap.get_random_address();
+    }
+
+    _connection_event_handler->on_connected(
+        event.getConnectionHandle(),
+        event.getOwnRole(),
+        event.getPeerAddressType(),
+        event.getPeerAddress(),
+        _address_type,
+        address
+    );
+}
+
+
 void Gap::signal_connection_complete(
     ConnectionCompleteEvent& event
 )
@@ -2259,9 +2264,8 @@ void Gap::signal_connection_complete(
 
     /* if successful then proceed to call the handler immediately same as for when privacy is disabled */
     if (address_resolved) {
-        _event_handler->onConnectionComplete(
-            event
-        );
+        report_internal_connection_complete(event);
+        _event_handler->onConnectionComplete(event);
     } else {
         bool resolution_pending = false;
         ble_error_t ret = _address_registry.queue_resolve_address(event.getPeerAddress());
@@ -2287,9 +2291,8 @@ void Gap::signal_connection_complete(
         }
     }
 #else
-    _event_handler->onConnectionComplete(
-            event
-    );
+    report_internal_connection_complete(event);
+    _event_handler->onConnectionComplete(event);
 #endif // BLE_FEATURE_PRIVACY
 }
 
@@ -2329,9 +2332,8 @@ void Gap::conclude_signal_connection_complete_after_address_resolution(
     }
 #endif // BLE_ROLE_PERIPHERAL
 
-    _event_handler->onConnectionComplete(
-        event
-    );
+    report_internal_connection_complete(event);
+    _event_handler->onConnectionComplete(event);
 #if BLE_ROLE_PERIPHERAL
 #if BLE_FEATURE_SECURITY
     if (resolvable_address_not_known) {

connectivity/FEATURE_BLE/source/generic/GapImpl.h

@@ -597,6 +597,13 @@ class Gap :
     ble_error_t prepare_legacy_advertising_set(const AdvertisingParameters& parameters);
 
 #if BLE_FEATURE_CONNECTABLE
+    /** Call the internal handlers that report to the security manager and GATT
+     * that a connection has been established.
+     *
+     * @param report Connection event
+     */
+    void report_internal_connection_complete(const ConnectionCompleteEvent& report);
+
     /** Pass the connection complete event to the application. This may involve privacy resolution.
      *
      * @param report Event to be passed to the user application.

connectivity/FEATURE_BLE/source/generic/SecurityDb.h

@@ -168,16 +168,26 @@ class SecurityDb {
      */
     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 @@ class SecurityDb {
         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);
+        }
     }
 
     /**

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
     );