Nordic BLE: Backport Gap from nRF5

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

@@ -25,18 +25,116 @@
 #include "common/common.h"
 #include "ble_advdata.h"
 #include "headers/ble_hci.h"
+#include "ble/pal/ConnectionEventMonitor.h"
+#include "nRF5xPalSecurityManager.h"
 
-#if  (NRF_SD_BLE_API_VERSION >= 3)
+using ble::pal::vendor::nordic::nRF5xSecurityManager;
-    #include "peer_manager.h"
+typedef nRF5xSecurityManager::resolving_list_entry_t resolving_list_entry_t;
-    #include "peer_data_storage.h"
+using ble::ArrayView;
+using ble::pal::advertising_peer_address_type_t;
+
+namespace {
+
+nRF5xSecurityManager& get_sm() {
+    return nRF5xSecurityManager::get_security_manager();
+}
+
+ble_error_t set_private_resolvable_address() {
+#if (NRF_SD_BLE_API_VERSION <= 2)
+    ble_gap_addr_t addr = {
+        BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE
+    };
+
+    sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_AUTO, &addr);
+    return BLE_ERROR_NONE;
+#else
+    ble_gap_privacy_params_t privacy_config = { 0 };
+    uint32_t err = sd_ble_gap_privacy_get(&privacy_config);
+    if (err) {
+        return BLE_ERROR_UNSPECIFIED;
+    }
+
+    privacy_config.private_addr_type = BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE;
+    err = sd_ble_gap_privacy_set(&privacy_config);
+    return err ? BLE_ERROR_UNSPECIFIED : BLE_ERROR_NONE;
 #endif
+}
 
+ble_error_t set_private_non_resolvable_address() {
+#if (NRF_SD_BLE_API_VERSION <= 2)
+    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);
+#else
+    ble_gap_privacy_params_t privacy_config = { 0 };
+    uint32_t err = sd_ble_gap_privacy_get(&privacy_config);
+    if (err) {
+        return BLE_ERROR_UNSPECIFIED;
+    }
+
+    privacy_config.private_addr_type = BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE;
+    err = sd_ble_gap_privacy_set(&privacy_config);
+    return err ? BLE_ERROR_UNSPECIFIED : BLE_ERROR_NONE;
+#endif
+}
+
+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();
     gap.processRadioNotificationEvent(param);
 }
 
+nRF5xGap::nRF5xGap() : Gap(),
+    advertisingPolicyMode(Gap::ADV_POLICY_IGNORE_WHITELIST),
+    scanningPolicyMode(Gap::SCAN_POLICY_IGNORE_WHITELIST),
+    whitelistAddressesSize(0),
+    whitelistAddresses(),
+    radioNotificationCallbackParam(false),
+    radioNotificationTimeout(),
+    _connection_event_handler(NULL),
+    _privacy_enabled(false),
+    _peripheral_privacy_configuration(default_peripheral_privacy_configuration),
+    _central_privacy_configuration(default_central_privacy_configuration),
+    _non_private_address_type(BLEProtocol::AddressType::RANDOM)
+{
+        m_connectionHandle = BLE_CONN_HANDLE_INVALID;
+}
 /**************************************************************************/
 /*!
     @brief  Sets the advertising parameters and payload for the device
@@ -196,6 +294,28 @@ 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. */
@@ -250,6 +370,7 @@ 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
@@ -264,6 +385,13 @@ ble_error_t nRF5xGap::startRadioScan(const GapScanningParams &scanningParams)
     scanParams.interval    = scanningParams.getInterval();  /**< Scan interval between 0x0004 and 0x4000 in 0.625ms units (2.5ms to 10.24s). */
     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;
@@ -313,6 +441,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);
 
@@ -352,6 +481,34 @@ ble_error_t nRF5xGap::connect(const Address_t             peerAddr,
     
     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. */
     
@@ -384,9 +541,9 @@ ble_error_t nRF5xGap::connect(const Address_t             peerAddr,
     }
 
 #if NRF_SD_BLE_API_VERSION >= 5
-    uint32_t rc = sd_ble_gap_connect(&addr, &scanParams, &connParams, NRF_CONNECTION_TAG);
+    uint32_t rc = sd_ble_gap_connect(addr_ptr, &scanParams, &connParams, NRF_CONNECTION_TAG);
 #else
-    uint32_t rc = sd_ble_gap_connect(&addr, &scanParams, &connParams);
+    uint32_t rc = sd_ble_gap_connect(addr_ptr, &scanParams, &connParams);
 #endif
     if (rc == NRF_SUCCESS) {
         return BLE_ERROR_NONE;
@@ -499,7 +656,6 @@ ble_error_t nRF5xGap::reset(void)
 
     /* Clear the internal whitelist */
     whitelistAddressesSize = 0;
-    
 
     return BLE_ERROR_NONE;
 }
@@ -542,103 +698,75 @@ 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.
+#if (NRF_SD_BLE_API_VERSION <= 2)
-       When using Random Private addresses, the cycle mode must be Auto.
+    uint32_t err = sd_ble_gap_address_set(BLE_GAP_ADDR_CYCLE_MODE_NONE, &dev_addr);
-       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;
 #else
-        privacy_params.privacy_mode = BLE_GAP_PRIVACY_MODE_OFF;
+    uint32_t err = sd_ble_gap_addr_set(&dev_addr);
-        dev_addr.addr_type = type;
-
-        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);
-#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;
+    switch (err) {
+        case NRF_SUCCESS:
+            return BLE_ERROR_NONE;
+        case NRF_ERROR_INVALID_ADDR:
+        case BLE_ERROR_GAP_INVALID_BLE_ADDR:
+            return BLE_ERROR_INVALID_PARAM;
+        case NRF_ERROR_BUSY:
+            return BLE_STACK_BUSY;
+        case NRF_ERROR_INVALID_STATE:
+            return BLE_ERROR_INVALID_STATE;
+        default:
+            return BLE_ERROR_UNSPECIFIED;
+    }
 }
 
 ble_error_t nRF5xGap::getAddress(AddressType_t *typeP, Address_t address)
 {
-    ble_gap_addr_t dev_addr;
+    // FIXME: check if privacy is enabled ?
-    ble_gap_irk_t irk = {0};
+    if (typeP == NULL || address == NULL) {
-    ble_gap_privacy_params_t privacy_params = {0};
+        return BLE_ERROR_INVALID_PARAM;
-    privacy_params.p_device_irk = &irk;
+    }
 
+    ble_gap_addr_t dev_addr;
 #if  (NRF_SD_BLE_API_VERSION <= 2)
     if (sd_ble_gap_address_get(&dev_addr) != NRF_SUCCESS) {
-        return BLE_ERROR_PARAM_OUT_OF_RANGE;
-    }
 #else
-    	// Check privacy mode
-    	if( pm_privacy_get(&privacy_params)  != NRF_SUCCESS) {
-        return BLE_ERROR_PARAM_OUT_OF_RANGE;
-    }
-
-    	// If in private mode, the address is generated by softdevice, so return a nulled address with correct type
-    	if( privacy_params.privacy_mode == BLE_GAP_PRIVACY_MODE_DEVICE_PRIVACY )
-    	{
-    		memset(address, 0, ADDR_LEN);
-    		switch( privacy_params.private_addr_type )
-    		{
-    		case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_RESOLVABLE:
-    			*typeP = BLEProtocol::AddressType::RANDOM_PRIVATE_RESOLVABLE;
-    			break;
-    		case BLE_GAP_ADDR_TYPE_RANDOM_PRIVATE_NON_RESOLVABLE:
-    		default:
-    			*typeP = BLEProtocol::AddressType::RANDOM_PRIVATE_NON_RESOLVABLE;
-    			break;
-    		}
-    		return BLE_ERROR_NONE;
-    	}
-
-    	// Otherwise recover public/static address
     if (sd_ble_gap_addr_get(&dev_addr) != NRF_SUCCESS) {
+#endif
         return BLE_ERROR_PARAM_OUT_OF_RANGE;
     }
-#endif
 
-    if (typeP != NULL) {
+    switch (dev_addr.addr_type) {
-        *typeP = static_cast<AddressType_t>(dev_addr.addr_type);
+        case BLE_GAP_ADDR_TYPE_PUBLIC:
-    }
+            *typeP = BLEProtocol::AddressType::PUBLIC;
-    if (address != NULL) {
+            break;
-        memcpy(address, dev_addr.addr, ADDR_LEN);
+
+        case BLE_GAP_ADDR_TYPE_RANDOM_STATIC:
+            *typeP = BLEProtocol::AddressType::RANDOM_STATIC;
+            break;
+
+        default:
+            return BLE_ERROR_INVALID_STATE;
     }
+
+    memcpy(address, dev_addr.addr, ADDR_LEN);
     return BLE_ERROR_NONE;
 }
 
@@ -988,5 +1116,247 @@ Gap::InitiatorPolicyMode_t nRF5xGap::getInitiatorPolicyMode(void) const
     return Gap::INIT_POLICY_IGNORE_WHITELIST;
 }
 
+ble_error_t nRF5xGap::enablePrivacy(bool enable_privacy)
+{
+    if (enable_privacy == _privacy_enabled) {
+        return BLE_ERROR_NONE;
+    }
+
+    ble_error_t err = BLE_ERROR_UNSPECIFIED;
+    if (enable_privacy == false) {
+        err = setAddress(_non_private_address_type, _non_private_address);
+    } else {
+        err = getAddress(&_non_private_address_type, _non_private_address);
+    }
+
+    if (err) {
+        return err;
+    }
+
+#if (NRF_SD_BLE_API_VERSION > 2)
+    ble_gap_privacy_params_t privacy_config = { 0 };
+    if (sd_ble_gap_privacy_get(&privacy_config)) {
+        return BLE_ERROR_UNSPECIFIED;
+    }
+
+    privacy_config.privacy_mode = enable_privacy ?
+        BLE_GAP_PRIVACY_MODE_DEVICE_PRIVACY :
+        BLE_GAP_PRIVACY_MODE_OFF;
+    if (sd_ble_gap_privacy_set(&privacy_config)) {
+        return BLE_ERROR_UNSPECIFIED;
+    }
+#endif
+
+    _privacy_enabled = enable_privacy;
+    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;
+}
+
+void nRF5xGap::set_connection_event_handler(
+    ConnectionEventMonitor::EventHandler* connection_event_handler
+) {
+    _connection_event_handler = connection_event_handler;
+}
+
+void nRF5xGap::processDisconnectionEvent(
+    Handle_t handle,
+    DisconnectionReason_t reason
+) {
+    if (_connection_event_handler) {
+        _connection_event_handler->on_disconnected(
+            handle,
+            reason
+        );
+    }
+
+    ::Gap::processDisconnectionEvent(
+        handle,
+        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
+    // FIXME: handle privacy ???
+    getAddress(&own_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 (NRF_SD_BLE_API_VERSION <= 2)
+    bool private_peer_known = evt.irk_match;
+#else
+    bool private_peer_known = evt.peer_addr.addr_id_peer;
+#endif
 
 
+    if (private_peer_known) {
+        // FIXME: Is this correct for SD > 2 ?
+        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 (!private_peer_known &&
+        _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_NRF5x/source/nRF5xGap.h

@@ -44,6 +44,7 @@
 #include "ble/GapAdvertisingData.h"
 #include "ble/Gap.h"
 #include "ble/GapScanningParams.h"
+#include "ble/pal/ConnectionEventMonitor.h"
 
 #include "nrf_soc.h"
 
@@ -52,8 +53,6 @@ extern "C" {
 #include "app_util_platform.h"
 }
 
-#include "btle_security.h"
-
 void radioNotificationStaticCallback(bool param);
 
 /**************************************************************************/
@@ -62,9 +61,11 @@ void radioNotificationStaticCallback(bool param);
 
 */
 /**************************************************************************/
-class nRF5xGap : public Gap
+class nRF5xGap : public ::Gap, public ble::pal::ConnectionEventMonitor {
-{
 public:
+    nRF5xGap();
+
+    virtual ~nRF5xGap() { }
     /* Functions that must be implemented from Gap */
     virtual ble_error_t setAddress(AddressType_t  type,  const Address_t address);
     virtual ble_error_t getAddress(AddressType_t *typeP, Address_t address);
@@ -129,6 +130,23 @@ 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);
@@ -229,22 +247,42 @@ private:
         radioNotificationTimeout.attach_us(mbed::callback(this, &nRF5xGap::postRadioNotificationCallback), 0);
     }
     friend void radioNotificationStaticCallback(bool param); /* allow invocations of processRadioNotificationEvent() */
+public:
+    /** @note Implements ConnectionEventMonitor.
+     *  @copydoc ConnectionEventMonitor::set_connection_event_handler
+     */
+    virtual void set_connection_event_handler(
+        ConnectionEventMonitor::EventHandler* connection_event_handler
+    );
+
+    /**
+     * @copydoc ::Gap::processDisconnectionEvent
+     */
+    void processDisconnectionEvent(
+        Handle_t handle,
+        DisconnectionReason_t reason
+    );
 
 private:
+    friend void btle_handler(const ble_evt_t *p_ble_evt);
+    friend void btle_handler(const ble_evt_t *p_ble_evt, void *p_context);
+
+    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_address_type;
+    Address_t _non_private_address;
 
     /*
      * Allow instantiation from nRF5xn when required.
      */
     friend class nRF5xn;
 
-    nRF5xGap() :
-        advertisingPolicyMode(Gap::ADV_POLICY_IGNORE_WHITELIST),
-        scanningPolicyMode(Gap::SCAN_POLICY_IGNORE_WHITELIST),
-        whitelistAddressesSize(0) {
-        m_connectionHandle = BLE_CONN_HANDLE_INVALID;
-    }
-
     nRF5xGap(nRF5xGap const &);
     void operator=(nRF5xGap const &);
 };