diff --git a/include/bluetooth/hci.h b/include/bluetooth/hci.h index dc26e521cc5..06f39d5a3ae 100644 --- a/include/bluetooth/hci.h +++ b/include/bluetooth/hci.h @@ -105,6 +105,7 @@ static inline bool bt_addr_le_is_identity(const bt_addr_le_t *addr) #define BT_HCI_ERR_UNSPECIFIED 0x1f #define BT_HCI_ERR_PAIRING_NOT_SUPPORTED 0x29 #define BT_HCI_ERR_UNACCEPT_CONN_PARAM 0x3b +#define BT_HCI_ERR_ADV_TIMEOUT 0x3c /* EIR/AD data type definitions */ #define BT_DATA_FLAGS 0x01 /* AD flags */ @@ -949,7 +950,7 @@ struct bt_hci_cp_le_set_addr_res_enable { #define BT_HCI_OP_LE_SET_RPA_TIMEOUT BT_OP(BT_OGF_LE, 0x002e) struct bt_hci_cp_le_set_rpa_timeout { - u8_t rpa_timeout; + u16_t rpa_timeout; } __packed; #define BT_HCI_OP_LE_READ_MAX_DATA_LEN BT_OP(BT_OGF_LE, 0x002f) diff --git a/subsys/bluetooth/common/rpa.c b/subsys/bluetooth/common/rpa.c index 9cbdff6a316..6f8a53885fc 100644 --- a/subsys/bluetooth/common/rpa.c +++ b/subsys/bluetooth/common/rpa.c @@ -71,7 +71,8 @@ bool bt_rpa_irk_matches(const u8_t irk[16], const bt_addr_t *addr) } #endif -#if defined(CONFIG_BLUETOOTH_PRIVACY) +#if defined(CONFIG_BLUETOOTH_PRIVACY) || \ + defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) int bt_rpa_create(const u8_t irk[16], bt_addr_t *rpa) { int err; diff --git a/subsys/bluetooth/controller/Kconfig b/subsys/bluetooth/controller/Kconfig index 8ff47ab63b8..a8cad31c40d 100644 --- a/subsys/bluetooth/controller/Kconfig +++ b/subsys/bluetooth/controller/Kconfig @@ -112,6 +112,23 @@ config BLUETOOTH_CONTROLLER_LE_PING help Enable support for Bluetooth v4.1 LE Ping feature in the Controller. +config BLUETOOTH_CONTROLLER_PRIVACY + bool "LE Controller-based Privacy" + default y + select BLUETOOTH_RPA + help + Enable support for Bluetooth v4.2 LE Controller-based Privacy feature in the Controller. + +config BLUETOOTH_CONTROLLER_RL_SIZE + prompt "LE Controller-based Privacy Resolving List size" + depends on BLUETOOTH_CONTROLLER_PRIVACY + int + default 8 + range 1 8 if SOC_FAMILY_NRF5 + help + Set the size of the Resolving List for LE Controller-based Privacy. + On nRF5x-based controllers, the hardware imposes a limit of 8 devices. + config BLUETOOTH_CONTROLLER_DATA_LENGTH bool "Data Length Update" default y diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c index 84f9d24ce1c..505c8166c9e 100644 --- a/subsys/bluetooth/controller/hci/hci.c +++ b/subsys/bluetooth/controller/hci/hci.c @@ -374,11 +374,18 @@ static void read_supported_commands(struct net_buf *buf, struct net_buf **evt) /* LE Remote Conn Param Req and Neg Reply */ rp->commands[33] = (1 << 4) | (1 << 5); +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + /* LE resolving list commands, LE Read Peer RPA */ + rp->commands[34] |= (1 << 3) | (1 << 4) | (1 << 5) | (1 << 6) | (1 << 7); + /* LE Read Local RPA, LE Set AR Enable, Set RPA Timeout */ + rp->commands[35] |= (1 << 0) | (1 << 1) | (1 << 2); +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + #if defined(CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH) /* LE Set Data Length, and LE Read Suggested Data Length. */ rp->commands[33] |= (1 << 6) | (1 << 7); /* LE Write Suggested Data Length. */ - rp->commands[34] = (1 << 0); + rp->commands[34] |= (1 << 0); #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */ #if defined(CONFIG_BLUETOOTH_HCI_RAW) && defined(CONFIG_BLUETOOTH_TINYCRYPT_ECC) @@ -388,7 +395,7 @@ static void read_supported_commands(struct net_buf *buf, struct net_buf **evt) #if defined(CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH) /* LE Read Maximum Data Length. */ - rp->commands[35] = (1 << 3); + rp->commands[35] |= (1 << 3); #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */ } @@ -911,6 +918,96 @@ static void le_read_max_data_len(struct net_buf *buf, struct net_buf **evt) } #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) +static void le_add_dev_to_rl(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_le_add_dev_to_rl *cmd = (void *)buf->data; + struct bt_hci_evt_cc_status *ccst; + u32_t status; + + status = ll_rl_add(&cmd->peer_id_addr, cmd->peer_irk, cmd->local_irk); + + ccst = cmd_complete(evt, sizeof(*ccst)); + ccst->status = status; +} + +static void le_rem_dev_from_rl(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_le_rem_dev_from_rl *cmd = (void *)buf->data; + struct bt_hci_evt_cc_status *ccst; + u32_t status; + + status = ll_rl_remove(&cmd->peer_id_addr); + + ccst = cmd_complete(evt, sizeof(*ccst)); + ccst->status = status; +} + +static void le_clear_rl(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_evt_cc_status *ccst; + ccst = cmd_complete(evt, sizeof(*ccst)); + + ccst->status = ll_rl_clear(); +} + +static void le_read_rl_size(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_rp_le_read_rl_size *rp; + + rp = cmd_complete(evt, sizeof(*rp)); + + rp->rl_size = ll_rl_size_get(); + rp->status = 0x00; +} + +static void le_read_peer_rpa(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_le_read_peer_rpa *cmd = (void *)buf->data; + struct bt_hci_rp_le_read_peer_rpa *rp; + bt_addr_le_t peer_id_addr; + + bt_addr_le_copy(&peer_id_addr, &cmd->peer_id_addr); + rp = cmd_complete(evt, sizeof(*rp)); + + rp->status = ll_rl_prpa_get(&peer_id_addr, &rp->peer_rpa); +} + +static void le_read_local_rpa(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_le_read_local_rpa *cmd = (void *)buf->data; + struct bt_hci_rp_le_read_local_rpa *rp; + bt_addr_le_t peer_id_addr; + + bt_addr_le_copy(&peer_id_addr, &cmd->peer_id_addr); + rp = cmd_complete(evt, sizeof(*rp)); + + rp->status = ll_rl_lrpa_get(&peer_id_addr, &rp->local_rpa); +} + +static void le_set_addr_res_enable(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_le_set_addr_res_enable *cmd = (void *)buf->data; + struct bt_hci_evt_cc_status *ccst; + u8_t enable = cmd->enable; + + ccst = cmd_complete(evt, sizeof(*ccst)); + ccst->status = ll_rl_enable(enable); +} + +static void le_set_rpa_timeout(struct net_buf *buf, struct net_buf **evt) +{ + struct bt_hci_cp_le_set_rpa_timeout *cmd = (void *)buf->data; + struct bt_hci_evt_cc_status *ccst; + u16_t timeout = sys_le16_to_cpu(cmd->rpa_timeout); + + ll_rl_timeout_set(timeout); + + ccst = cmd_complete(evt, sizeof(*ccst)); + ccst->status = 0x00; +} +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + #if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY) static void le_read_phy(struct net_buf *buf, struct net_buf **evt) { @@ -1128,6 +1225,33 @@ static int controller_cmd_handle(u8_t ocf, struct net_buf *cmd, break; #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH */ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + case BT_OCF(BT_HCI_OP_LE_ADD_DEV_TO_RL): + le_add_dev_to_rl(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_REM_DEV_FROM_RL): + le_rem_dev_from_rl(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_CLEAR_RL): + le_clear_rl(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_READ_RL_SIZE): + le_read_rl_size(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_READ_PEER_RPA): + le_read_peer_rpa(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_READ_LOCAL_RPA): + le_read_local_rpa(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_SET_ADDR_RES_ENABLE): + le_set_addr_res_enable(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_SET_RPA_TIMEOUT): + le_set_rpa_timeout(cmd, evt); + break; +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + #if defined(CONFIG_BLUETOOTH_CONTROLLER_PHY) case BT_OCF(BT_HCI_OP_LE_READ_PHY): le_read_phy(cmd, evt); diff --git a/subsys/bluetooth/controller/include/ll.h b/subsys/bluetooth/controller/include/ll.h index d6967211fbe..55fb3f4acce 100644 --- a/subsys/bluetooth/controller/include/ll.h +++ b/subsys/bluetooth/controller/include/ll.h @@ -38,6 +38,16 @@ u32_t ll_wl_clear(void); u32_t ll_wl_add(bt_addr_le_t *addr); u32_t ll_wl_remove(bt_addr_le_t *addr); +u32_t ll_rl_size_get(void); +u32_t ll_rl_clear(void); +u32_t ll_rl_add(bt_addr_le_t *id_addr, const u8_t pirk[16], + const u8_t lirk[16]); +u32_t ll_rl_remove(bt_addr_le_t *id_addr); +u32_t ll_rl_prpa_get(bt_addr_le_t *id_addr, bt_addr_t *prpa); +u32_t ll_rl_lrpa_get(bt_addr_le_t *id_addr, bt_addr_t *lrpa); +u32_t ll_rl_enable(u8_t enable); +void ll_rl_timeout_set(u16_t timeout); + void ll_irk_clear(void); u32_t ll_irk_add(u8_t *irk); u32_t ll_create_connection(u16_t scan_interval, u16_t scan_window, diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c index d02e1347425..19dcf684ca3 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.c +++ b/subsys/bluetooth/controller/ll_sw/ctrl.c @@ -33,7 +33,6 @@ #include "ctrl.h" #include "ctrl_internal.h" -#include "ll.h" #include "ll_filter.h" #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_HCI_DRIVER) @@ -460,8 +459,8 @@ void ll_reset(void) _radio.packet_release_first = 0; _radio.packet_release_last = 0; - /* reset whitelist */ - ll_wl_clear(); + /* reset whitelist and resolving list */ + ll_filter_reset(false); /* memory allocations */ common_init(); } @@ -4742,6 +4741,7 @@ static void adv_setup(void) struct pdu_adv *pdu; u8_t bitmap; u8_t chan; + u8_t upd = 0; /* Use latest adv data PDU buffer */ if (_radio.advertiser.adv_data.first != @@ -4753,6 +4753,7 @@ static void adv_setup(void) first = 0; } _radio.advertiser.adv_data.first = first; + upd = 1; } /* Use latest scan data PDU buffer */ @@ -4765,14 +4766,27 @@ static void adv_setup(void) first = 0; } _radio.advertiser.scan_data.first = first; + upd = 1; } pdu = (struct pdu_adv *) _radio.advertiser.adv_data.data[ _radio.advertiser.adv_data.first]; - /* TODO: Privacy 1.2, copy AdvA from adv data PDU buffer into scan data - * PDU buffer, here. So that Scan Response has same AdvA. - */ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (upd) { + struct pdu_adv *scan_pdu = (struct pdu_adv *) + _radio.advertiser.scan_data.data[ + _radio.advertiser.scan_data.first]; + + /* Copy the address from the adv packet we will send into the + * scan response. + */ + memcpy(&scan_pdu->payload.scan_rsp.addr[0], + &pdu->payload.adv_ind.addr[0], BDADDR_SIZE); + } +#else + ARG_UNUSED(upd); +#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ radio_pkt_tx_set(pdu); if ((pdu->type != PDU_ADV_TYPE_NONCONN_IND) && (!IS_ENABLED(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) || diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.h b/subsys/bluetooth/controller/ll_sw/ctrl.h index a8933cf6a6b..634bc6e1cad 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.h +++ b/subsys/bluetooth/controller/ll_sw/ctrl.h @@ -55,6 +55,12 @@ #define RADIO_LL_LENGTH_OCTETS_RX_MAX 27 #endif /* CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH_MAX */ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) +#define RADIO_BLE_FEAT_BIT_PRIVACY BIT64(BT_LE_FEAT_BIT_PRIVACY) +#else /* !CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ +#define RADIO_BLE_FEAT_BIT_PRIVACY 0 +#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + #if defined(CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2) #define RADIO_BLE_FEAT_BIT_CHAN_SEL_2 BIT64(BT_LE_FEAT_BIT_CHAN_SEL_ALGO_2) #else /* !CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2 */ @@ -130,6 +136,7 @@ BIT(BT_LE_FEAT_BIT_SLAVE_FEAT_REQ) | \ RADIO_BLE_FEAT_BIT_PING | \ RADIO_BLE_FEAT_BIT_DLE | \ + RADIO_BLE_FEAT_BIT_PRIVACY | \ RADIO_BLE_FEAT_BIT_PHY_2M | \ RADIO_BLE_FEAT_BIT_PHY_CODED | \ RADIO_BLE_FEAT_BIT_CHAN_SEL_2) diff --git a/subsys/bluetooth/controller/ll_sw/ll.c b/subsys/bluetooth/controller/ll_sw/ll.c index 3b5837281d2..1e3775399db 100644 --- a/subsys/bluetooth/controller/ll_sw/ll.c +++ b/subsys/bluetooth/controller/ll_sw/ll.c @@ -36,6 +36,7 @@ #include "ctrl.h" #include "ctrl_internal.h" #include "ll.h" +#include "ll_filter.h" /* Global singletons */ static u8_t MALIGN(4) _rand_context[3 + 4 + 1]; @@ -211,6 +212,8 @@ int ll_init(struct k_sem *sem_rx) return -ENOMEM; } + ll_filter_reset(true); + IRQ_DIRECT_CONNECT(NRF5_IRQ_RADIO_IRQn, CONFIG_BLUETOOTH_CONTROLLER_WORKER_PRIO, radio_nrf5_isr, 0); diff --git a/subsys/bluetooth/controller/ll_sw/ll_adv.c b/subsys/bluetooth/controller/ll_sw/ll_adv.c index 27704076b64..754ba24fd0e 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ll_adv.c @@ -16,17 +16,17 @@ #include "ctrl.h" #include "ll.h" -static struct { - u8_t chl_map:3; - u8_t filter_policy:2; +#include "hal/debug.h" -#if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) - u8_t phy_p:3; - u32_t interval; -#else /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ - u16_t interval; -#endif /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ -} ll_adv; +#include "ll_filter.h" +#include "ll_adv.h" + +static struct ll_adv_set ll_adv; + +struct ll_adv_set *ll_adv_set_get(void) +{ + return &ll_adv; +} #if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) u32_t ll_adv_params_set(u8_t handle, u16_t evt_prop, u32_t interval, @@ -123,7 +123,14 @@ u32_t ll_adv_params_set(u16_t interval, u8_t adv_type, pdu->chan_sel = 0; } - pdu->tx_addr = own_addr_type; +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + ll_adv.own_addr_type = own_addr_type; + if (own_addr_type >= BT_ADDR_LE_PUBLIC_ID) { + ll_adv.id_addr_type = direct_addr_type; + memcpy(&ll_adv.id_addr, direct_addr, BDADDR_SIZE); + } +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + pdu->tx_addr = own_addr_type & 0x1; pdu->rx_addr = 0; if (pdu->type == PDU_ADV_TYPE_DIRECT_IND) { pdu->rx_addr = direct_addr_type; @@ -229,7 +236,7 @@ u32_t ll_adv_params_set(u16_t interval, u8_t adv_type, pdu->type = PDU_ADV_TYPE_SCAN_RSP; pdu->rfu = 0; pdu->chan_sel = 0; - pdu->tx_addr = own_addr_type; + pdu->tx_addr = own_addr_type & 0x1; pdu->rx_addr = 0; if (pdu->len == 0) { pdu->len = BDADDR_SIZE; @@ -374,14 +381,35 @@ u32_t ll_adv_enable(u8_t enable) /* TODO: TargetA, fill here at enable */ #endif /* CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ - } else { - memcpy(&pdu_adv->payload.adv_ind.addr[0], - ll_addr_get(pdu_adv->tx_addr, NULL), BDADDR_SIZE); - memcpy(&pdu_scan->payload.scan_rsp.addr[0], - ll_addr_get(pdu_adv->tx_addr, NULL), BDADDR_SIZE); - } + bool priv = false; +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (ctrl_rl_enabled()) { + /*@todo: Enable AR */ + } + if (ll_adv.own_addr_type >= BT_ADDR_LE_PUBLIC_ID) { + /* Look up the resolving list */ + int idx = ll_rl_idx_find(ll_adv.id_addr_type, + ll_adv.id_addr); + + if (idx >= 0) { + /* Generate RPAs if required */ + ll_rl_rpa_update(false); + } + + ll_rl_pdu_adv_update(idx, pdu_adv); + ll_rl_pdu_adv_update(idx, pdu_scan); + priv = true; + } +#endif /* !CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + if (!priv) { + memcpy(&pdu_adv->payload.adv_ind.addr[0], + ll_addr_get(pdu_adv->tx_addr, NULL), BDADDR_SIZE); + memcpy(&pdu_scan->payload.scan_rsp.addr[0], + ll_addr_get(pdu_adv->tx_addr, NULL), BDADDR_SIZE); + } + } #if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) status = radio_adv_enable(ll_adv.phy_p, ll_adv.interval, ll_adv.chl_map, ll_adv.filter_policy); diff --git a/subsys/bluetooth/controller/ll_sw/ll_adv.h b/subsys/bluetooth/controller/ll_sw/ll_adv.h new file mode 100644 index 00000000000..0b21acb7cec --- /dev/null +++ b/subsys/bluetooth/controller/ll_sw/ll_adv.h @@ -0,0 +1,25 @@ +/* + * Copyright (c) 2017 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +struct ll_adv_set { + u8_t chl_map:3; + u8_t filter_policy:2; +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + u8_t rl_idx:5; + u8_t own_addr_type:2; + u8_t id_addr_type:1; + u8_t id_addr[BDADDR_SIZE]; +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + +#if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) + u8_t phy_p:3; + u32_t interval; +#else /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ + u16_t interval; +#endif /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ +}; + +struct ll_adv_set *ll_adv_set_get(void); diff --git a/subsys/bluetooth/controller/ll_sw/ll_filter.c b/subsys/bluetooth/controller/ll_sw/ll_filter.c index 5275538f934..321cdbfb1c2 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_filter.c +++ b/subsys/bluetooth/controller/ll_sw/ll_filter.c @@ -10,10 +10,12 @@ #include #include "util/util.h" +#include "util/mem.h" #include "pdu.h" #include "ctrl.h" #include "ll.h" +#include "ll_adv.h" #include "ll_filter.h" #define ADDR_TYPE_ANON 0xFF @@ -21,8 +23,47 @@ #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_HCI_DRIVER) #include "common/log.h" +#include "hal/debug.h" +#include "pdu.h" + static struct ll_wl wl; +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) +#include "common/rpa.h" + +static u8_t rl_enable; +static struct rl_dev { + u8_t taken:1; + u8_t rpas_ready:1; + u8_t pirk:1; + u8_t pirk_idx:4; + u8_t lirk:1; + + u8_t peer_id_addr_type:1; + bt_addr_t peer_id_addr; + u8_t local_irk[16]; + bt_addr_t peer_rpa; + bt_addr_t local_rpa; + +} rl[CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE]; + +static u8_t peer_irks[CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE][16]; +static u8_t peer_irk_count; + +#define DEFAULT_RPA_TIMEOUT_MS 900 * 1000 +u32_t rpa_timeout_ms; +s64_t rpa_last_ms; + +struct k_delayed_work rpa_work; +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + +static void wl_clear(void) +{ + wl.enable_bitmask = 0; + wl.addr_type_bitmask = 0; + wl.anon = 0; +} + struct ll_wl *ctrl_wl_get(void) { return &wl; @@ -39,9 +80,7 @@ u32_t ll_wl_clear(void) return BT_HCI_ERR_CMD_DISALLOWED; } - wl.enable_bitmask = 0; - wl.addr_type_bitmask = 0; - wl.anon = 0; + wl_clear(); return 0; } @@ -106,3 +145,345 @@ u32_t ll_wl_remove(bt_addr_le_t *addr) return BT_HCI_ERR_INVALID_PARAM; } +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + +#define RL_MATCH(i, id_addr_type, id_addr) (rl[i].taken && \ + rl[i].peer_id_addr_type == (id_addr_type & 0x1) && \ + !memcmp(rl[i].peer_id_addr.val, id_addr, BDADDR_SIZE)) + + +int ll_rl_idx_find(u8_t id_addr_type, u8_t *id_addr) +{ + int i; + for (i = 0; i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; i++) { + if (RL_MATCH(i, id_addr_type, id_addr)) { + return i; + } + } + + return -1; +} + +bool ctrl_rl_enabled(void) +{ + return rl_enable; +} + +void ll_rl_pdu_adv_update(int idx, struct pdu_adv *pdu) +{ + u8_t *adva = pdu->type == PDU_ADV_TYPE_SCAN_RSP ? + &pdu->payload.scan_rsp.addr[0] : + &pdu->payload.adv_ind.addr[0]; + + struct ll_adv_set *ll_adv = ll_adv_set_get(); + + /* AdvA */ + if (idx >= 0 && rl[idx].lirk) { + LL_ASSERT(rl[idx].rpas_ready); + pdu->tx_addr = 1; + memcpy(adva, rl[idx].local_rpa.val, BDADDR_SIZE); + } else { + pdu->tx_addr = ll_adv->own_addr_type & 0x1; + ll_addr_get(ll_adv->own_addr_type & 0x1, adva); + } + + /* TargetA */ + if (pdu->type == PDU_ADV_TYPE_DIRECT_IND) { + if (idx >= 0 && rl[idx].pirk) { + pdu->rx_addr = 1; + memcpy(&pdu->payload.direct_ind.tgt_addr[0], + rl[idx].peer_rpa.val, BDADDR_SIZE); + } else { + pdu->rx_addr = ll_adv->id_addr_type; + memcpy(&pdu->payload.direct_ind.tgt_addr[0], + ll_adv->id_addr, BDADDR_SIZE); + } + } +} + +static void rpa_adv_refresh(void) +{ + struct radio_adv_data *radio_adv_data; + struct ll_adv_set *ll_adv; + struct pdu_adv *prev; + struct pdu_adv *pdu; + u8_t last; + int idx; + + ll_adv = ll_adv_set_get(); + + if (ll_adv->own_addr_type < BT_ADDR_LE_PUBLIC_ID) { + return; + } + + radio_adv_data = radio_adv_data_get(); + prev = (struct pdu_adv *)&radio_adv_data->data[radio_adv_data->last][0]; + /* use the last index in double buffer, */ + if (radio_adv_data->first == radio_adv_data->last) { + last = radio_adv_data->last + 1; + if (last == DOUBLE_BUFFER_SIZE) { + last = 0; + } + } else { + last = radio_adv_data->last; + } + + /* update adv pdu fields. */ + pdu = (struct pdu_adv *)&radio_adv_data->data[last][0]; + pdu->type = prev->type; + pdu->rfu = 0; + + if (IS_ENABLED(CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2)) { + pdu->chan_sel = prev->chan_sel; + } else { + pdu->chan_sel = 0; + } + + + idx = ll_rl_idx_find(ll_adv->id_addr_type, ll_adv->id_addr); + LL_ASSERT(idx >= 0); + ll_rl_pdu_adv_update(idx, pdu); + + memcpy(&pdu->payload.adv_ind.data[0], &prev->payload.adv_ind.data[0], + prev->len - BDADDR_SIZE); + pdu->len = prev->len;; + + /* commit the update so controller picks it. */ + radio_adv_data->last = last; +} + +static void rl_clear(void) +{ + for (int i = 0; i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; i++) { + rl[i].taken = 0; + } +} + +static int rl_access_check(bool check_ar) +{ + if (check_ar) { + /* If address resolution is disabled, allow immediately */ + if (!rl_enable) { + return -1; + } + } + + return (radio_adv_is_enabled() || radio_scan_is_enabled()) ? 0 : 1; +} + + +void ll_rl_rpa_update(bool timeout) +{ + int i, err; + s64_t now = k_uptime_get(); + bool all = timeout || (rpa_last_ms == -1) || + (now - rpa_last_ms >= rpa_timeout_ms); + BT_DBG(""); + + for (i = 0; i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; i++) { + if ((rl[i].taken) && (all || !rl[i].rpas_ready)) { + + if (rl[i].pirk) { + err = bt_rpa_create(peer_irks[rl[i].pirk_idx], + &rl[i].peer_rpa); + LL_ASSERT(!err); + } + if (rl[i].lirk) { + err = bt_rpa_create(rl[i].local_irk, + &rl[i].local_rpa); + LL_ASSERT(!err); + } + + rl[i].rpas_ready = 1; + } + } + + if (all) { + rpa_last_ms = now; + } + + if (timeout) { + if (radio_adv_is_enabled()) { + rpa_adv_refresh(); + } + } +} + +static void rpa_timeout(struct k_work *work) +{ + ll_rl_rpa_update(true); + k_delayed_work_submit(&rpa_work, rpa_timeout_ms); +} + +static void rpa_refresh_start(void) +{ + if (!rl_enable) { + return; + } + + BT_DBG(""); + k_delayed_work_submit(&rpa_work, rpa_timeout_ms); +} + +static void rpa_refresh_stop(void) +{ + if (!rl_enable) { + return; + } + + k_delayed_work_cancel(&rpa_work); +} + +void ll_adv_scan_state_cb(u8_t bm) +{ + if (bm) { + rpa_refresh_start(); + } else { + rpa_refresh_stop(); + } +} + + +u32_t ll_rl_size_get(void) +{ + return CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; +} + +u32_t ll_rl_clear(void) +{ + if (!rl_access_check(false)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + rl_clear(); + + return 0; +} + +u32_t ll_rl_add(bt_addr_le_t *id_addr, const u8_t pirk[16], + const u8_t lirk[16]) +{ + int i; + + if (!rl_access_check(false)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + /* find an empty slot and insert device */ + for (i = 0; i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; i++) { + if (!rl[i].taken) { + bt_addr_copy(&rl[i].peer_id_addr, + &id_addr->a); + rl[i].peer_id_addr_type = id_addr->type & 0x1; + rl[i].pirk = mem_nz((u8_t *)pirk, 16); + rl[i].lirk = mem_nz((u8_t *)lirk, 16); + if (rl[i].pirk) { + rl[i].pirk_idx = peer_irk_count; + memcpy(peer_irks[peer_irk_count++], + pirk, 16); + } + if (rl[i].lirk) { + memcpy(rl[i].local_irk, lirk, 16); + } + rl[i].rpas_ready = 0; + rl[i].taken = 1; + break; + } + } + return (i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE) ? + 0x00 : BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; + +} + +u32_t ll_rl_remove(bt_addr_le_t *id_addr) +{ + int i; + + if (!rl_access_check(false)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + /* find the device and mark it as empty */ + i = ll_rl_idx_find(id_addr->type, id_addr->a.val); + if (i >= 0) { + if (rl[i].pirk) { + uint8_t idx = rl[i].pirk_idx; + memmove(peer_irks[idx], peer_irks[idx + 1], + 16 * peer_irk_count--); + } + rl[i].taken = 0; + } + + return (i >= 0) ? 0x00 : BT_HCI_ERR_UNKNOWN_CONN_ID; +} + +u32_t ll_rl_prpa_get(bt_addr_le_t *id_addr, bt_addr_t *prpa) +{ + int i; + + /* find the device and return its RPA */ + i = ll_rl_idx_find(id_addr->type, id_addr->a.val); + if (i >= 0) { + bt_addr_copy(prpa, &rl[i].peer_rpa); + } + + return (i >= 0) ? 0x00 : BT_HCI_ERR_UNKNOWN_CONN_ID; + +} + +u32_t ll_rl_lrpa_get(bt_addr_le_t *id_addr, bt_addr_t *lrpa) +{ + int i; + + /* find the device and return the local RPA */ + i = ll_rl_idx_find(id_addr->type, id_addr->a.val); + if (i >= 0) { + bt_addr_copy(lrpa, &rl[i].local_rpa); + } + + return (i >= 0) ? 0x00 : BT_HCI_ERR_UNKNOWN_CONN_ID; +} + +u32_t ll_rl_enable(u8_t enable) +{ + if (!rl_access_check(false)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + switch (enable) { + case BT_HCI_ADDR_RES_DISABLE: + rl_enable = 0; + break; + case BT_HCI_ADDR_RES_ENABLE: + rl_enable = 1; + break; + default: + return BT_HCI_ERR_INVALID_PARAM; + } + + return 0; +} + +void ll_rl_timeout_set(u16_t timeout) +{ + rpa_timeout_ms = timeout * 1000; +} +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + +void ll_filter_reset(bool init) +{ + wl_clear(); + +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + rl_enable = 0; + rpa_timeout_ms = DEFAULT_RPA_TIMEOUT_MS; + rpa_last_ms = -1; + rl_clear(); + if (init) { + k_delayed_work_init(&rpa_work, rpa_timeout); + } else { + k_delayed_work_cancel(&rpa_work); + } +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ + +} diff --git a/subsys/bluetooth/controller/ll_sw/ll_filter.h b/subsys/bluetooth/controller/ll_sw/ll_filter.h index 4182f28def8..70c65269d0c 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_filter.h +++ b/subsys/bluetooth/controller/ll_sw/ll_filter.h @@ -13,5 +13,12 @@ struct ll_wl { u8_t anon; }; +void ll_filter_reset(bool init); + struct ll_wl *ctrl_wl_get(void); +bool ctrl_rl_enabled(void); +void ll_rl_rpa_update(bool timeout); + +int ll_rl_idx_find(u8_t id_addr_type, u8_t *id_addr); +void ll_rl_pdu_adv_update(int idx, struct pdu_adv *pdu); diff --git a/subsys/bluetooth/host/Kconfig b/subsys/bluetooth/host/Kconfig index 6c3cca9805f..c6fefa9888d 100644 --- a/subsys/bluetooth/host/Kconfig +++ b/subsys/bluetooth/host/Kconfig @@ -192,6 +192,7 @@ config BLUETOOTH_SMP select TINYCRYPT select TINYCRYPT_AES select TINYCRYPT_AES_CMAC + select BLUETOOTH_RPA help This option enables support for the Security Manager Protocol (SMP), making it possible to pair devices over LE. @@ -199,7 +200,6 @@ config BLUETOOTH_SMP if BLUETOOTH_SMP config BLUETOOTH_PRIVACY bool "Privacy Feature" - select BLUETOOTH_RPA help Enable local Privacy Feature support. This makes it possible to use Resolvable Private Addresses (RPAs). diff --git a/tests/bluetooth/init/prj_controller_4_0.conf b/tests/bluetooth/init/prj_controller_4_0.conf index 85ee126d35a..abe98159765 100644 --- a/tests/bluetooth/init/prj_controller_4_0.conf +++ b/tests/bluetooth/init/prj_controller_4_0.conf @@ -6,6 +6,7 @@ CONFIG_BLUETOOTH_CONTROLLER_XTAL_ADVANCED=y CONFIG_BLUETOOTH_CONTROLLER_SCHED_ADVANCED=y CONFIG_BLUETOOTH_CONTROLLER_DUP_FILTER_LEN=16 CONFIG_BLUETOOTH_CONTROLLER_LE_PING=n +CONFIG_BLUETOOTH_CONTROLLER_PRIVACY=n CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH=n CONFIG_BLUETOOTH_CONTROLLER_PHY=n CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2=n diff --git a/tests/bluetooth/init/prj_controller_dbg.conf b/tests/bluetooth/init/prj_controller_dbg.conf index f4ecd9eb992..416c2d711a9 100644 --- a/tests/bluetooth/init/prj_controller_dbg.conf +++ b/tests/bluetooth/init/prj_controller_dbg.conf @@ -6,6 +6,7 @@ CONFIG_BLUETOOTH_CONTROLLER_XTAL_ADVANCED=n CONFIG_BLUETOOTH_CONTROLLER_SCHED_ADVANCED=n CONFIG_BLUETOOTH_CONTROLLER_DUP_FILTER_LEN=16 CONFIG_BLUETOOTH_CONTROLLER_LE_PING=y +CONFIG_BLUETOOTH_CONTROLLER_PRIVACY=y CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH=y CONFIG_BLUETOOTH_CONTROLLER_PHY=y CONFIG_BLUETOOTH_CONTROLLER_PHY_2M=y diff --git a/tests/bluetooth/init/prj_controller_tiny.conf b/tests/bluetooth/init/prj_controller_tiny.conf index d71418ada3c..ad6352aaba2 100644 --- a/tests/bluetooth/init/prj_controller_tiny.conf +++ b/tests/bluetooth/init/prj_controller_tiny.conf @@ -6,6 +6,7 @@ CONFIG_BLUETOOTH_CONTROLLER_XTAL_ADVANCED=n CONFIG_BLUETOOTH_CONTROLLER_SCHED_ADVANCED=n CONFIG_BLUETOOTH_CONTROLLER_DUP_FILTER_LEN=0 CONFIG_BLUETOOTH_CONTROLLER_LE_PING=n +CONFIG_BLUETOOTH_CONTROLLER_PRIVACY=n CONFIG_BLUETOOTH_CONTROLLER_DATA_LENGTH=n CONFIG_BLUETOOTH_CONTROLLER_PHY=n CONFIG_BLUETOOTH_CONTROLLER_CHAN_SEL_2=n