From bb35cba134fb6d9a09c6391e39d1946e61486929 Mon Sep 17 00:00:00 2001
From: Vincent Coubard <vincent.coubard@arm.com>
Date: Tue, 29 Sep 2020 10:39:24 +0100
Subject: [PATCH] BLE: Add function to signal privacy initialization.

---
 connectivity/FEATURE_BLE/include/ble/Gap.h    | 16 ++++++++++
 .../FEATURE_BLE/source/generic/GapImpl.cpp    | 29 +++++++++++++++++++
 .../FEATURE_BLE/source/generic/GapImpl.h      |  1 +
 3 files changed, 46 insertions(+)

diff --git a/connectivity/FEATURE_BLE/include/ble/Gap.h b/connectivity/FEATURE_BLE/include/ble/Gap.h
index 88dd9b5824..b056b399b0 100644
--- a/connectivity/FEATURE_BLE/include/ble/Gap.h
+++ b/connectivity/FEATURE_BLE/include/ble/Gap.h
@@ -538,6 +538,14 @@ public:
         )
         {
         }
+
+        /**
+         * Function invoked when the privacy subsystem has been enabled and is
+         * ready to be used.
+         */
+        virtual void onPrivacyEnabled()
+        {
+        }
     protected:
         /**
          * Prevent polymorphic deletion and avoid unnecessary virtual destructor
@@ -1244,6 +1252,14 @@ public:
      * resolved and advertisement packets are forwarded to the application
      * even if the advertiser private address is unknown.
      *
+     * @par Initialization of the privacy subsystem
+     *
+     * When privacy is enabled, the system generates new resolvable and non
+     * resolvable private addresses. Scan, Advertising and Connecting to a peer
+     * won't be available until the generation process completes. When addresses
+     * have been generated, the application is notified that privacy
+     * initialisation as completed with a call to EventHandler::onPrivacyEnabled .
+     *
      * @param[in] enable Should be set to true to enable the privacy mode and
      * false to disable it.
      *
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
index ffb42a47e9..a18d1609b4 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.cpp
@@ -925,8 +925,20 @@ ble_error_t Gap::enablePrivacy(bool enable)
 
     if (_privacy_enabled) {
         _address_registry.start_private_address_generation();
+        if (_address_registry.get_non_resolvable_private_address() != address_t {} &&
+            _address_registry.get_resolvable_private_address() != address_t{}
+        ) {
+            _event_queue.post([this] {
+               if (_event_handler) {
+                   _event_handler->onPrivacyEnabled();
+               }
+            });
+        } else {
+            _privacy_initialization_pending = true;
+        }
     } else {
         _address_registry.stop_private_address_generation();
+        _privacy_initialization_pending = false;
     }
 
 #if !BLE_GAP_HOST_BASED_PRIVATE_ADDRESS_RESOLUTION
@@ -1016,6 +1028,10 @@ ble_error_t Gap::reset()
 
     _event_handler = nullptr;
 
+#if BLE_FEATURE_PRIVACY
+    _privacy_initialization_pending = false;
+#endif
+
 #if BLE_ROLE_BROADCASTER
     _advertising_timeout.detach();
 #endif
@@ -3174,6 +3190,15 @@ void Gap::on_private_address_generated(bool connectable)
         return;
     }
 
+    if (_privacy_initialization_pending &&
+        _address_registry.get_resolvable_private_address() != address_t{} &&
+        _address_registry.get_non_resolvable_private_address() != address_t{}
+    ) {
+        _privacy_initialization_pending = false;
+        if (_event_handler) {
+            _event_handler->onPrivacyEnabled();
+        }
+    }
 
     // refresh for address for all connectable advertising sets
     for (size_t i = 0; i < BLE_GAP_MAX_ADVERTISING_SETS; ++i) {
@@ -3372,6 +3397,10 @@ const address_t *Gap::get_random_address(controller_operation_t operation, size_
             break;
     }
 
+    if (*desired_address == address_t{}) {
+        return nullptr;
+    }
+
     if (!address_in_use) {
         return desired_address;
     }
diff --git a/connectivity/FEATURE_BLE/source/generic/GapImpl.h b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
index c0ab2a9063..2e841c81e6 100644
--- a/connectivity/FEATURE_BLE/source/generic/GapImpl.h
+++ b/connectivity/FEATURE_BLE/source/generic/GapImpl.h
@@ -832,6 +832,7 @@ private:
 
     bool _privacy_enabled;
 #if BLE_FEATURE_PRIVACY
+    bool _privacy_initialization_pending = false;
 #if BLE_ROLE_PERIPHERAL
     peripheral_privacy_configuration_t _peripheral_privacy_configuration;
 #endif // BLE_ROLE_PERIPHERAL