Merge pull request #32 from pan-/sm-privacy-nordic

Sm privacy nordic

features/FEATURE_BLE/ble/BLETypes.h

@@ -426,10 +426,10 @@ typedef uint32_t sign_count_t;
  */
 struct address_t : public byte_array_t<6> {
     /**
-     * Create an invalid mac address, equal to FF:FF:FF:FF:FF:FF
+     * Create an invalid mac address, equal to 00:00:00:00:00:00
      */
     address_t() {
-        memset(_value, 0xFF, sizeof(_value));
+        memset(_value, 0x00, sizeof(_value));
     }
 
     /**

features/FEATURE_BLE/ble/Gap.h

@@ -887,6 +887,12 @@ public:
      */
     static const uint16_t UNIT_1_25_MS  = 1250;
 
+    static const PeripheralPrivacyConfiguration_t
+        default_peripheral_privacy_configuration;
+
+    static const CentralPrivacyConfiguration_t
+        default_central_privacy_configuration;
+
     /**
      * Convert milliseconds into 1.25ms units.
      *
@@ -1062,7 +1068,7 @@ public:
      * the address in input was not identifiable as a random address.
      */
     static ble_error_t getRandomAddressType(
-        BLEProtocol::AddressBytes_t address,
+        const BLEProtocol::AddressBytes_t address,
         RandomAddressType_t* addressType
     );
 

features/FEATURE_BLE/ble/generic/GenericSecurityManager.h

@@ -25,6 +25,7 @@
 #include "ble/pal/SigningEventMonitor.h"
 #include "ble/generic/GenericGap.h"
 #include "ble/pal/PalSecurityManager.h"
+#include "ble/ArrayView.h"
 
 namespace ble {
 namespace generic {
@@ -400,21 +401,28 @@ private:
     );
 
     /**
-     * Inform the security manager of a new connection.
+     * Callback invoked by the secure DB when an identity entry has been
-     *
+     * retrieved.
-     * @param[in] params information about the new connection.
+     * @param entry Handle of the entry.
+     * @param identity The identity associated with the entry; may be NULL.
      */
-    void connection_callback(
+    void on_security_entry_retrieved(
-        const Gap::ConnectionCallbackParams_t* params
+        pal::SecurityDb::entry_handle_t entry,
+        const pal::SecurityEntryIdentity_t* identity
     );
 
     /**
-     * Inform the security manager that a connection ended.
+     * Callback invoked by the secure DB when the identity list has been
+     * retrieved.
      *
-     * @param[in] params handle and reason of the disconnection.
+     * @param identity View to the array passed to the secure DB. It contains
+     * identity entries retrieved.
+     *
+     * @param count Number of identities entries retrieved.
      */
-    void disconnection_callback(
+    void on_identity_list_retrieved(
-        const Gap::DisconnectionCallbackParams_t* params
+        ble::ArrayView<pal::SecurityEntryIdentity_t*>& identity_list,
+        size_t count
     );
 
 private:

features/FEATURE_BLE/ble/pal/MemorySecurityDb.h

@@ -17,7 +17,8 @@
 #ifndef PAL_MEMORY_SECURITY_DB_H_
 #define PAL_MEMORY_SECURITY_DB_H_
 
-#include "SecurityDB.h"
+#include "ble/Gap.h"
+#include "SecurityDb.h"
 
 namespace ble {
 namespace pal {
@@ -49,8 +50,9 @@ private:
     }
 
 public:
-    MemorySecurityDb() { };
+    MemorySecurityDb() : _local_sign_counter(0) { }
-    virtual ~MemorySecurityDb() { };
+
+    virtual ~MemorySecurityDb() { }
 
     virtual const SecurityDistributionFlags_t* get_distribution_flags(
         entry_handle_t entry_handle
@@ -173,6 +175,35 @@ public:
         cb(entry_handle, key);
     }
 
+    virtual void get_entry_identity(
+        SecurityEntryIdentityDbCb_t cb,
+        entry_handle_t entry_handle
+    ) {
+        entry_t *entry = as_entry(entry_handle);
+        if (entry && entry->flags.irk_stored) {
+            cb(entry_handle, &entry->peer_identity);
+        } else {
+            cb(entry_handle, NULL);
+        }
+    }
+
+    virtual void get_identity_list(
+        IdentitylistDbCb_t cb,
+        ArrayView<SecurityEntryIdentity_t*>& entries
+    ) {
+        size_t count = 0;
+        for (size_t i = 0; i < MAX_ENTRIES && count < entries.size(); ++i) {
+            entry_t& e = _entries[i];
+
+            if (e.state == ENTRY_WRITTEN && e.flags.irk_stored) {
+                entries[count] = &e.peer_identity;
+                ++count;
+            }
+        }
+
+        cb(entries, count);
+    }
+
     /* set */
 
     virtual void set_entry_peer_ltk(
@@ -207,6 +238,7 @@ public:
         if (entry) {
             entry->state = ENTRY_WRITTEN;
             entry->peer_identity.irk = irk;
+            entry->flags.irk_stored = true;
         }
     }
 
@@ -219,6 +251,7 @@ public:
         if (entry) {
             entry->state = ENTRY_WRITTEN;
             entry->peer_identity.identity_address = peer_address;
+            entry->peer_identity.identity_address_is_public = address_is_public;
         }
     }
 
@@ -271,14 +304,46 @@ public:
         const address_t &peer_address
     ) {
         const bool peer_address_public =
-            (peer_address_type == BLEProtocol::AddressType::PUBLIC);
+            (peer_address_type == BLEProtocol::AddressType::PUBLIC) ||
+            (peer_address_type == BLEProtocol::AddressType::PUBLIC_IDENTITY);
 
         for (size_t i = 0; i < MAX_ENTRIES; i++) {
-            if (_entries[i].state == ENTRY_FREE) {
+            entry_t& e = _entries[i];
+
+            if (e.state == ENTRY_FREE) {
                 continue;
-            } else if (peer_address == _entries[i].peer_identity.identity_address
+            } else {
-                && _entries[i].flags.peer_address_is_public == peer_address_public) {
+                if (peer_address_type == BLEProtocol::AddressType::PUBLIC_IDENTITY &&
-                return &_entries[i];
+                    e.flags.irk_stored == false
+                ) {
+                    continue;
+                }
+
+                // lookup for the identity address then the connection address.
+                if (e.flags.irk_stored &&
+                    e.peer_identity.identity_address == peer_address &&
+                    e.peer_identity.identity_address_is_public == peer_address_public
+                ) {
+                    return &e;
+                // lookup for connection address used during bonding
+                } else if (e.flags.peer_address == peer_address &&
+                           e.flags.peer_address_is_public == peer_address_public
+                ) {
+                    return &e;
+                }
+            }
+        }
+
+        // determine if the address in input is private or not.
+        bool is_private_address = false;
+        if (peer_address_type == BLEProtocol::AddressType::RANDOM) {
+            ::Gap::RandomAddressType_t random_type(::Gap::RandomAddressType_t::STATIC);
+            ble_error_t err = ::Gap::getRandomAddressType(peer_address.data(), &random_type);
+            if (err) {
+                return NULL;
+            }
+            if (random_type != ::Gap::RandomAddressType_t::STATIC) {
+                is_private_address = true;
             }
         }
 
@@ -286,8 +351,14 @@ public:
         for (size_t i = 0; i < MAX_ENTRIES; i++) {
             if (_entries[i].state == ENTRY_FREE) {
                 _entries[i] = entry_t();
-                _entries[i].flags.peer_address = peer_address;
+                // do not store private addresses in the flags; just store public
-                _entries[i].flags.peer_address_is_public = peer_address_public;
+                // or random static address so it can be reused latter.
+                if (is_private_address == false) {
+                    _entries[i].flags.peer_address = peer_address;
+                    _entries[i].flags.peer_address_is_public = peer_address_public;
+                } else {
+                    _entries[i].flags.peer_address = address_t();
+                }
                 _entries[i].state = ENTRY_RESERVED;
                 return &_entries[i];
             }

features/FEATURE_BLE/ble/pal/SecurityDb.h

@@ -35,12 +35,12 @@ struct SecurityDistributionFlags_t {
         peer_address(),
         encryption_key_size(0),
         peer_address_is_public(false),
-        local_address_is_public(false),
         csrk_stored(false),
         csrk_mitm_protected(false),
         ltk_stored(false),
         ltk_mitm_protected(false),
-        secure_connections_paired(false) {
+        secure_connections_paired(false),
+        irk_stored(false) {
     }
 
     /** peer address */
@@ -50,8 +50,6 @@ struct SecurityDistributionFlags_t {
     uint8_t encryption_key_size;
     /** true if peer address is public, false if it's static random */
     uint8_t peer_address_is_public:1;
-    /** true if local address is public, false if it's static random */
-    uint8_t local_address_is_public:1;
 
     /** CSRK (Connection Signature Resolving Key) has been distributed and stored */
     uint8_t csrk_stored:1;
@@ -63,6 +61,8 @@ struct SecurityDistributionFlags_t {
     uint8_t ltk_mitm_protected:1;
     /** the current pairing was done using Secure Connections */
     uint8_t secure_connections_paired:1;
+    /** the security entry has been distributed and stored */
+    uint8_t irk_stored:1;
 };
 
 /** Long Term Key and data used to identify it */
@@ -81,6 +81,8 @@ struct SecurityEntryIdentity_t {
     address_t identity_address;
     /** Identity Resolving Key */
     irk_t irk;
+    /** true if peer identity address is public, false if it's static random */
+    uint8_t identity_address_is_public:1;
 };
 
 /**
@@ -103,6 +105,10 @@ public:
         SecurityEntryKeysDbCb_t;
     typedef mbed::Callback<void(entry_handle_t, const csrk_t*, uint32_t sign_counter)>
         SecurityEntryCsrkDbCb_t;
+    typedef mbed::Callback<void(entry_handle_t, const SecurityEntryIdentity_t*)>
+        SecurityEntryIdentityDbCb_t;
+    typedef mbed::Callback<void(ArrayView<SecurityEntryIdentity_t*>&, size_t count)>
+        IdentitylistDbCb_t;
     typedef mbed::Callback<void(::Gap::Whitelist_t*)>
         WhitelistDbCb_t;
 
@@ -259,6 +265,31 @@ public:
         const address_t &peer_address
     ) = 0;
 
+    /**
+     * Retrieve stored identity address and IRK.
+     *
+     * @param[in] cb callback that will receive the SecurityEntryIdentity_t struct
+     * @param[in] db_entry handle of the entry being queried.
+     */
+    virtual void get_entry_identity(
+        SecurityEntryIdentityDbCb_t cb,
+        entry_handle_t db_entry
+    ) = 0;
+
+    /**
+     * Asynchronously return the identity list stored in NVM through a callback.
+     * Function takes ownership of the memory. The identity list and the
+     * ownership will be returned in the callback.
+     *
+     * @param[in] cb callback that will receive the whitelist
+     * @param[in] identity_list preallocated identity_list that will be filled
+     * in.
+     */
+    virtual void get_identity_list(
+        IdentitylistDbCb_t cb,
+        ArrayView<SecurityEntryIdentity_t*>& identity_list
+    ) = 0;
+
     /**
      * Update peer signing key.
      *

features/FEATURE_BLE/source/Gap.cpp

@@ -17,7 +17,7 @@
 #include "ble/Gap.h"
 
 ble_error_t Gap::getRandomAddressType(
-    BLEProtocol::AddressBytes_t address,
+    const BLEProtocol::AddressBytes_t address,
     RandomAddressType_t* type
 ) {
     // see section Device address in Bluetooth Link Layer specification
@@ -36,3 +36,13 @@ ble_error_t Gap::getRandomAddressType(
             return BLE_ERROR_INVALID_PARAM;
     }
 }
+
+const Gap::PeripheralPrivacyConfiguration_t Gap::default_peripheral_privacy_configuration = {
+    /* use_non_resolvable_random_address */ false,
+    /* resolution_strategy */ PeripheralPrivacyConfiguration_t::PERFORM_PAIRING_PROCEDURE
+};
+
+const Gap::CentralPrivacyConfiguration_t Gap::default_central_privacy_configuration = {
+    /* use_non_resolvable_random_address */ false,
+    /* resolution_strategy */ CentralPrivacyConfiguration_t::DO_NOT_RESOLVE
+};

features/FEATURE_BLE/source/generic/GenericSecurityManager.cpp

@@ -74,6 +74,22 @@ ble_error_t GenericSecurityManager::init(
     _signing_monitor.set_signing_event_handler(this);
     _pal.set_event_handler(this);
 
+    uint8_t resolving_list_capacity = _pal.read_resolving_list_capacity();
+    pal::SecurityEntryIdentity_t** identity_list_p =
+        new (std::nothrow) pal::SecurityEntryIdentity_t*[resolving_list_capacity];
+
+    if (identity_list_p) {
+        ArrayView<pal::SecurityEntryIdentity_t*> identity_list(
+            identity_list_p,
+            resolving_list_capacity
+        );
+
+        _db.get_identity_list(
+            mbed::callback(this, &GenericSecurityManager::on_identity_list_retrieved),
+            identity_list
+        );
+    }
+
     return BLE_ERROR_NONE;
 }
 
@@ -866,6 +882,46 @@ void GenericSecurityManager::on_disconnected(
     _db.sync();
 }
 
+void GenericSecurityManager::on_security_entry_retrieved(
+    pal::SecurityDb::entry_handle_t entry,
+    const pal::SecurityEntryIdentity_t* identity
+) {
+    if (!identity) {
+        return;
+    }
+
+    typedef advertising_peer_address_type_t address_type_t;
+
+    _pal.add_device_to_resolving_list(
+        identity->identity_address_is_public ?
+            address_type_t::PUBLIC_ADDRESS :
+            address_type_t::RANDOM_ADDRESS,
+        identity->identity_address,
+        identity->irk
+    );
+}
+
+void GenericSecurityManager::on_identity_list_retrieved(
+    ble::ArrayView<pal::SecurityEntryIdentity_t*>& identity_list,
+    size_t count
+) {
+    typedef advertising_peer_address_type_t address_type_t;
+
+    _pal.clear_resolving_list();
+    for (size_t i = 0; i < count; ++i) {
+        _pal.add_device_to_resolving_list(
+            identity_list[i]->identity_address_is_public ?
+                address_type_t::PUBLIC_ADDRESS :
+                address_type_t::RANDOM_ADDRESS,
+            identity_list[i]->identity_address,
+            identity_list[i]->irk
+        );
+    }
+
+    delete [] identity_list.data();
+}
+
+
 /* Implements ble::pal::SecurityManagerEventHandler */
 
 ////////////////////////////////////////////////////////////////////////////
@@ -937,6 +993,10 @@ void GenericSecurityManager::on_pairing_completed(connection_handle_t connection
     if (cb) {
         // set the distribution flags in the db
         _db.set_distribution_flags(cb->db_entry, *cb);
+        _db.get_entry_identity(
+            mbed::callback(this, &GenericSecurityManager::on_security_entry_retrieved),
+            cb->db_entry
+        );
     }
 
     eventHandler->pairingResult(

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

@@ -61,7 +61,7 @@ void            app_error_handler(uint32_t error_code, uint32_t line_num, const
 extern "C" void SD_EVT_IRQHandler(void); // export the softdevice event handler for registration by nvic-set-vector.
 
 
-static void btle_handler(ble_evt_t *p_ble_evt);
+void btle_handler(ble_evt_t *p_ble_evt);
 
 static void sys_evt_dispatch(uint32_t sys_evt)
 {
@@ -161,7 +161,7 @@ error_t btle_init(void)
     return btle_gap_init();
 }
 
-static void btle_handler(ble_evt_t *p_ble_evt)
+void btle_handler(ble_evt_t *p_ble_evt)
 {
     using ble::pal::vendor::nordic::nRF5XGattClient;
     using ble::pal::vendor::nordic::nRF5xSecurityManager;
@@ -189,38 +189,12 @@ static void btle_handler(ble_evt_t *p_ble_evt)
 
     /* Custom event handler */
     switch (p_ble_evt->header.evt_id) {
-        case BLE_GAP_EVT_CONNECTED: {
+        case BLE_GAP_EVT_CONNECTED:
-            Gap::Handle_t handle = p_ble_evt->evt.gap_evt.conn_handle;
+            gap.on_connection(
-#if defined(TARGET_MCU_NRF51_16K_S110) || defined(TARGET_MCU_NRF51_32K_S110)
+                p_ble_evt->evt.gap_evt.conn_handle,
-            /* Only peripheral role is supported by S110 */
+                p_ble_evt->evt.gap_evt.params.connected
-            Gap::Role_t role = Gap::PERIPHERAL;
+            );
-#else
-            Gap::Role_t role = static_cast<Gap::Role_t>(p_ble_evt->evt.gap_evt.params.connected.role);
-#endif
-            gap.setConnectionHandle(handle);
-            const Gap::ConnectionParams_t *params = reinterpret_cast<Gap::ConnectionParams_t *>(&(p_ble_evt->evt.gap_evt.params.connected.conn_params));
-            const ble_gap_addr_t *peer = &p_ble_evt->evt.gap_evt.params.connected.peer_addr;
-#if  (NRF_SD_BLE_API_VERSION <= 2)
-            const ble_gap_addr_t *own  = &p_ble_evt->evt.gap_evt.params.connected.own_addr;
-
-            gap.processConnectionEvent(handle,
-                                       role,
-                                       static_cast<BLEProtocol::AddressType_t>(peer->addr_type), peer->addr,
-                                       static_cast<BLEProtocol::AddressType_t>(own->addr_type),  own->addr,
-                                       params);
-#else
-            Gap::AddressType_t addr_type;
-            Gap::Address_t     own_address;
-            gap.getAddress(&addr_type, own_address);
-
-            gap.processConnectionEvent(handle,
-                                       role,
-                                       static_cast<BLEProtocol::AddressType_t>(peer->addr_type), peer->addr,
-                                       addr_type,  own_address,
-                                       params);
-#endif
             break;
-        }
 
         case BLE_GAP_EVT_DISCONNECTED: {
             Gap::Handle_t handle = p_ble_evt->evt.gap_evt.conn_handle;
@@ -266,17 +240,9 @@ static void btle_handler(ble_evt_t *p_ble_evt)
             // BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION));
             break;
 
-        case BLE_GAP_EVT_ADV_REPORT: {
+        case BLE_GAP_EVT_ADV_REPORT:
-            const ble_gap_evt_adv_report_t *advReport = &p_ble_evt->evt.gap_evt.params.adv_report;
+            gap.on_advertising_packet(p_ble_evt->evt.gap_evt.params.adv_report);
-            gap.processAdvertisementReport(advReport->peer_addr.addr,
-                                           advReport->rssi,
-                                           advReport->scan_rsp,
-                                           static_cast<GapAdvertisingParams::AdvertisingType_t>(advReport->type),
-                                           advReport->dlen,
-                                           advReport->data,
-                                           static_cast<BLEProtocol::AddressType_t>(advReport->peer_addr.addr_type));
             break;
-        }
 
         default:
             break;

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

@@ -37,6 +37,8 @@
 #include "cmsis.h"
 #include "nRF5xCrypto.h"
 #include "platform/mbed_assert.h"
+#include "nrf_soc.h"
+
 
 namespace ble {
 namespace pal {
@@ -128,6 +130,36 @@ bool CryptoToolbox::generate_shared_secret(
     return err ? false : true;
 }
 
+bool CryptoToolbox::ah(
+    const ArrayView<const uint8_t, irk_size_>& irk,
+    const ArrayView<const uint8_t, prand_size_>& prand,
+    ArrayView<uint8_t, hash_size_> hash
+) {
+    // Note copy then swap operation can be optimized.
+
+    // Note: the encryption block works in big endian; go figure.
+    nrf_ecb_hal_data_t ecb_hal_data;
+
+    memcpy(ecb_hal_data.key, irk.data(), irk.size());
+    swap_endian(ecb_hal_data.key, sizeof(ecb_hal_data.key));
+
+    memcpy(ecb_hal_data.cleartext, prand.data(), prand.size());
+    memset(ecb_hal_data.cleartext + prand.size(), 0, sizeof(ecb_hal_data.cleartext) - prand.size());
+    swap_endian(ecb_hal_data.cleartext, sizeof(ecb_hal_data.cleartext));
+
+    uint32_t err = sd_ecb_block_encrypt(&ecb_hal_data);
+
+    if (err) {
+        return false;
+    }
+
+    swap_endian(ecb_hal_data.ciphertext, sizeof(ecb_hal_data.ciphertext));
+
+    memcpy(hash.data(), ecb_hal_data.ciphertext, hash.size());
+
+    return true;
+}
+
 
 void CryptoToolbox::load_mpi(mbedtls_mpi& dest, const ArrayView<const uint8_t, lesc_key_size_>& src) {
     ble::public_key_coord_t src_be = src.data();

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

@@ -48,6 +48,21 @@ public:
      */
     static const ptrdiff_t lesc_key_size_ = public_key_coord_t::size_;
 
+    /**
+     * Size of an IRK.
+     */
+    static const ptrdiff_t irk_size_ = irk_t::size_;
+
+    /**
+     * Size of the hash generated by ah.
+     */
+    static const ptrdiff_t hash_size_ = 3;
+
+    /**
+     * Size of prand.
+     */
+    static const ptrdiff_t prand_size_ = 3;
+
     /**
      * Create a new CryptoToolbox.
      */
@@ -88,6 +103,25 @@ public:
         ArrayView<uint8_t, lesc_key_size_> shared_secret
     );
 
+    /**
+     * Execute the function ah. This function can be used to generate private
+     * resolvable addresses and resolve them.
+     *
+     * @note all parameters passed and return by this fucntion are in little
+     * endian.
+     *
+     * @param[in] irk The key used to create hash.
+     * @param[in] prand The random part from which the hash will be generated.
+     * @param[out] hash The hash generated.
+     *
+     * @return true in case of success and false otherwise.
+     */
+    bool ah(
+        const ArrayView<const uint8_t, irk_size_>& irk,
+        const ArrayView<const uint8_t, prand_size_>& prand,
+        ArrayView<uint8_t, hash_size_> hash
+    );
+
 private:
     void load_mpi(mbedtls_mpi& dest, const ArrayView<const uint8_t, lesc_key_size_>& src);
 

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

@@ -26,7 +26,66 @@
 #include "ble_advdata.h"
 #include "headers/nrf_ble_hci.h"
 #include "ble/pal/ConnectionEventMonitor.h"
+#include "nRF5XPalSecurityManager.h"
 
+using ble::pal::vendor::nordic::nRF5xSecurityManager;
+typedef nRF5xSecurityManager::resolving_list_entry_t resolving_list_entry_t;
+using ble::ArrayView;
+using ble::pal::advertising_peer_address_type_t;
+
+namespace {
+
+nRF5xSecurityManager& get_sm() {
+    return nRF5xSecurityManager::get_security_manager();
+}
+
+void set_private_resolvable_address() {
+    ble_gap_addr_t addr = { BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE };
+    sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_AUTO, &addr);
+}
+
+void set_private_non_resolvable_address() {
+    ble_gap_addr_t addr = { BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE };
+    sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_AUTO, &addr);
+}
+
+bool is_advertising_non_connectable(const GapAdvertisingParams &params) {
+    switch (params.getAdvertisingType()) {
+        case GapAdvertisingParams::ADV_SCANNABLE_UNDIRECTED:
+        case GapAdvertisingParams::ADV_NON_CONNECTABLE_UNDIRECTED:
+            return true;
+        default:
+            return false;
+    }
+}
+
+bool is_identity_address(BLEProtocol::AddressType_t address_type) {
+    switch (address_type) {
+        case BLEProtocol::AddressType::PUBLIC_IDENTITY:
+        case BLEProtocol::AddressType::RANDOM_STATIC_IDENTITY:
+            return true;
+        default:
+            return false;
+    }
+}
+
+BLEProtocol::AddressType_t convert_nordic_address(uint8_t address) {
+    if (address == BLE_GAP_ADDR_TYPE_PUBLIC) {
+        return BLEProtocol::AddressType::PUBLIC;
+    } else {
+        return BLEProtocol::AddressType::RANDOM;
+    }
+}
+
+BLEProtocol::AddressType_t convert_identity_address(advertising_peer_address_type_t address) {
+    if (address == advertising_peer_address_type_t::PUBLIC_ADDRESS) {
+        return BLEProtocol::AddressType::PUBLIC_IDENTITY;
+    } else {
+        return BLEProtocol::AddressType::RANDOM_STATIC_IDENTITY;
+    }
+}
+
+} // namespace
 
 void radioNotificationStaticCallback(bool param) {
     nRF5xGap &gap = (nRF5xGap &) nRF5xn::Instance(BLE::DEFAULT_INSTANCE).getGap();
@@ -40,7 +99,11 @@ nRF5xGap::nRF5xGap() : Gap(),
     whitelistAddresses(),
     radioNotificationCallbackParam(false),
     radioNotificationTimeout(),
-    _connection_event_handler(NULL)
+    _connection_event_handler(NULL),
+    _privacy_enabled(false),
+    _peripheral_privacy_configuration(default_peripheral_privacy_configuration),
+    _central_privacy_configuration(default_central_privacy_configuration),
+    _non_private_type(BLEProtocol::AddressType::RANDOM)
 {
         m_connectionHandle = BLE_CONN_HANDLE_INVALID;
 }
@@ -204,6 +267,29 @@ ble_error_t nRF5xGap::startAdvertising(const GapAdvertisingParams &params)
         }
     }
 
+    if (_privacy_enabled) {
+        if (_peripheral_privacy_configuration.resolution_strategy != PeripheralPrivacyConfiguration_t::DO_NOT_RESOLVE) {
+            ArrayView<resolving_list_entry_t> entries = get_sm().get_resolving_list();
+
+            size_t limit = std::min(
+                entries.size(), (size_t) YOTTA_CFG_IRK_TABLE_MAX_SIZE
+            );
+
+            for (size_t i = 0; i < limit; ++i) {
+                whitelistIrkPtrs[i] = (ble_gap_irk_t*) entries[i].peer_irk.data();
+            }
+            whitelist.irk_count = limit;
+        }
+
+        if (_peripheral_privacy_configuration.use_non_resolvable_random_address &&
+            is_advertising_non_connectable(params)
+        ) {
+            set_private_non_resolvable_address();
+        } else {
+            set_private_resolvable_address();
+        }
+    }
+
     adv_para.p_whitelist = &whitelist;
 #endif
     /* For NRF_SD_BLE_API_VERSION >= 3 nRF5xGap::setWhitelist setups the whitelist. */
@@ -255,6 +341,8 @@ ble_error_t nRF5xGap::startRadioScan(const GapScanningParams &scanningParams)
         }
     }
 
+    // FIXME: fill the irk list once addresses are resolved by the softdevice.
+
     scanParams.selective   = scanningPolicyMode;    /**< If 1, ignore unknown devices (non whitelisted). */
     scanParams.p_whitelist = &whitelist; /**< Pointer to whitelist, NULL if none is given. */
 #else
@@ -270,6 +358,14 @@ ble_error_t nRF5xGap::startRadioScan(const GapScanningParams &scanningParams)
     scanParams.window      = scanningParams.getWindow();    /**< Scan window between 0x0004 and 0x4000 in 0.625ms units (2.5ms to 10.24s). */
     scanParams.timeout     = scanningParams.getTimeout();   /**< Scan timeout between 0x0001 and 0xFFFF in seconds, 0x0000 disables timeout. */
 
+    if (_privacy_enabled) {
+        if (_central_privacy_configuration.use_non_resolvable_random_address) {
+            set_private_non_resolvable_address();
+        } else {
+            set_private_resolvable_address();
+        }
+    }
+
     if (sd_ble_gap_scan_start(&scanParams) != NRF_SUCCESS) {
         return BLE_ERROR_PARAM_OUT_OF_RANGE;
     }
@@ -318,6 +414,7 @@ ble_error_t nRF5xGap::connect(const Address_t             peerAddr,
                               const GapScanningParams    *scanParamsIn)
 {
     ble_gap_addr_t addr;
+    ble_gap_addr_t* addr_ptr = &addr;
     addr.addr_type = peerAddrType;
     memcpy(addr.addr, peerAddr, Gap::ADDR_LEN);
 
@@ -354,9 +451,37 @@ ble_error_t nRF5xGap::connect(const Address_t             peerAddr,
             return error;
         }
     }
-
     scanParams.selective   = scanningPolicyMode;    /**< If 1, ignore unknown devices (non whitelisted). */
     scanParams.p_whitelist = &whitelist; /**< Pointer to whitelist, NULL if none is given. */
+
+    if (_privacy_enabled) {
+        // configure the "whitelist" with the IRK associated with the identity
+        // address in input.
+        if (is_identity_address(peerAddrType)) {
+            ArrayView<resolving_list_entry_t> entries = get_sm().get_resolving_list();
+
+            size_t i;
+            for (i = 0; i < entries.size(); ++i) {
+                const ble::address_t& entry_address = entries[i].peer_identity_address;
+
+                // entry found; fill the whitelist and invalidate addr_ptr
+                if (memcmp(entry_address.data(), peerAddr, entry_address.size_) == 0) {
+                    whitelist.pp_irks[0] = (ble_gap_irk_t*) entries[i].peer_irk.data();
+                    whitelist.irk_count = 1;
+                    scanParams.selective = 1;
+                    addr_ptr = NULL;
+                    break;
+                }
+            }
+
+            // Occur only if the address in input hasn't been resolved.
+            if (i == entries.size()) {
+                return BLE_ERROR_INVALID_PARAM;
+            }
+        }
+
+        set_private_resolvable_address();
+    }
 #else
     /* For NRF_SD_BLE_API_VERSION >= 3 nRF5xGap::setWhitelist setups the whitelist. */
 
@@ -388,7 +513,7 @@ ble_error_t nRF5xGap::connect(const Address_t             peerAddr,
         scanParams.timeout     = _scanningParams.getTimeout();        /**< Scan timeout between 0x0001 and 0xFFFF in seconds, 0x0000 disables timeout. */
     }
 
-    uint32_t rc = sd_ble_gap_connect(&addr, &scanParams, &connParams);
+    uint32_t rc = sd_ble_gap_connect(addr_ptr, &scanParams, &connParams);
     if (rc == NRF_SUCCESS) {
         return BLE_ERROR_NONE;
     }
@@ -543,52 +668,33 @@ uint16_t nRF5xGap::getConnectionHandle(void)
 /**************************************************************************/
 ble_error_t nRF5xGap::setAddress(AddressType_t type, const Address_t address)
 {
-#if  (NRF_SD_BLE_API_VERSION <= 2)
+    using BLEProtocol::AddressType;
-    uint8_t cycle_mode;
-#else
-    ble_gap_privacy_params_t privacy_params = {0};
-#endif
 
+    if (type != AddressType::PUBLIC || type != AddressType::RANDOM_STATIC) {
+        return BLE_ERROR_INVALID_PARAM;
+    }
+
+    if (_privacy_enabled) {
+        return BLE_ERROR_INVALID_STATE;
+    }
 
     ble_gap_addr_t dev_addr;
+    memcpy(dev_addr.addr, address, ADDR_LEN);
+    if (type == AddressType::PUBLIC) {
+        dev_addr.addr_type = BLE_GAP_ADDR_TYPE_PUBLIC;
+    } else {
+        dev_addr.addr_type = BLE_GAP_ADDR_TYPE_RANDOM_STATIC;
+    }
 
-    /* When using Public or Static addresses, the cycle mode must be None.
-       When using Random Private addresses, the cycle mode must be Auto.
-       In auto mode, the given address is ignored.
-    */
-    if ((type == BLEProtocol::AddressType::PUBLIC) || (type == BLEProtocol::AddressType::RANDOM_STATIC))
-    {
-    	memcpy(dev_addr.addr, address, ADDR_LEN);
 #if  (NRF_SD_BLE_API_VERSION <= 2)
-        cycle_mode = BLE_GAP_ADDR_CYCLE_MODE_NONE;
+    ASSERT_INT(ERROR_NONE, sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_NONE, &dev_addr), BLE_ERROR_PARAM_OUT_OF_RANGE);
 #else
-        privacy_params.privacy_mode = BLE_GAP_PRIVACY_MODE_OFF;
+    // FIXME is there any reason to use the pm ?
-        dev_addr.addr_type = type;
+    ble_gap_privacy_params_t privacy_params = {0};
+    privacy_params.privacy_mode = BLE_GAP_PRIVACY_MODE_OFF;
 
-        ASSERT_INT(ERROR_NONE, pm_id_addr_set(&dev_addr), BLE_ERROR_PARAM_OUT_OF_RANGE);
+    ASSERT_INT(ERROR_NONE, pm_id_addr_set(&dev_addr), BLE_ERROR_PARAM_OUT_OF_RANGE);
-        ASSERT_INT(ERROR_NONE, pm_privacy_set(&privacy_params), BLE_ERROR_PARAM_OUT_OF_RANGE);
+    ASSERT_INT(ERROR_NONE, pm_privacy_set(&privacy_params), BLE_ERROR_PARAM_OUT_OF_RANGE);
-#endif
-    }
-    else if ((type == BLEProtocol::AddressType::RANDOM_PRIVATE_RESOLVABLE) || (type == BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE))
-    {
-#if  (NRF_SD_BLE_API_VERSION <= 2)
-        cycle_mode = BLE_GAP_ADDR_CYCLE_MODE_AUTO;
-#else
-        privacy_params.privacy_mode = BLE_GAP_PRIVACY_MODE_DEVICE_PRIVACY;
-        privacy_params.private_addr_type = type;
-
-        ASSERT_INT(ERROR_NONE, pm_privacy_set(&privacy_params), BLE_ERROR_PARAM_OUT_OF_RANGE);
-#endif
-        // address is ignored when in auto mode
-    }
-    else
-    {
-        return BLE_ERROR_PARAM_OUT_OF_RANGE;
-    }
-
-#if  (NRF_SD_BLE_API_VERSION <= 2)
-    dev_addr.addr_type = type;
-    ASSERT_INT(ERROR_NONE, sd_ble_gap_address_set(cycle_mode, &dev_addr), BLE_ERROR_PARAM_OUT_OF_RANGE);
 #endif
 
     return BLE_ERROR_NONE;
@@ -597,6 +703,7 @@ ble_error_t nRF5xGap::setAddress(AddressType_t type, const Address_t address)
 ble_error_t nRF5xGap::getAddress(AddressType_t *typeP, Address_t address)
 {
     ble_gap_addr_t dev_addr;
+
 #if  (NRF_SD_BLE_API_VERSION <= 2)
     if (sd_ble_gap_address_get(&dev_addr) != NRF_SUCCESS) {
 #else
@@ -606,11 +713,17 @@ ble_error_t nRF5xGap::getAddress(AddressType_t *typeP, Address_t address)
     }
 
     if (typeP != NULL) {
-        *typeP = static_cast<AddressType_t>(dev_addr.addr_type);
+        if (dev_addr.addr_type == BLE_GAP_ADDR_TYPE_PUBLIC){
+            *typeP = BLEProtocol::AddressType::PUBLIC;
+        } else {
+            *typeP = BLEProtocol::AddressType::RANDOM;
+        }
     }
+
     if (address != NULL) {
         memcpy(address, dev_addr.addr, ADDR_LEN);
     }
+
     return BLE_ERROR_NONE;
 }
 
@@ -939,6 +1052,55 @@ Gap::InitiatorPolicyMode_t nRF5xGap::getInitiatorPolicyMode(void) const
     return Gap::INIT_POLICY_IGNORE_WHITELIST;
 }
 
+ble_error_t nRF5xGap::enablePrivacy(bool enable)
+{
+    if (enable == _privacy_enabled) {
+        return BLE_ERROR_NONE;
+    }
+
+    ble_error_t err = BLE_ERROR_UNSPECIFIED;
+    if (enable == false) {
+        err = setAddress(_non_private_type, _non_private_address);
+    } else {
+        err = getAddress(&_non_private_type, _non_private_address);
+    }
+
+    if (err) {
+        return err;
+    }
+
+    _privacy_enabled = enable;
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t nRF5xGap::setPeripheralPrivacyConfiguration(
+    const PeripheralPrivacyConfiguration_t *configuration
+) {
+    _peripheral_privacy_configuration = *configuration;
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t nRF5xGap::getPeripheralPrivacyConfiguration(
+    PeripheralPrivacyConfiguration_t *configuration
+) {
+    *configuration = _peripheral_privacy_configuration;
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t nRF5xGap::setCentralPrivacyConfiguration(
+    const CentralPrivacyConfiguration_t *configuration
+) {
+    _central_privacy_configuration = *configuration;
+    return BLE_ERROR_NONE;
+}
+
+ble_error_t nRF5xGap::getCentralPrivacyConfiguration(
+    CentralPrivacyConfiguration_t *configuration
+) {
+    *configuration = _central_privacy_configuration;
+    return BLE_ERROR_NONE;
+}
+
 #if  (NRF_SD_BLE_API_VERSION <= 2)
 /**************************************************************************/
 /*!
@@ -1055,38 +1217,6 @@ void nRF5xGap::set_connection_event_handler(
     _connection_event_handler = connection_event_handler;
 }
 
-void nRF5xGap::processConnectionEvent(
-    Handle_t handle,
-    Role_t role,
-    BLEProtocol::AddressType_t peerAddrType,
-    const BLEProtocol::AddressBytes_t peerAddr,
-    BLEProtocol::AddressType_t ownAddrType,
-    const BLEProtocol::AddressBytes_t ownAddr,
-    const ConnectionParams_t *connectionParams
-) {
-    if (_connection_event_handler) {
-        _connection_event_handler->on_connected(
-            handle,
-            role,
-            peerAddrType,
-            peerAddr,
-            ownAddrType,
-            ownAddr,
-            connectionParams
-        );
-    }
-
-    ::Gap::processConnectionEvent(
-        handle,
-        role,
-        peerAddrType,
-        peerAddr,
-        ownAddrType,
-        ownAddr,
-        connectionParams
-   );
-}
-
 void nRF5xGap::processDisconnectionEvent(
     Handle_t handle,
     DisconnectionReason_t reason
@@ -1103,3 +1233,155 @@ void nRF5xGap::processDisconnectionEvent(
         reason
     );
 }
+
+void nRF5xGap::on_connection(Gap::Handle_t handle, const ble_gap_evt_connected_t& evt) {
+    using BLEProtocol::AddressType;
+
+    // set the new connection handle as the _default_ handle in gap
+    setConnectionHandle(handle);
+
+    // deal with own address
+    AddressType_t own_addr_type;
+    Address_t own_address;
+    const uint8_t* own_resolvable_address = NULL;
+
+#if  (NRF_SD_BLE_API_VERSION <= 2)
+    if (evt.own_addr.addr_type == BLE_GAP_ADDR_TYPE_PUBLIC) {
+        own_addr_type = AddressType::PUBLIC;
+    } else {
+        own_addr_type = AddressType::RANDOM;
+    }
+    memcpy(own_address, evt.own_addr.addr, sizeof(own_address));
+#else
+    gap.getAddress(&addr_type, own_address);
+#endif
+
+    if (_privacy_enabled) {
+        own_resolvable_address = own_address;
+    }
+
+    // deal with the peer address: If privacy is enabled then the softdevice
+    // indicates if the address has been resolved or not. If the address has
+    // been resolved then the identity address should be passed to the application.
+    // Depending on the privacy chosen by the application, connection request
+    // from privacy enabled peers may trigger a disconnection, the pairing procedure
+    // or the authentication procedure.
+    AddressType_t peer_addr_type;
+    const uint8_t* peer_address;
+    const uint8_t* peer_resolvable_address;
+
+    if (evt.irk_match) {
+        const resolving_list_entry_t* entry = get_sm().resolve_address(
+            evt.peer_addr.addr
+        );
+        MBED_ASSERT(entry == NULL);
+
+        peer_addr_type = convert_identity_address(entry->peer_identity_address_type);
+        peer_address = entry->peer_identity_address.data();
+        peer_resolvable_address = evt.peer_addr.addr;
+    } else {
+        if (_privacy_enabled &&
+            evt.role == BLE_GAP_ROLE_PERIPH &&
+            _peripheral_privacy_configuration.resolution_strategy == PeripheralPrivacyConfiguration_t::REJECT_NON_RESOLVED_ADDRESS &&
+            evt.peer_addr.addr_type == BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE &&
+            get_sm().get_resolving_list().size() > 0
+        ) {
+            // FIXME: should use BLE_HCI_AUTHENTICATION_FAILURE; not possible
+            // with the softdevice ...
+            sd_ble_gap_disconnect(handle, BLE_HCI_REMOTE_USER_TERMINATED_CONNECTION);
+            return;
+        }
+
+        peer_addr_type = convert_nordic_address(evt.peer_addr.addr_type);
+        peer_address = evt.peer_addr.addr;
+        peer_resolvable_address = NULL;
+    }
+
+    // notify internal event handler before applying the resolution strategy
+    if (_connection_event_handler) {
+        _connection_event_handler->on_connected(
+            handle,
+            static_cast<Role_t>(evt.role),
+            peer_addr_type,
+            peer_address,
+            own_addr_type,
+            own_address,
+            reinterpret_cast<const ConnectionParams_t *>(&(evt.conn_params))
+        );
+    }
+
+    // Apply authentication strategy before application notification
+    if (!evt.irk_match &&
+        _privacy_enabled &&
+        evt.role == BLE_GAP_ROLE_PERIPH &&
+        evt.peer_addr.addr_type == BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE
+    ) {
+        switch (_peripheral_privacy_configuration.resolution_strategy) {
+            case PeripheralPrivacyConfiguration_t::PERFORM_PAIRING_PROCEDURE:
+                nRF5xn::Instance(BLE::DEFAULT_INSTANCE).getSecurityManager().requestPairing(handle);
+                break;
+
+            case PeripheralPrivacyConfiguration_t::PERFORM_AUTHENTICATION_PROCEDURE:
+                // FIXME: lookup secure DB to know what to do.
+                break;
+
+            default:
+                break;
+        }
+    }
+
+    processConnectionEvent(
+        handle,
+        static_cast<Role_t>(evt.role),
+        peer_addr_type,
+        peer_address,
+        own_addr_type,
+        own_address,
+        reinterpret_cast<const ConnectionParams_t *>(&(evt.conn_params)),
+        peer_resolvable_address,
+        own_resolvable_address
+    );
+}
+
+void nRF5xGap::on_advertising_packet(const ble_gap_evt_adv_report_t &evt) {
+    using BLEProtocol::AddressType;
+
+    AddressType_t peer_addr_type;
+    const uint8_t* peer_address = evt.peer_addr.addr;
+
+    if (_privacy_enabled &&
+        evt.peer_addr.addr_type == BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE &&
+        _central_privacy_configuration.resolution_strategy != CentralPrivacyConfiguration_t::DO_NOT_RESOLVE
+    ) {
+        using ble::pal::vendor::nordic::nRF5xSecurityManager;
+
+        const resolving_list_entry_t* entry =  get_sm().resolve_address(
+            peer_address
+        );
+
+        if (entry) {
+            peer_address = entry->peer_identity_address.data();
+            peer_addr_type = convert_identity_address(entry->peer_identity_address_type);
+        } else if (_central_privacy_configuration.resolution_strategy != CentralPrivacyConfiguration_t::RESOLVE_AND_FORWARD) {
+            peer_addr_type = convert_nordic_address(evt.peer_addr.addr_type);
+        } else {
+            // filter out the packet.
+            return;
+        }
+    } else {
+        peer_addr_type = convert_nordic_address(evt.peer_addr.addr_type);
+    }
+
+    processAdvertisementReport(
+        peer_address,
+        evt.rssi,
+        evt.scan_rsp,
+        static_cast<GapAdvertisingParams::AdvertisingType_t>(evt.type),
+        evt.dlen,
+        evt.data,
+        peer_addr_type
+    );
+}
+
+
+

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

@@ -122,6 +122,24 @@ public:
         return BLE_ERROR_UNSPECIFIED;
     }
 
+    virtual ble_error_t enablePrivacy(bool enable);
+
+    virtual ble_error_t setPeripheralPrivacyConfiguration(
+        const PeripheralPrivacyConfiguration_t *configuration
+    );
+
+    virtual ble_error_t getPeripheralPrivacyConfiguration(
+        PeripheralPrivacyConfiguration_t *configuration
+    );
+
+    virtual ble_error_t setCentralPrivacyConfiguration(
+        const CentralPrivacyConfiguration_t *configuration
+    );
+
+    virtual ble_error_t getCentralPrivacyConfiguration(
+        CentralPrivacyConfiguration_t *configuration
+    );
+
 /* Observer role is not supported by S110, return BLE_ERROR_NOT_IMPLEMENTED */
 #if !defined(TARGET_MCU_NRF51_16K_S110) && !defined(TARGET_MCU_NRF51_32K_S110)
     virtual ble_error_t startRadioScan(const GapScanningParams &scanningParams);
@@ -260,23 +278,10 @@ public:
     /** @note Implements ConnectionEventMonitor.
      *  @copydoc ConnectionEventMonitor::set_connection_event_handler
      */
-    void set_connection_event_handler(
+    virtual void set_connection_event_handler(
         ConnectionEventMonitor::EventHandler* connection_event_handler
     );
 
-    /**
-     * @copydoc ::Gap::processConnectionEvent
-     */
-    void processConnectionEvent(
-        Handle_t handle,
-        Role_t role,
-        BLEProtocol::AddressType_t peerAddrType,
-        const BLEProtocol::AddressBytes_t peerAddr,
-        BLEProtocol::AddressType_t ownAddrType,
-        const BLEProtocol::AddressBytes_t ownAddr,
-        const ConnectionParams_t *connectionParams
-    );
-
     /**
      * @copydoc ::Gap::processDisconnectionEvent
      */
@@ -285,10 +290,22 @@ public:
         DisconnectionReason_t reason
     );
 
+private:
+    friend void btle_handler(ble_evt_t *p_ble_evt);
+
+    void on_connection(Handle_t handle, const ble_gap_evt_connected_t& evt);
+    void on_advertising_packet(const ble_gap_evt_adv_report_t &evt);
+
     uint16_t m_connectionHandle;
 
     ConnectionEventMonitor::EventHandler* _connection_event_handler;
 
+    bool _privacy_enabled;
+    PeripheralPrivacyConfiguration_t _peripheral_privacy_configuration;
+    CentralPrivacyConfiguration_t _central_privacy_configuration;
+    AddressType_t _non_private_type;
+    Address_t _non_private_address;
+
     /*
      * Allow instantiation from nRF5xn when required.
      */

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 MAX_RESOLVING_LIST_ENTRIES;
-    return 0;
 }
 
 ble_error_t nRF5xSecurityManager::add_device_to_resolving_list(
@@ -150,24 +150,94 @@ ble_error_t nRF5xSecurityManager::add_device_to_resolving_list(
     const address_t &peer_identity_address,
     const irk_t &peer_irk
 ) {
-    // FIXME: implement with privacy
+    if (resolving_list_entry_count >= MAX_RESOLVING_LIST_ENTRIES) {
-    return BLE_ERROR_NOT_IMPLEMENTED;
+        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
+    size_t entry_index;
-    return BLE_ERROR_NOT_IMPLEMENTED;
+
+    // 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
+    resolving_list_entry_count = 0;
-    return BLE_ERROR_NOT_IMPLEMENTED;
+    return BLE_ERROR_NONE;
 }
 
+ArrayView<nRF5xSecurityManager::resolving_list_entry_t>
+nRF5xSecurityManager::get_resolving_list() {
+    return ArrayView<nRF5xSecurityManager::resolving_list_entry_t>(
+        resolving_list,
+        resolving_list_entry_count
+    );
+}
+
+const nRF5xSecurityManager::resolving_list_entry_t*
+nRF5xSecurityManager::resolve_address(const address_t& resolvable_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) {
+            return &entry;
+        }
+    }
+    return NULL;
+}
+
+
+
 ////////////////////////////////////////////////////////////////////////////
 // Pairing
 //

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

@@ -83,6 +83,40 @@ 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.
+     */
+    ArrayView<resolving_list_entry_t> get_resolving_list();
+
+    /**
+     * Try to resolve a private resolvable address.
+     *
+     * @param resolvable_address The address to resolve.
+     *
+     * @return Pointer to the entry found if any.
+     */
+    const resolving_list_entry_t*  resolve_address(
+        const address_t& resolvable_address
+    );
+
+
     ////////////////////////////////////////////////////////////////////////////
     // Pairing
     //
@@ -351,6 +385,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