Fix nordic security cancellation

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

@@ -104,7 +104,8 @@ nRF5xGap::nRF5xGap() : Gap(),
     _privacy_enabled(false),
     _peripheral_privacy_configuration(default_peripheral_privacy_configuration),
     _central_privacy_configuration(default_central_privacy_configuration),
-    _non_private_address_type(LegacyAddressType::RANDOM_STATIC)
+    _non_private_address_type(LegacyAddressType::RANDOM_STATIC),
+    _connections_role()
 {
         m_connectionHandle = BLE_CONN_HANDLE_INVALID;
 }
@@ -682,6 +683,8 @@ ble_error_t nRF5xGap::reset(void)
     /* Clear the internal whitelist */
     whitelistAddressesSize = 0;
 
+    /* Reset existing mapping between a connection and its role */
+    release_all_connections_role();
 
     return BLE_ERROR_NONE;
 }
@@ -1195,6 +1198,8 @@ void nRF5xGap::processDisconnectionEvent(
     Handle_t handle,
     DisconnectionReason_t reason
 ) {
+    release_connection_role(handle);
+
     if (_connection_event_handler) {
         _connection_event_handler->on_disconnected(
             handle,
@@ -1214,6 +1219,9 @@ void nRF5xGap::on_connection(Gap::Handle_t handle, const ble_gap_evt_connected_t
     // set the new connection handle as the _default_ handle in gap
     setConnectionHandle(handle);
 
+    // add the connection and the role of the device in the local table
+    allocate_connection_role(handle, static_cast<Role_t>(evt.role));
+
     // deal with own address
     LegacyAddressType_t own_addr_type;
     Address_t own_address;
@@ -1375,5 +1383,45 @@ void nRF5xGap::on_advertising_packet(const ble_gap_evt_adv_report_t &evt) {
     );
 }
 
+ble_error_t nRF5xGap::get_role(ble::connection_handle_t connection, Role_t& role) {
+    for (size_t i = 0; i < max_connections_count; ++i) {
+        connection_role_t& c = _connections_role[i];
+        if (c.is_allocated && c.connection == connection) {
+            role = c.is_peripheral ? PERIPHERAL : CENTRAL;
+            return BLE_ERROR_NONE;
+        }
+    }
+
+    return BLE_ERROR_INVALID_PARAM;
+}
 
+void nRF5xGap::allocate_connection_role(
+    ble::connection_handle_t connection,
+    Role_t role
+) {
+    for (size_t i = 0; i < max_connections_count; ++i) {
+        connection_role_t& c = _connections_role[i];
+        if (c.is_allocated == false) {
+            c.connection = connection;
+            c.is_peripheral = (role == Gap::PERIPHERAL);
+            c.is_allocated = true;
+            return;
+        }
+    }
+}
+void nRF5xGap::release_connection_role(ble::connection_handle_t connection) {
+    for (size_t i = 0; i < max_connections_count; ++i) {
+        connection_role_t& c = _connections_role[i];
+        if (c.is_allocated && c.connection == connection) {
+            c.is_allocated = false;
+            return;
+        }
+    }
+}
+
+void nRF5xGap::release_all_connections_role() {
+    for (size_t i = 0; i < max_connections_count; ++i) {
+        _connections_role[i].is_allocated = false;
+    }
+}
 

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

@@ -259,12 +259,26 @@ class nRF5xGap : public ::Gap, public ble::pal::ConnectionEventMonitor {
         DisconnectionReason_t reason
     );
 
+    /**
+     * Return the role of the local peripheral for a given connection.
+     *
+     * @param[in] connection The connection queried.
+     * @param[out] role The role of the local device in the connection.
+     *
+     * @return BLE_ERROR_NONE in case of success or an appropriate error code.
+     */
+    ble_error_t get_role(ble::connection_handle_t connection, Role_t& role);
+
 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);
 
+    void allocate_connection_role(ble::connection_handle_t, Role_t);
+    void release_connection_role(ble::connection_handle_t);
+    void release_all_connections_role();
+
     uint16_t m_connectionHandle;
 
     ConnectionEventMonitor::EventHandler* _connection_event_handler;
@@ -275,6 +289,23 @@ class nRF5xGap : public ::Gap, public ble::pal::ConnectionEventMonitor {
     AddressType_t _non_private_address_type;
     Address_t _non_private_address;
 
+    struct connection_role_t {
+        connection_role_t() :
+            connection(),
+            is_peripheral(false),
+            is_allocated(false)
+        { }
+
+        ble::connection_handle_t connection;
+        uint8_t is_peripheral:1;
+        uint8_t is_allocated:1;
+    };
+
+    static const size_t max_connections_count =
+        NRF_SDH_BLE_PERIPHERAL_LINK_COUNT + NRF_SDH_BLE_CENTRAL_LINK_COUNT;
+
+    connection_role_t _connections_role[max_connections_count];
+
     /*
      * Allow instantiation from nRF5xn when required.
      */

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

@@ -16,6 +16,9 @@
 
 #include <stdint.h>
 #include "nRF5xPalSecurityManager.h"
+#include "nRF5xn.h"
+#include "ble/Gap.h"
+#include "nRF5xGap.h"
 #include "nrf_ble.h"
 #include "nrf_ble_gap.h"
 #include "nrf_soc.h"
@@ -297,6 +300,9 @@ ble_error_t nRF5xSecurityManager::send_pairing_response(
 ) {
     pairing_control_block_t* pairing_cb = allocate_pairing_cb(connection);
     if (!pairing_cb) {
+        // not enough memory; try to reject the pairing request instead of
+        // waiting for timeout.
+        cancel_pairing(connection, pairing_failure_t::UNSPECIFIED_REASON);
         return BLE_ERROR_NO_MEM;
     }
     pairing_cb->role = PAIRING_RESPONDER;
@@ -339,22 +345,33 @@ ble_error_t nRF5xSecurityManager::send_pairing_response(
 ble_error_t nRF5xSecurityManager::cancel_pairing(
     connection_handle_t connection, pairing_failure_t reason
 ) {
-    // this is the default path except when a key is expected to be entered by
-    // the user.
-    uint32_t err = sd_ble_gap_sec_params_reply(
-        connection,
-        reason.value() | 0x80,
-        /* sec params */ NULL,
-        /* keyset */ NULL
-    );
+    uint32_t err = 0;
 
-    if (!err) {
-        return BLE_ERROR_NONE;
-    }
+    pairing_control_block_t* pairing_cb = get_pairing_cb(connection);
 
-    // Failed because we're in the wrong state; try to cancel pairing with
-    // sd_ble_gap_auth_key_reply
-    if (err == NRF_ERROR_INVALID_STATE) {
+    // If there is no control block yet then if the local device is a central
+    // then we must reject the security request otherwise it is a response to
+    // a pairing feature exchange from a central.
+    if (!pairing_cb) {
+        ::Gap::Role_t current_role;
+        if (nRF5xn::Instance().getGap().get_role(connection, current_role) != BLE_ERROR_NONE) {
+            return BLE_ERROR_INVALID_PARAM;
+        }
+
+        if (current_role == ::Gap::PERIPHERAL) {
+            // response to a pairing feature request
+            err = sd_ble_gap_sec_params_reply(
+                connection,
+                reason.value() | 0x80,
+                /* sec params */ NULL,
+                /* keyset */ NULL
+            );
+        } else {
+            // response to a peripheral security request
+            err = sd_ble_gap_authenticate(connection, NULL);
+        }
+    } else {
+        // At this point this must be a response to a key
         err = sd_ble_gap_auth_key_reply(
             connection,
             /* key type */ BLE_GAP_AUTH_KEY_TYPE_NONE,

features/FEATURE_BLE/targets/TARGET_NORDIC/TARGET_NRF51/source/nRF5xn.h

@@ -55,7 +55,7 @@ class nRF5xn : public BLEInstanceBase
      *        always needed in a BLE application. Therefore it is allocated
      *        statically.
      */
-    virtual Gap &getGap() {
+    virtual nRF5xGap &getGap() {
         return gapInstance;
     };
 
@@ -134,7 +134,7 @@ class nRF5xn : public BLEInstanceBase
     virtual void processEvents();
 
 public:
-    static nRF5xn& Instance(BLE::InstanceID_t instanceId);
+    static nRF5xn& Instance(BLE::InstanceID_t instanceId = BLE::DEFAULT_INSTANCE);
 
 private:
     bool              initialized;