nimble/ll: Add vs hci to set local IRK

nimble/controller/include/controller/ble_ll_resolv.h

@@ -115,6 +115,17 @@ int ble_ll_resolv_peer_rpa_any(const uint8_t *rpa);
 /* Initialize resolv*/
 void ble_ll_resolv_init(void);
 
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+int ble_ll_resolv_local_irk_set(uint8_t own_addr_type, const uint8_t *irk);
+int ble_ll_resolv_local_rpa_get(uint8_t own_addr_type, uint8_t *rpa);
+#else
+static inline int
+ble_ll_resolv_local_rpa_get(uint8_t own_addr_type, uint8_t *rpa)
+{
+    return -1;
+}
+#endif
+
 #ifdef __cplusplus
 }
 #endif

nimble/controller/src/ble_ll_adv.c

@@ -332,7 +332,9 @@ static void
 ble_ll_adv_rpa_update(struct ble_ll_adv_sm *advsm)
 {
     if (ble_ll_resolv_gen_rpa(advsm->peer_addr, advsm->peer_addr_type,
-                          advsm->adva, 1)) {
+                              advsm->adva, 1) ||
+        (ble_ll_resolv_local_rpa_get(advsm->own_addr_type & 1,
+                                     advsm->adva) == 0)) {
         ble_ll_adv_flags_set(advsm, BLE_LL_ADV_SM_FLAG_TX_ADD);
     } else {
         if (advsm->own_addr_type & 1) {

nimble/controller/src/ble_ll_conn.c

@@ -3118,15 +3118,20 @@ ble_ll_conn_prepare_connect_ind(struct ble_ll_conn_sm *connsm,
 
     /* XXX: do this ahead of time? Calculate the local rpa I mean */
 #if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY)
-        if ((connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) &&
-            (addrd->rpa_index >= 0)) {
-            /* We are using RPA and advertiser was on our resolving list, so
-             * we'll use RPA to reply (see Core 5.3, Vol 6, Part B, 6.4).
-             */
-            rl = &g_ble_ll_resolv_list[addrd->rpa_index];
-            if (rl->rl_has_local) {
+        if (connsm->own_addr_type > BLE_HCI_ADV_OWN_ADDR_RANDOM) {
+            if (addrd->rpa_index >= 0) {
+                /* We are using RPA and advertiser was on our resolving list, so
+                 * we'll use RPA to reply (see Core 5.3, Vol 6, Part B, 6.4).
+                 */
+                rl = &g_ble_ll_resolv_list[addrd->rpa_index];
+                if (rl->rl_has_local) {
+                    hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
+                    ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita);
+                    addr = NULL;
+                }
+            } else if (ble_ll_resolv_local_rpa_get(connsm->own_addr_type & 1,
+                                                   pdu_data->inita) == 0) {
                 hdr |= BLE_ADV_PDU_HDR_TXADD_RAND;
-                ble_ll_resolv_get_priv_addr(rl, 1, pdu_data->inita);
                 addr = NULL;
             }
         }

nimble/controller/src/ble_ll_hci_vs.c

@@ -29,6 +29,7 @@
 #include "controller/ble_fem.h"
 #include "ble_ll_conn_priv.h"
 #include "ble_ll_priv.h"
+#include "controller/ble_ll_resolv.h"
 
 #if MYNEWT_VAL(BLE_LL_HCI_VS)
 
@@ -328,6 +329,33 @@ ble_ll_hci_vs_set_antenna(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
 }
 #endif
 
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+static int
+ble_ll_hci_vs_set_local_irk(uint16_t ocf, const uint8_t *cmdbuf, uint8_t cmdlen,
+                            uint8_t *rspbuf, uint8_t *rsplen)
+{
+    const struct ble_hci_vs_set_local_irk_cp *cmd = (const void *)cmdbuf;
+    int rc;
+
+    if (cmdlen != sizeof(*cmd)) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    if (ble_ll_is_busy(BLE_LL_BUSY_EXCLUDE_CONNECTIONS)) {
+        return BLE_ERR_CMD_DISALLOWED;
+    }
+
+    rc = ble_ll_resolv_local_irk_set(cmd->own_addr_type, cmd->irk);
+    if (rc) {
+        return BLE_ERR_INV_HCI_CMD_PARMS;
+    }
+
+    *rsplen = 0;
+
+    return 0;
+}
+#endif
+
 static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = {
     BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_RD_STATIC_ADDR,
                       ble_ll_hci_vs_rd_static_addr),
@@ -354,6 +382,10 @@ static struct ble_ll_hci_vs_cmd g_ble_ll_hci_vs_cmds[] = {
 #if MYNEWT_VAL(BLE_FEM_ANTENNA)
     BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_SET_ANTENNA, ble_ll_hci_vs_set_antenna),
 #endif
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+    BLE_LL_HCI_VS_CMD(BLE_HCI_OCF_VS_SET_LOCAL_IRK,
+                      ble_ll_hci_vs_set_local_irk),
+#endif
 };
 
 static struct ble_ll_hci_vs_cmd *

nimble/controller/src/ble_ll_resolv.c

@@ -48,6 +48,16 @@ struct ble_ll_resolv_data g_ble_ll_resolv_data;
 __attribute__((aligned(4)))
 struct ble_ll_resolv_entry g_ble_ll_resolv_list[MYNEWT_VAL(BLE_LL_RESOLV_LIST_SIZE)];
 
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+struct local_irk_data {
+    uint8_t is_set;
+    uint8_t irk[16];
+    uint8_t rpa[6];
+};
+/* 0 is for public, 1 is for static address */
+static struct local_irk_data g_local_irk[2];
+#endif
+
 /**
  * Called to determine if a change is allowed to the resolving list at this
  * time. We are not allowed to modify the resolving list if address translation
@@ -70,6 +80,30 @@ ble_ll_resolv_list_chg_allowed(void)
     return rc;
 }
 
+static void
+generate_rpa(const uint8_t *irk, uint8_t *rpa)
+{
+    uint8_t *prand;
+    struct ble_encryption_block ecb;
+
+    /* Get prand */
+    prand = rpa + 3;
+    ble_ll_rand_prand_get(prand);
+
+    /* Calculate hash, hash = ah(local IRK, prand) */
+    memcpy(ecb.key, irk, 16);
+    memset(ecb.plain_text, 0, 13);
+    ecb.plain_text[13] = prand[2];
+    ecb.plain_text[14] = prand[1];
+    ecb.plain_text[15] = prand[0];
+
+    /* Calculate hash */
+    ble_hw_encrypt_block(&ecb);
+
+    rpa[0] = ecb.cipher_text[15];
+    rpa[1] = ecb.cipher_text[14];
+    rpa[2] = ecb.cipher_text[13];
+}
 
 /**
  * Called to generate a resolvable private address in rl structure
@@ -81,8 +115,6 @@ static void
 ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local)
 {
     uint8_t *irk;
-    uint8_t *prand;
-    struct ble_encryption_block ecb;
     uint8_t *addr;
 
     BLE_LL_ASSERT(rl != NULL);
@@ -95,23 +127,7 @@ ble_ll_resolv_gen_priv_addr(struct ble_ll_resolv_entry *rl, int local)
         irk = rl->rl_peer_irk;
     }
 
-    /* Get prand */
-    prand = addr + 3;
-    ble_ll_rand_prand_get(prand);
-
-    /* Calculate hash, hash = ah(local IRK, prand) */
-    memcpy(ecb.key, irk, 16);
-    memset(ecb.plain_text, 0, 13);
-    ecb.plain_text[13] = prand[2];
-    ecb.plain_text[14] = prand[1];
-    ecb.plain_text[15] = prand[0];
-
-    /* Calculate hash */
-    ble_hw_encrypt_block(&ecb);
-
-    addr[0] = ecb.cipher_text[15];
-    addr[1] = ecb.cipher_text[14];
-    addr[2] = ecb.cipher_text[13];
+    generate_rpa(irk, addr);
 }
 
 /**
@@ -124,6 +140,10 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev)
     int i;
     os_sr_t sr;
     struct ble_ll_resolv_entry *rl;
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+    struct local_irk_data *irk_data;
+    uint8_t rpa[6];
+#endif
 
     rl = &g_ble_ll_resolv_list[0];
     for (i = 0; i < g_ble_ll_resolv_data.rl_cnt; ++i) {
@@ -141,6 +161,18 @@ ble_ll_resolv_rpa_timer_cb(struct ble_npl_event *ev)
         ++rl;
     }
 
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+    for (i = 0; i < ARRAY_SIZE(g_local_irk); i++) {
+        irk_data = &g_local_irk[i];
+        if (irk_data->is_set) {
+            generate_rpa(irk_data->irk, rpa);
+            OS_ENTER_CRITICAL(sr);
+            memcpy(irk_data->rpa, rpa, 6);
+            OS_EXIT_CRITICAL(sr);
+        }
+    }
+#endif
+
     ble_npl_callout_reset(&g_ble_ll_resolv_data.rpa_timer,
                           g_ble_ll_resolv_data.rpa_tmo);
 
@@ -637,6 +669,58 @@ ble_ll_resolv_gen_rpa(uint8_t *addr, uint8_t addr_type, uint8_t *rpa, int local)
     return 0;
 }
 
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+int
+ble_ll_resolv_local_irk_set(uint8_t own_addr_type, const uint8_t *irk)
+{
+    struct local_irk_data *irk_data;
+    int i;
+
+    if (own_addr_type >= 2) {
+        return -1;
+    }
+
+    irk_data = &g_local_irk[own_addr_type];
+
+    memcpy(irk_data->irk, irk, 16);
+
+    irk_data->is_set = 0;
+
+    for (i = 0; i < 16; i++) {
+        if (irk[i]) {
+            irk_data->is_set = 1;
+            break;
+        }
+    }
+
+    if (irk_data->is_set) {
+        generate_rpa(irk_data->irk, irk_data->rpa);
+    }
+
+    return 0;
+}
+
+int
+ble_ll_resolv_local_rpa_get(uint8_t own_addr_type, uint8_t *rpa)
+{
+    struct local_irk_data *irk_data;
+
+    if (own_addr_type >= 2) {
+        return -1;
+    }
+
+    irk_data = &g_local_irk[own_addr_type];
+
+    if (!irk_data->is_set) {
+        return -1;
+    }
+
+    memcpy(rpa, irk_data->rpa, 6);
+
+    return 0;
+}
+#endif
+
 /**
  * Resolve a Resolvable Private Address
  *
@@ -738,6 +822,10 @@ ble_ll_resolv_init(void)
                          &g_ble_ll_data.ll_evq,
                          ble_ll_resolv_rpa_timer_cb,
                          NULL);
+
+#if MYNEWT_VAL(BLE_LL_HCI_VS_LOCAL_IRK)
+    memset(&g_local_irk, 0, sizeof(g_local_irk));
+#endif
 }
 
 #endif  /* if MYNEWT_VAL(BLE_LL_CFG_FEAT_LL_PRIVACY) */

nimble/controller/syscfg.yml

@@ -402,6 +402,18 @@ syscfg.defs:
         value: 0
         restrictions:
             - BLE_LL_HCI_VS if 1
+    BLE_LL_HCI_VS_LOCAL_IRK:
+        description: >
+            Enables HCI command to set local IRK.
+            The local IRK is used by controller to generate RPA address in case
+            own address type 0x02 or 0x03 was requested by host but there is no
+            corresponding entry on resolving list. This allows to handle privacy
+            scenarios almost entirely in controller. If no local IRK is set, the
+            controller behaves as if feature is not enabled.
+        value: 0
+        restrictions:
+            - BLE_LL_HCI_VS if 1
+
 
     BLE_LL_HCI_VS_EVENT_ON_ASSERT:
         description: >

nimble/include/nimble/hci_common.h

@@ -1208,8 +1208,11 @@ struct ble_hci_vs_set_data_len_rp {
 struct ble_hci_vs_set_antenna_cp {
     uint8_t antenna;
 } __attribute__((packed));
-
-
+#define BLE_HCI_OCF_VS_SET_LOCAL_IRK                   (MYNEWT_VAL(BLE_HCI_VS_OCF_OFFSET) + (0x000A))
+struct ble_hci_vs_set_local_irk_cp {
+    uint8_t own_addr_type;
+    uint8_t irk[16];
+} __attribute__((packed));
 
 /* Command Specific Definitions */
 /* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */