NimBLE: Add Host based privacy (RPA) support

nimble/host/include/host/ble_hs_pvcy.h

@@ -0,0 +1,40 @@
+/*
+ * Copyright 2020 Espressif Systems (Shanghai) PTE LTD
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one
+ * or more contributor license agreements.  See the NOTICE file
+ * distributed with this work for additional information
+ * regarding copyright ownership.  The ASF licenses this file
+ * to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance
+ * with the License.  You may obtain a copy of the License at
+ *
+ *  http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing,
+ * software distributed under the License is distributed on an
+ * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
+ * KIND, either express or implied.  See the License for the
+ * specific language governing permissions and limitations
+ * under the License.
+ */
+
+#include "host/ble_hs.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+/* Called to configure local(own) privacy (RPA) when using host based privacy. In
+ * Host based privacy as controller is not aware of RPA, we do it via
+ * 'BLE_ADDR_RANDOM' addr_type route.
+ *
+ * @param                enable RPA when enable is not 0
+ *                       disable RPA otherwise
+ *
+ * @return               return 0 when successful.
+ *                       return appropriate error code otherwise
+ */
+int ble_hs_pvcy_rpa_config(uint8_t enable);
+#endif

nimble/host/include/host/ble_store.h

@@ -27,9 +27,10 @@
 extern "C" {
 #endif
 
-#define BLE_STORE_OBJ_TYPE_OUR_SEC      1
-#define BLE_STORE_OBJ_TYPE_PEER_SEC     2
-#define BLE_STORE_OBJ_TYPE_CCCD         3
+#define BLE_STORE_OBJ_TYPE_OUR_SEC           1
+#define BLE_STORE_OBJ_TYPE_PEER_SEC          2
+#define BLE_STORE_OBJ_TYPE_CCCD              3
+#define BLE_STORE_OBJ_TYPE_PEER_DEV_REC      4
 
 /** Failed to persist record; insufficient storage capacity. */
 #define BLE_STORE_EVENT_OVERFLOW        1

nimble/host/src/ble_gap.c

@@ -26,6 +26,7 @@
 #include "host/ble_hs_hci.h"
 #include "ble_hs_priv.h"
 #include "ble_gap_priv.h"
+#include "ble_hs_resolv_priv.h"
 
 #ifndef min
 #define min(a, b) ((a) < (b) ? (a) : (b))
@@ -409,6 +410,23 @@ ble_gap_fill_conn_desc(struct ble_hs_conn *conn,
     ble_hs_conn_addrs(conn, &addrs);
 
     desc->our_id_addr = addrs.our_id_addr;
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    /* Check if the privacy is enabled, change the address type accordingly
+     * */
+    if (ble_host_rpa_enabled())
+    {
+        uint8_t *local_id = NULL;
+        struct ble_hs_resolv_entry *rl = NULL;
+        rl = ble_hs_resolv_list_find(conn->bhc_peer_addr.val);
+
+        if (rl != NULL) {
+            /* Get public ID address here */
+            ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL);
+            memcpy(desc->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN);
+            desc->our_id_addr.type = BLE_ADDR_PUBLIC;
+        }
+    }
+#endif
     desc->peer_id_addr = addrs.peer_id_addr;
     desc->our_ota_addr = addrs.our_ota_addr;
     desc->peer_ota_addr = addrs.peer_ota_addr;
@@ -2309,6 +2327,12 @@ ble_gap_timer(void)
 static int
 ble_gap_wl_busy(void)
 {
+
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    if (ble_host_rpa_enabled()) {
+        return BLE_HS_ENOTSUP;
+    }
+#endif
     /* Check if an auto or selective connection establishment procedure is in
      * progress.
      */
@@ -2345,6 +2369,12 @@ ble_gap_wl_tx_clear(void)
 int
 ble_gap_wl_set(const ble_addr_t *addrs, uint8_t white_list_count)
 {
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    if (ble_host_rpa_enabled()) {
+        return BLE_HS_ENOTSUP;
+    }
+#endif
+
 #if MYNEWT_VAL(BLE_WHITELIST)
     int rc;
     int i;

nimble/host/src/ble_hs_conn.c

@@ -23,6 +23,7 @@
 #include "os/os.h"
 #include "host/ble_hs_id.h"
 #include "ble_hs_priv.h"
+#include "ble_hs_resolv_priv.h"
 
 /** At least three channels required per connection (sig, att, sm). */
 #define BLE_HS_CONN_MIN_CHANS       3
@@ -444,6 +445,35 @@ ble_hs_conn_addrs(const struct ble_hs_conn *conn,
     /* Determine peer address information. */
     addrs->peer_id_addr = conn->bhc_peer_addr;
     addrs->peer_ota_addr = conn->bhc_peer_addr;
+
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+    /* RPA: Override peer address information. */
+    struct ble_hs_resolv_entry *rl = NULL;
+
+    ble_addr_t bhc_peer_addr;
+    bhc_peer_addr.type = conn->bhc_peer_addr.type;
+    memcpy(bhc_peer_addr.val, conn->bhc_peer_addr.val, BLE_DEV_ADDR_LEN);
+    if (ble_host_rpa_enabled()) {
+
+        uint8_t *local_id = NULL;
+        ble_hs_id_addr(BLE_ADDR_PUBLIC, (const uint8_t **) &local_id, NULL);
+
+        rl = ble_hs_resolv_list_find(bhc_peer_addr.val);
+        if (rl != NULL) {
+            memcpy(addrs->peer_ota_addr.val, addrs->peer_id_addr.val, BLE_DEV_ADDR_LEN);
+            memcpy(addrs->peer_id_addr.val, rl->rl_identity_addr, BLE_DEV_ADDR_LEN);
+
+            addrs->peer_id_addr.type = rl->rl_addr_type;
+
+            /* RL is present: populate our id addr with public ID */
+            memcpy(addrs->our_id_addr.val, local_id, BLE_DEV_ADDR_LEN);
+            addrs->our_id_addr.type = BLE_ADDR_PUBLIC;
+            BLE_HS_LOG(DEBUG, "Revised our id addr:\n");
+            ble_hs_log_flat_buf(our_id_addr_val, BLE_DEV_ADDR_LEN);
+            BLE_HS_LOG(DEBUG, "\n");
+        }
+    }
+#endif
     switch (conn->bhc_peer_addr.type) {
     case BLE_ADDR_PUBLIC:
     case BLE_ADDR_RANDOM:

nimble/host/src/ble_hs_hci_evt.c

@@ -24,6 +24,7 @@
 #include "nimble/hci_common.h"
 #include "host/ble_gap.h"
 #include "ble_hs_priv.h"
+#include "ble_hs_resolv_priv.h"
 
 _Static_assert(sizeof (struct hci_data_hdr) == BLE_HCI_DATA_HDR_SZ,
                "struct hci_data_hdr must be 4 bytes");
@@ -347,6 +348,19 @@ ble_hs_hci_evt_le_enh_conn_complete(uint8_t subevent, const void *data,
         memcpy(evt.peer_addr, ev->peer_addr, BLE_DEV_ADDR_LEN);
         memcpy(evt.local_rpa, ev->local_rpa, BLE_DEV_ADDR_LEN);
         memcpy(evt.peer_rpa,ev->peer_rpa, BLE_DEV_ADDR_LEN);
+
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+        /* RPA needs to be resolved here, as controller is not aware of the
+         * address is RPA in Host based RPA  */
+        if (ble_host_rpa_enabled()) {
+            uint8_t *local_id_rpa = ble_hs_get_rpa_local();
+            memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN);
+        }
+
+        struct ble_hs_resolv_entry *rl = NULL;
+        ble_rpa_replace_peer_params_with_rl(evt.peer_addr,
+                                            &evt.peer_addr_type, &rl);
+#endif
         evt.conn_itvl = le16toh(ev->conn_itvl);
         evt.conn_latency = le16toh(ev->conn_latency);
         evt.supervision_timeout = le16toh(ev->supervision_timeout);
@@ -389,6 +403,23 @@ ble_hs_hci_evt_le_conn_complete(uint8_t subevent, const void *data,
         evt.peer_addr_type = ev->peer_addr_type;
         memcpy(evt.peer_addr, ev->peer_addr, BLE_DEV_ADDR_LEN);
 
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+        /* RPA needs to be resolved here, as controller is not aware of the
+         * address is RPA in Host based RPA  */
+        if (ble_host_rpa_enabled()) {
+            uint8_t *local_id_rpa = ble_hs_get_rpa_local();
+            memcpy(evt.local_rpa, local_id_rpa, BLE_DEV_ADDR_LEN);
+
+            struct ble_hs_resolv_entry *rl = NULL;
+            ble_rpa_replace_peer_params_with_rl(evt.peer_addr,
+                                                &evt.peer_addr_type, &rl);
+            if (rl == NULL) {
+                if (ble_rpa_resolv_add_peer_rec(evt.peer_addr) != 0) {
+                    BLE_HS_LOG(DEBUG, "Memory unavailable for new peer record\n");
+                }
+            }
+        }
+#endif
         evt.conn_itvl = le16toh(ev->conn_itvl);
         evt.conn_latency = le16toh(ev->conn_latency);
         evt.supervision_timeout = le16toh(ev->supervision_timeout);
@@ -478,6 +509,14 @@ ble_hs_hci_evt_le_adv_rpt(uint8_t subevent, const void *data, unsigned int len)
 
     for (i = 0; i < ev->num_reports; i++) {
         rpt = data;
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+        if (ble_host_rpa_enabled()) {
+            /* Now RPA to be resolved here, since controller is unaware of the
+             * address is RPA  */
+            ble_rpa_replace_peer_params_with_rl(desc.addr.val,
+                                                &desc.addr.type, NULL);
+        }
+#endif
 
         data += sizeof(rpt) + rpt->data_len + 1;
 

nimble/host/src/ble_hs_id.c

@@ -20,12 +20,25 @@
 #include <string.h>
 #include "host/ble_hs_id.h"
 #include "ble_hs_priv.h"
+#include "ble_hs_resolv_priv.h"
 
 static uint8_t ble_hs_id_pub[6];
 static uint8_t ble_hs_id_rnd[6];
 static const uint8_t ble_hs_misc_null_addr[6];
 
 
+bool
+ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type)
+{
+    bool rc = 0;
+    /* According to spec v4.2, Vol 6, Part B, section 1.3.2.2, the two most
+     * significant bits of RPA shall be equal to 0 and 1 */
+    if (addr_type && ((addr[5] & 0xc0) == 0x40)) {
+        rc = 1;
+    }
+    return rc;
+}
+
 void
 ble_hs_id_set_pub(const uint8_t *pub_addr)
 {
@@ -55,6 +68,57 @@ ble_hs_id_gen_rnd(int nrpa, ble_addr_t *out_addr)
     return 0;
 }
 
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+/**
+ * Sets the device's pseudo RPA address when 'Host based privacy' is in use.
+ * The address type (RPA) is inferred from the most-significant bits. The
+ * address is specified in host byte order (little-endian!).
+ *
+ * @param rnd_addr              The RPA address to set.
+ *
+ * @return                      0 on success;
+ *                              BLE_HS_EINVAL if the specified address is not a
+ *                                  resolvable private address.
+ *                              Other nonzero on error.
+ */
+int
+ble_hs_id_set_pseudo_rnd(const uint8_t *rnd_addr)
+{
+    uint8_t addr_type_byte;
+    int rc;
+    int i, rnd_part_sum = 0;
+
+    ble_hs_lock();
+
+    /* Make sure all bits of rnd_addr are neither one nor zero (3rd, 4th and
+     * 5th bytes of rnd_addr(RPA) are prand) Vol 6, Part B, section 1.3.2.2
+     * The two most significant bits of RPA shall be equal to 0 and 1 */
+    addr_type_byte = rnd_addr[5] & 0xc0;
+    for (i = 3; i < BLE_DEV_ADDR_LEN; i++) {
+        rnd_part_sum += *(rnd_addr + i);
+    }
+    rnd_part_sum -= addr_type_byte;
+
+    /* All ones in random part: 3*(0xFF) - 0x40 = 0x2BD  */
+    if ((addr_type_byte != 0x40) ||
+        (rnd_part_sum == 0) || (rnd_part_sum == 0x2BD)) {
+        rc = BLE_HS_EINVAL;
+        goto done;
+    }
+    /* set the RPA address as pseudo random address in controller */
+    rc = ble_hs_hci_util_set_random_addr(rnd_addr);
+    if (rc != 0) {
+        goto done;
+    }
+
+    memcpy(ble_hs_id_rnd, rnd_addr, BLE_DEV_ADDR_LEN);
+
+done:
+    ble_hs_unlock();
+    return rc;
+}
+#endif
+
 int
 ble_hs_id_set_rnd(const uint8_t *rnd_addr)
 {

nimble/host/src/ble_hs_id_priv.h

@@ -33,6 +33,10 @@ int ble_hs_id_use_addr(uint8_t addr_type);
 void ble_hs_id_reset(void);
 void ble_hs_id_rnd_reset(void);
 
+#if MYNEWT_VAL(BLE_HOST_BASED_PRIVACY)
+bool ble_hs_is_rpa(uint8_t *addr, uint8_t addr_type);
+int ble_hs_id_set_pseudo_rnd(const uint8_t *);
+#endif
 #ifdef __cplusplus
 }
 #endif

nimble/host/src/ble_hs_priv.h

@@ -129,6 +129,11 @@ void ble_hs_notifications_sched(void);
 struct ble_npl_eventq *ble_hs_evq_get(void);
 void ble_hs_stop_init(void);
 
+int
+ble_sm_alg_encrypt(const uint8_t *key, const uint8_t *plaintext,
+                   uint8_t *enc_data);
+
+
 struct ble_mqueue {
     STAILQ_HEAD(, os_mbuf_pkthdr) head;
     struct ble_npl_event ev;