fix privacy initialisation and give access to local identity

Initialises identity addres when privacy is enabled. Stores the identity. Retrieves local identity if previously stored.

features/FEATURE_BLE/ble/SecurityManager.h

@@ -285,6 +285,22 @@ public:
             (void)result;
         }
 
+        /**
+         * Indicate that a peer address has been saved by the security manager or if we are
+         * bonded to the peer the identity has been retrieved from the database on connection.
+         *
+         * @param[in] connectionHandle Connection handle.
+         * @param[in] peer_address Peer address that has been saved by the security database, NULL it not found.
+         * @param[in] address_is_public Address type, true if public. Invalid if peer_address NULL.
+         */
+        virtual void peerIdentity(ble::connection_handle_t connectionHandle,
+                                  const address_t *peer_address,
+                                  bool address_is_public) {
+            (void)connectionHandle;
+            (void)peer_address;
+            (void)address_is_public;
+        }
+
         ////////////////////////////////////////////////////////////////////////////
         // Security
         //
@@ -561,6 +577,14 @@ public:
      */
     ble_error_t setPairingRequestAuthorisation(bool required = true);
 
+    /**
+     * Retrieve identity address for the peer on the given connection.
+     *
+     * @param[in] connectionHandle Handle to identify the connection.
+     * @return BLE_ERROR_NONE or appropriate error code indicating the failure reason.
+     */
+    ble_error_t getPeerIdentity(ble::connection_handle_t connectionHandle);
+
     ////////////////////////////////////////////////////////////////////////////
     // Feature support
     //
@@ -899,6 +923,10 @@ protected:
         ble::connection_handle_t connectionHandle
     );
 
+    ble_error_t getPeerIdentity_(
+        ble::connection_handle_t connectionHandle
+    );
+
     ble_error_t setPairingRequestAuthorisation_(
         bool required
     );

features/FEATURE_BLE/ble/generic/GenericSecurityManager.h

@@ -124,6 +124,10 @@ public:
         bool required = true
     );
 
+    ble_error_t getPeerIdentity_(
+        connection_handle_t connection
+    );
+
     ////////////////////////////////////////////////////////////////////////////
     // Feature support
     //
@@ -321,6 +325,13 @@ private:
      */
     ble_error_t init_signing();
 
+    /**
+     * Generate the IRK if needed.
+     *
+     * @return BLE_ERROR_NONE or appropriate error code indicating the failure reason.
+     */
+    ble_error_t init_identity();
+
     /**
      * Fills the buffer with the specified number of bytes of random data
      * produced by the link controller

features/FEATURE_BLE/source/generic/GenericSecurityManager.tpp

@@ -102,13 +102,19 @@ ble_error_t GenericSecurityManager<TPalSecurityManager, SigningMonitor>::init_(
     result = init_resolving_list();
 #endif
 
+#if BLE_FEATURE_PRIVACY
+    // set the local identity address and irk
+    if (result != BLE_ERROR_NONE) {
+    	result = init_identity();
+    }
+#endif // BLE_FEATURE_PRIVACY
+
     if (result != BLE_ERROR_NONE) {
         delete _db;
         _db = NULL;
-        return result;
     }
 
-    return BLE_ERROR_NONE;
+    return result;
 }
 
 template<template<class> class TPalSecurityManager, template<class> class SigningMonitor>
@@ -309,6 +315,33 @@ ble_error_t GenericSecurityManager<TPalSecurityManager, SigningMonitor>::setPair
     return BLE_ERROR_NONE;
 }
 
+template<template<class> class TPalSecurityManager, template<class> class SigningMonitor>
+ble_error_t GenericSecurityManager<TPalSecurityManager, SigningMonitor>::getPeerIdentity_(connection_handle_t connection) {
+    if (!_db) return BLE_ERROR_INITIALIZATION_INCOMPLETE;
+    if (eventHandler) {
+        ControlBlock_t *cb = get_control_block(connection);
+        if (!cb) {
+            return BLE_ERROR_INVALID_PARAM;
+        }
+
+        _db->get_entry_identity(
+            [connection,this](SecurityDb::entry_handle_t handle, const SecurityEntryIdentity_t* identity) {
+                if (eventHandler) {
+                    eventHandler->peerIdentity(
+                        connection,
+                        identity ? &identity->identity_address : nullptr,
+                        identity ? identity->identity_address_is_public : false
+                    );
+                }
+            },
+            cb->db_entry
+        );
+        return BLE_ERROR_NONE;
+    } else {
+        return BLE_ERROR_INVALID_STATE;
+    }
+}
+
 ////////////////////////////////////////////////////////////////////////////
 // Feature support
 //
@@ -901,6 +934,33 @@ ble_error_t GenericSecurityManager<TPalSecurityManager, SigningMonitor>::init_si
     return _pal.set_csrk(*pcsrk, local_sign_counter);
 }
 
+template<template<class> class TPalSecurityManager, template<class> class SigningMonitor>
+ble_error_t GenericSecurityManager<TPalSecurityManager, SigningMonitor>::init_identity() {
+    if (!_db) return BLE_ERROR_INITIALIZATION_INCOMPLETE;
+    const irk_t *pirk = nullptr;
+
+    irk_t irk = _db->get_local_irk();
+    if (irk != irk_t()) {
+        pirk = &irk;
+    } else {
+        ble_error_t ret = get_random_data(irk.data(), irk.size());
+        if (ret != BLE_ERROR_NONE) {
+            return ret;
+        }
+
+        pirk = &irk;
+        address_t identity_address;
+        bool public_address;
+        ret = _pal.get_identity_address(identity_address, public_address);
+        if (ret != BLE_ERROR_NONE) {
+            return ret;
+        }
+        _db->set_local_identity(irk, identity_address, public_address);
+    }
+
+    return _pal.set_irk(*pirk);
+}
+
 template<template<class> class TPalSecurityManager, template<class> class SigningMonitor>
 ble_error_t GenericSecurityManager<TPalSecurityManager, SigningMonitor>::get_random_data(uint8_t *buffer, size_t size) {
     byte_array_t<8> random_data;