BLE: Implement resolving list in Nordic PAL security manager

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF5/source/nRF5xPalSecurityManager.cpp

@@ -89,7 +89,8 @@ nRF5xSecurityManager::nRF5xSecurityManager()
     _io_capability(io_capability_t::NO_INPUT_NO_OUTPUT),
     _min_encryption_key_size(7),
     _max_encryption_key_size(16),
-    _control_blocks(NULL)
+    _control_blocks(NULL),
+    resolving_list_entry_count(0)
 {
 
 }
@@ -141,8 +142,7 @@ ble_error_t nRF5xSecurityManager::reset()
 
 uint8_t nRF5xSecurityManager::read_resolving_list_capacity()
 {
-    // FIXME: implement with privacy
-    return 0;
+    return MAX_RESOLVING_LIST_ENTRIES;
 }
 
 ble_error_t nRF5xSecurityManager::add_device_to_resolving_list(
@@ -150,22 +150,95 @@ ble_error_t nRF5xSecurityManager::add_device_to_resolving_list(
     const address_t &peer_identity_address,
     const irk_t &peer_irk
 ) {
-    // FIXME: implement with privacy
-    return BLE_ERROR_NOT_IMPLEMENTED;
+    if (resolving_list_entry_count >= MAX_RESOLVING_LIST_ENTRIES) {
+        return BLE_ERROR_INVALID_STATE;
+    }
+
+    resolving_list_entry_t& entry = resolving_list[resolving_list_entry_count];
+    entry.peer_identity_address_type = peer_identity_address_type;
+    entry.peer_identity_address = peer_identity_address;
+    entry.peer_irk = peer_irk;
+
+    ++resolving_list_entry_count;
+
+    return BLE_ERROR_NONE;
 }
 
 ble_error_t nRF5xSecurityManager::remove_device_from_resolving_list(
     advertising_peer_address_type_t peer_identity_address_type,
     const address_t &peer_identity_address
 ) {
-    // FIXME: implement with privacy
-    return BLE_ERROR_NOT_IMPLEMENTED;
+    size_t entry_index;
+
+    // first the index needs to be found
+    for (entry_index = 0; entry_index < resolving_list_entry_count; ++entry_index) {
+        resolving_list_entry_t& entry = resolving_list[entry_index];
+        if (entry.peer_identity_address_type == peer_identity_address_type &&
+            entry.peer_identity_address == peer_identity_address
+        ) {
+            break;
+        }
+    }
+
+    if (entry_index == resolving_list_entry_count) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    // Elements after the entry can be moved in the list
+    for (size_t i = entry_index; i < (resolving_list_entry_count - 1); ++i) {
+        resolving_list[i] = resolving_list[i + 1];
+    }
+
+    --resolving_list_entry_count;
+
+    return BLE_ERROR_NONE;
 }
 
 ble_error_t nRF5xSecurityManager::clear_resolving_list()
 {
-    // FIXME: implement with privacy
-    return BLE_ERROR_NOT_IMPLEMENTED;
+    resolving_list_entry_count = 0;
+    return BLE_ERROR_NONE;
+}
+
+void nRF5xSecurityManager::get_resolving_list(
+    size_t& count,
+    const resolving_list_entry_t*& entries
+) {
+    count = resolving_list_entry_count;
+    entries = resolving_list;
+}
+
+bool nRF5xSecurityManager::resolve_address(
+    const address_t& resolvable_address,
+    advertising_peer_address_type_t& resolved_address_type,
+    address_t& resolved_address
+) {
+    typedef byte_array_t<CryptoToolbox::hash_size_> hash_t;
+
+    for (size_t i = 0; i < resolving_list_entry_count; ++i) {
+        resolving_list_entry_t& entry = resolving_list[i];
+        hash_t hash_generated;
+
+        // Compute the hash part from the random address part when the irk of
+        // the entry is used
+        _crypto.ah(
+            make_const_ArrayView<CryptoToolbox::irk_size_>(entry.peer_irk),
+            make_const_ArrayView<CryptoToolbox::prand_size_>(
+                resolvable_address.data() + CryptoToolbox::hash_size_
+            ),
+            make_ArrayView(hash_generated)
+        );
+
+        // Compare hash generated with the hash present in the address passed as
+        // parameter. If they are equal then the IRK of the entry has been used
+        // to generate the resolvable address.
+        if (memcmp(hash_generated.data(), resolvable_address.data(), CryptoToolbox::hash_size_) == 0) {
+            resolved_address_type = entry.peer_identity_address_type;
+            resolved_address = entry.peer_identity_address;
+            return true;
+        }
+    }
+    return false;
 }
 
 ////////////////////////////////////////////////////////////////////////////

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF5/source/nRF5xPalSecurityManager.h

@@ -83,6 +83,46 @@ public:
      */
     virtual ble_error_t clear_resolving_list();
 
+    /**
+     * An entry of the resolving list stored in the SecurityManager.
+     */
+    struct resolving_list_entry_t {
+        resolving_list_entry_t() :
+            peer_identity_address_type(
+                advertising_peer_address_type_t::PUBLIC_ADDRESS
+            )
+        { }
+
+        irk_t peer_irk;
+        address_t peer_identity_address;
+        advertising_peer_address_type_t peer_identity_address_type;
+    };
+
+    /**
+     * Return the IRKs present in the resolving list
+     * @param count The number of entries present in the resolving list.
+     * @param pointer to the first entry of the resolving list.
+     */
+    void get_resolving_list(
+        size_t& count,
+        const resolving_list_entry_t*& entries
+    );
+
+    /**
+     * Try to resolve a private resolvable address.
+     *
+     * @param resolvable_address The address to resolve.
+     * @param resolved_address_type The type of the identity address resolved.
+     * @param resolved_address The identity address resolved.
+     * @return True if the address has been resolved and false otherwise.
+     */
+    bool resolve_address(
+        const address_t& resolvable_address,
+        advertising_peer_address_type_t& resolved_address_type,
+        address_t& resolved_address
+    );
+
+
     ////////////////////////////////////////////////////////////////////////////
     // Pairing
     //
@@ -351,6 +391,11 @@ private:
     ble::public_key_coord_t X;
     ble::public_key_coord_t Y;
     ble::public_key_coord_t secret;
+
+    static const size_t MAX_RESOLVING_LIST_ENTRIES = BLE_GAP_WHITELIST_IRK_MAX_COUNT;
+
+    size_t resolving_list_entry_count;
+    resolving_list_entry_t resolving_list[MAX_RESOLVING_LIST_ENTRIES];
 };
 
 } // nordic