Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

nimble/ll: Add vs hci to set local IRK #1630

Merged
merged 1 commit into from
Oct 26, 2023
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
11 changes: 11 additions & 0 deletions nimble/controller/include/controller/ble_ll_resolv.h
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 3 additions & 1 deletion nimble/controller/src/ble_ll_adv.c
Original file line number Diff line number Diff line change
Expand Up @@ -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) {
Expand Down
21 changes: 13 additions & 8 deletions nimble/controller/src/ble_ll_conn.c
Original file line number Diff line number Diff line change
Expand Up @@ -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;
}
}
Expand Down
32 changes: 32 additions & 0 deletions nimble/controller/src/ble_ll_hci_vs.c
Original file line number Diff line number Diff line change
Expand Up @@ -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)

Expand Down Expand Up @@ -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;
}

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

we should probably disallow this when initiating and advertising (?) (connectable, undirected)

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),
Expand All @@ -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 *
Expand Down
126 changes: 107 additions & 19 deletions nimble/controller/src/ble_ll_resolv.c
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand All @@ -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
Expand All @@ -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);
Expand All @@ -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);
}

/**
Expand All @@ -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) {
Expand All @@ -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);

Expand Down Expand Up @@ -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)
{
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

use ble_ll_resolv_irk_nonzero()

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
*
Expand Down Expand Up @@ -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) */
Expand Down
12 changes: 12 additions & 0 deletions nimble/controller/syscfg.yml
Original file line number Diff line number Diff line change
Expand Up @@ -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: >
Expand Down
7 changes: 5 additions & 2 deletions nimble/include/nimble/hci_common.h
Original file line number Diff line number Diff line change
Expand Up @@ -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];
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

just a loose thought, maybe this could also have address type? public vs static_random

} __attribute__((packed));

/* Command Specific Definitions */
/* --- Set controller to host flow control (OGF 0x03, OCF 0x0031) --- */
Expand Down
Loading