From 803eab08ec31e518d5abf963b43919ff5a5f57fd Mon Sep 17 00:00:00 2001 From: Carles Cufi Date: Mon, 26 Jun 2017 14:28:46 +0200 Subject: [PATCH] Bluetooth: controller: Privacy filtering in advertiser Implement privacy-enabled filtering in the advertiser role. This includes all required checks when running address generation and resolution so that the advertiser complies with the relevant specification sections. Signed-off-by: Carles Cufi --- subsys/bluetooth/controller/include/ll.h | 2 - subsys/bluetooth/controller/ll_sw/ctrl.c | 176 ++++++++++++------ subsys/bluetooth/controller/ll_sw/ctrl.h | 5 +- subsys/bluetooth/controller/ll_sw/ll.c | 4 + subsys/bluetooth/controller/ll_sw/ll_adv.c | 16 +- subsys/bluetooth/controller/ll_sw/ll_adv.h | 2 +- subsys/bluetooth/controller/ll_sw/ll_filter.c | 88 +++++++-- subsys/bluetooth/controller/ll_sw/ll_filter.h | 8 +- 8 files changed, 221 insertions(+), 80 deletions(-) diff --git a/subsys/bluetooth/controller/include/ll.h b/subsys/bluetooth/controller/include/ll.h index 6514134a6cb..0473ab4b538 100644 --- a/subsys/bluetooth/controller/include/ll.h +++ b/subsys/bluetooth/controller/include/ll.h @@ -51,8 +51,6 @@ u32_t ll_rl_enable(u8_t enable); void ll_rl_timeout_set(u16_t timeout); u32_t ll_priv_mode_set(bt_addr_le_t *id_addr, u8_t mode); -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, u8_t filter_policy, u8_t peer_addr_type, u8_t *p_peer_addr, u8_t own_addr_type, diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c index db040eb5096..21eaf8c55aa 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.c +++ b/subsys/bluetooth/controller/ll_sw/ctrl.c @@ -86,6 +86,9 @@ struct advertiser { #endif /* CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ u8_t chl_map:3; u8_t filter_policy:2; +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + u8_t rl_idx:4; +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ struct radio_adv_data adv_data; struct radio_adv_data scan_data; @@ -135,9 +138,6 @@ static struct { enum role volatile role; enum state state; - u8_t nirk; - u8_t irk[RADIO_IRK_COUNT_MAX][16]; - struct advertiser advertiser; struct scanner scanner; @@ -443,7 +443,6 @@ void ll_reset(void) } /* reset controller context members */ - _radio.nirk = 0; _radio.advertiser.is_enabled = 0; _radio.advertiser.conn = NULL; _radio.scanner.is_enabled = 0; @@ -587,9 +586,13 @@ static inline void isr_radio_state_tx(void) /* assert if radio packet ptr is not set and radio started rx */ LL_ASSERT(!radio_is_ready()); - if (_radio.advertiser.filter_policy && _radio.nirk) { - radio_ar_configure(_radio.nirk, _radio.irk); +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (ctrl_rl_enabled()) { + u8_t count, *irks = ctrl_irks_get(&count); + + radio_ar_configure(count, irks); } +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ hcto += radio_rx_chain_delay_get(0, 0); hcto += addr_us_get(0); @@ -709,8 +712,68 @@ static u32_t isr_rx_adv_sr_report(struct pdu_adv *pdu_adv_rx, u8_t rssi_ready) } #endif /* CONFIG_BLUETOOTH_CONTROLLER_SCAN_REQ_NOTIFY */ -static inline u32_t isr_rx_adv(u8_t devmatch_ok, u8_t irkmatch_ok, - u8_t irkmatch_id, u8_t rssi_ready) +static inline bool isr_adv_sr_check(struct pdu_adv *pdu, u8_t devmatch_ok, + u8_t irkmatch_ok, u8_t irkmatch_id) +{ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + return ((((_radio.advertiser.filter_policy & 0x01) == 0) && + ctrl_rl_allowed(pdu->tx_addr, + pdu->payload.scan_req.scan_addr)) || + (devmatch_ok) || (ctrl_rl_enabled() && irkmatch_ok && + ctrl_irk_whitelisted(irkmatch_id))) && + (1 /** @todo own addr match check */); +#else + return (((_radio.advertiser.filter_policy & 0x01) == 0) || + (devmatch_ok)) && + (1 /** @todo own addr match check */); +#endif +} + +static inline bool isr_adv_tgta_check(struct pdu_adv *adv, struct pdu_adv *ci, + u8_t irkmatch_ok, u8_t irkmatch_id) +{ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (ctrl_rl_enabled() && irkmatch_ok) { + return ctrl_rl_idx_match(irkmatch_id, _radio.advertiser.rl_idx); + } +#endif + return !memcmp(adv->payload.direct_ind.tgt_addr, + ci->payload.connect_ind.init_addr, BDADDR_SIZE); +} + +static inline bool isr_adv_ci_direct_check(struct pdu_adv *adv, + struct pdu_adv *ci, + u8_t irkmatch_ok, u8_t irkmatch_id) +{ + return ((adv->type != PDU_ADV_TYPE_DIRECT_IND) || + ((adv->tx_addr == ci->rx_addr) && + (adv->rx_addr == ci->tx_addr) && + !memcmp(adv->payload.direct_ind.adv_addr, + ci->payload.connect_ind.adv_addr, BDADDR_SIZE) && + isr_adv_tgta_check(adv, ci, irkmatch_ok, irkmatch_id))); +} + +static inline bool isr_adv_ci_check(struct pdu_adv *adv, struct pdu_adv *ci, + u8_t devmatch_ok, u8_t irkmatch_ok, + u8_t irkmatch_id) +{ +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + return ((((_radio.advertiser.filter_policy & 0x02) == 0) && + ctrl_rl_allowed(ci->tx_addr, + ci->payload.connect_ind.init_addr)) || + (devmatch_ok) || (ctrl_rl_enabled() && irkmatch_ok && + ctrl_irk_whitelisted(irkmatch_id))) && + isr_adv_ci_direct_check(adv, ci, irkmatch_ok, irkmatch_id); +#else + return (((_radio.advertiser.filter_policy & 0x02) == 0) || + (devmatch_ok)) && + isr_adv_ci_direct_check(adv, ci, irkmatch_ok, irkmatch_id); +#endif +} + +static inline u32_t isr_rx_adv(u8_t devmatch_ok, u8_t devmatch_id, + u8_t irkmatch_ok, u8_t irkmatch_id, + u8_t rssi_ready) { struct pdu_adv *pdu_adv, *_pdu_adv; struct radio_pdu_node_rx *radio_pdu_node_rx; @@ -721,9 +784,7 @@ static inline u32_t isr_rx_adv(u8_t devmatch_ok, u8_t irkmatch_ok, if ((pdu_adv->type == PDU_ADV_TYPE_SCAN_REQ) && (pdu_adv->len == sizeof(struct pdu_adv_payload_scan_req)) && - (((_radio.advertiser.filter_policy & 0x01) == 0) || - (devmatch_ok) || (irkmatch_ok)) && - (1 /** @todo own addr match check */)) { + isr_adv_sr_check(pdu_adv, devmatch_ok, irkmatch_ok, irkmatch_id)) { #if defined(CONFIG_BLUETOOTH_CONTROLLER_SCAN_REQ_NOTIFY) if (!IS_ENABLED(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) || @@ -749,17 +810,8 @@ static inline u32_t isr_rx_adv(u8_t devmatch_ok, u8_t irkmatch_ok, return 0; } else if ((pdu_adv->type == PDU_ADV_TYPE_CONNECT_IND) && (pdu_adv->len == sizeof(struct pdu_adv_payload_connect_ind)) && - (((_radio.advertiser.filter_policy & 0x02) == 0) || - (devmatch_ok) || (irkmatch_ok)) && - ((_pdu_adv->type != PDU_ADV_TYPE_DIRECT_IND) || - ((_pdu_adv->tx_addr == pdu_adv->rx_addr) && - (_pdu_adv->rx_addr == pdu_adv->tx_addr) && - !memcmp(_pdu_adv->payload.direct_ind.adv_addr, - pdu_adv->payload.connect_ind.adv_addr, - BDADDR_SIZE) && - !memcmp(_pdu_adv->payload.direct_ind.tgt_addr, - pdu_adv->payload.connect_ind.init_addr, - BDADDR_SIZE))) && + isr_adv_ci_check(_pdu_adv, pdu_adv, devmatch_ok, irkmatch_ok, + irkmatch_id) && ((_radio.fc_ena == 0) || (_radio.fc_req == _radio.fc_ack)) && (_radio.advertiser.conn)) { struct radio_le_conn_cmplt *radio_le_conn_cmplt; @@ -2760,8 +2812,9 @@ isr_rx_conn_exit: } static inline void isr_radio_state_rx(u8_t trx_done, u8_t crc_ok, - u8_t devmatch_ok, u8_t irkmatch_ok, - u8_t irkmatch_id, u8_t rssi_ready) + u8_t devmatch_ok, u8_t devmatch_id, + u8_t irkmatch_ok, u8_t irkmatch_id, + u8_t rssi_ready) { u32_t err; @@ -2776,7 +2829,7 @@ static inline void isr_radio_state_rx(u8_t trx_done, u8_t crc_ok, switch (_radio.role) { case ROLE_ADV: if (crc_ok) { - err = isr_rx_adv(devmatch_ok, irkmatch_ok, + err = isr_rx_adv(devmatch_ok, devmatch_id, irkmatch_ok, irkmatch_id, rssi_ready); } else { err = 1; @@ -2880,11 +2933,13 @@ static inline u32_t isr_close_scan(void) radio_pkt_rx_set(_radio.packet_rx[_radio.packet_rx_last]-> pdu_data); radio_rssi_measure(); +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (ctrl_rl_enabled()) { + u8_t count, *irks = ctrl_irks_get(&count); - if (_radio.scanner.filter_policy && _radio.nirk) { - radio_ar_configure(_radio.nirk, _radio.irk); + radio_ar_configure(count, irks); } - +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ _radio.state = STATE_RX; radio_rx_enable(); @@ -3285,6 +3340,7 @@ static void isr(void) u8_t trx_done; u8_t crc_ok; u8_t devmatch_ok; + u8_t devmatch_id; u8_t irkmatch_ok; u8_t irkmatch_id; u8_t rssi_ready; @@ -3304,12 +3360,13 @@ static void isr(void) crc_ok = radio_crc_is_valid(); devmatch_ok = radio_filter_has_match(); + devmatch_id = radio_filter_match_get(); irkmatch_ok = radio_ar_has_match(); irkmatch_id = radio_ar_match_get(); rssi_ready = radio_rssi_is_ready(); } else { crc_ok = devmatch_ok = irkmatch_ok = rssi_ready = 0; - irkmatch_id = 0xFF; + devmatch_id = irkmatch_id = 0xFF; } /* Clear radio status and events */ @@ -3325,8 +3382,8 @@ static void isr(void) break; case STATE_RX: - isr_radio_state_rx(trx_done, crc_ok, devmatch_ok, irkmatch_ok, - irkmatch_id, rssi_ready); + isr_radio_state_rx(trx_done, crc_ok, devmatch_ok, devmatch_id, + irkmatch_ok, irkmatch_id, rssi_ready); break; case STATE_ABORT: @@ -4917,10 +4974,20 @@ static void event_adv(u32_t ticks_at_expire, u32_t remainder, _radio.advertiser.chl_map_current = _radio.advertiser.chl_map; adv_setup(); +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (ctrl_rl_enabled()) { + struct ll_filter *filter = + ctrl_filter_get(!!(_radio.advertiser.filter_policy)); + + radio_filter_configure(filter->enable_bitmask, + filter->addr_type_bitmask, + (u8_t *)filter->bdaddr); + } else +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ /* Setup Radio Filter */ if (_radio.advertiser.filter_policy) { - struct ll_filter *wl = ctrl_filter_get(); + struct ll_filter *wl = ctrl_filter_get(true); radio_filter_configure(wl->enable_bitmask, wl->addr_type_bitmask, @@ -5107,17 +5174,27 @@ static void event_scan(u32_t ticks_at_expire, u32_t remainder, u16_t lazy, radio_pkt_rx_set(_radio.packet_rx[_radio.packet_rx_last]->pdu_data); radio_rssi_measure(); +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + if (ctrl_rl_enabled()) { + struct ll_filter *filter = + ctrl_filter_get(!!(_radio.scanner.filter_policy & 0x1)); + u8_t count, *irks = ctrl_irks_get(&count); + + radio_filter_configure(filter->enable_bitmask, + filter->addr_type_bitmask, + (u8_t *)filter->bdaddr); + + radio_ar_configure(count, irks); + } else +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ /* Setup Radio Filter */ if (_radio.scanner.filter_policy) { - struct ll_filter *wl = ctrl_filter_get(); + struct ll_filter *wl = ctrl_filter_get(true); radio_filter_configure(wl->enable_bitmask, wl->addr_type_bitmask, (u8_t *)wl->bdaddr); - if (_radio.nirk) { - radio_ar_configure(_radio.nirk, _radio.irk); - } } radio_tmr_start(0, @@ -7909,23 +7986,6 @@ struct radio_adv_data *radio_scan_data_get(void) return &_radio.advertiser.scan_data; } -void ll_irk_clear(void) -{ - _radio.nirk = 0; -} - -u32_t ll_irk_add(u8_t *irk) -{ - if (_radio.nirk >= RADIO_IRK_COUNT_MAX) { - return 1; - } - - memcpy(&_radio.irk[_radio.nirk][0], irk, 16); - _radio.nirk++; - - return 0; -} - static struct connection *connection_get(u16_t handle) { struct connection *conn; @@ -8189,9 +8249,10 @@ role_disable_cleanup: #if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) u32_t radio_adv_enable(u8_t phy_p, u16_t interval, u8_t chl_map, - u8_t filter_policy) + u8_t filter_policy, u8_t rl_idx) #else /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ -u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy) +u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy, + u8_t rl_idx) #endif /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ { u32_t volatile ret_cb = TICKER_STATUS_BUSY; @@ -8314,6 +8375,11 @@ u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy) _radio.advertiser.chl_map = chl_map; _radio.advertiser.filter_policy = filter_policy; +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + _radio.advertiser.rl_idx = rl_idx; +#else + ARG_UNUSED(rl_idx); +#endif /* CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ _radio.advertiser.hdr.ticks_active_to_start = _radio.ticks_active_to_start; diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.h b/subsys/bluetooth/controller/ll_sw/ctrl.h index 634bc6e1cad..d51b3af3b4a 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.h +++ b/subsys/bluetooth/controller/ll_sw/ctrl.h @@ -330,9 +330,10 @@ struct radio_adv_data *radio_scan_data_get(void); #if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT) u32_t radio_adv_enable(u8_t phy_p, u16_t interval, u8_t chl_map, - u8_t filter_policy); + u8_t filter_policy, u8_t rl_idx); #else /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ -u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy); +u32_t radio_adv_enable(u16_t interval, u8_t chl_map, u8_t filter_policy, + u8_t rl_idx); #endif /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ u32_t radio_adv_disable(void); diff --git a/subsys/bluetooth/controller/ll_sw/ll.c b/subsys/bluetooth/controller/ll_sw/ll.c index 22ae6bcc5d1..14e887c9f19 100644 --- a/subsys/bluetooth/controller/ll_sw/ll.c +++ b/subsys/bluetooth/controller/ll_sw/ll.c @@ -254,6 +254,10 @@ void ll_timeslice_ticker_id_get(u8_t * const instance_index, u8_t * const user_i u8_t *ll_addr_get(u8_t addr_type, u8_t *bdaddr) { + if (addr_type > 1) { + return NULL; + } + if (addr_type) { if (bdaddr) { memcpy(bdaddr, _ll_context.rnd_addr, BDADDR_SIZE); diff --git a/subsys/bluetooth/controller/ll_sw/ll_adv.c b/subsys/bluetooth/controller/ll_sw/ll_adv.c index 32870a49755..6f452309330 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ll_adv.c @@ -337,6 +337,7 @@ u32_t ll_adv_enable(u8_t enable) { struct radio_adv_data *radio_scan_data; struct radio_adv_data *radio_adv_data; + int rl_idx = RL_IDX_NONE; struct pdu_adv *pdu_scan; struct pdu_adv *pdu_adv; u32_t status; @@ -391,17 +392,18 @@ u32_t ll_adv_enable(u8_t enable) if (ll_adv.own_addr_type == BT_ADDR_LE_PUBLIC_ID || ll_adv.own_addr_type == BT_ADDR_LE_RANDOM_ID) { /* Look up the resolving list */ - int idx = ll_rl_find(ll_adv.id_addr_type, - ll_adv.id_addr); + rl_idx = ll_rl_find(ll_adv.id_addr_type, + ll_adv.id_addr); - if (idx >= 0) { + if (rl_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); + ll_rl_pdu_adv_update(rl_idx, pdu_adv); + ll_rl_pdu_adv_update(rl_idx, pdu_scan); priv = true; + rl_idx = rl_idx >= 0 ? rl_idx : RL_IDX_NONE; } #endif /* !CONFIG_BLUETOOTH_CONTROLLER_PRIVACY */ if (!priv) { @@ -413,10 +415,10 @@ u32_t ll_adv_enable(u8_t enable) } #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); + ll_adv.filter_policy, rl_idx); #else /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ status = radio_adv_enable(ll_adv.interval, ll_adv.chl_map, - ll_adv.filter_policy); + ll_adv.filter_policy, rl_idx); #endif /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */ return status; diff --git a/subsys/bluetooth/controller/ll_sw/ll_adv.h b/subsys/bluetooth/controller/ll_sw/ll_adv.h index 0b21acb7cec..8a02b9b6f2b 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_adv.h +++ b/subsys/bluetooth/controller/ll_sw/ll_adv.h @@ -8,7 +8,7 @@ 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 rl_idx:4; u8_t own_addr_type:2; u8_t id_addr_type:1; u8_t id_addr[BDADDR_SIZE]; diff --git a/subsys/bluetooth/controller/ll_sw/ll_filter.c b/subsys/bluetooth/controller/ll_sw/ll_filter.c index e0cf0f8b80e..bc52bb0128d 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_filter.c +++ b/subsys/bluetooth/controller/ll_sw/ll_filter.c @@ -33,8 +33,6 @@ u8_t wl_anon; #if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) #include "common/rpa.h" -#define IDX_NONE 0xF - /* Whitelist peer list */ static struct { u8_t taken:1; @@ -119,7 +117,7 @@ static u32_t wl_peers_add(bt_addr_le_t *id_addr) wl_peers[i].rl_idx = j; rl[j].wl = 1; } else { - wl_peers[i].rl_idx = IDX_NONE; + wl_peers[i].rl_idx = RL_IDX_NONE; } wl_peers[i].taken = 1; return 0; @@ -136,7 +134,8 @@ static u32_t wl_peers_remove(bt_addr_le_t *id_addr) if (i >= 0) { int j = wl_peers[i].rl_idx; - if (j != IDX_NONE) { + + if (j != RL_IDX_NONE) { rl[j].wl = 0; } wl_peers[i].taken = 0; @@ -205,9 +204,48 @@ static u32_t filter_remove(struct ll_filter *filter, u8_t addr_type, } #endif -struct ll_filter *ctrl_filter_get(void) +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) +u8_t *ctrl_irks_get(u8_t *count) { - return &wl; + *count = peer_irk_count; + return (u8_t *)peer_irks; +} + +bool ctrl_irk_whitelisted(u8_t irkmatch_id) +{ + u8_t i; + + LL_ASSERT(irkmatch_id < peer_irk_count); + i = peer_irk_rl_ids[irkmatch_id]; + LL_ASSERT(i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE); + LL_ASSERT(rl[i].taken); + + return rl[i].wl; +} + +bool ctrl_rl_idx_match(u8_t irkmatch_id, u8_t rl_idx) +{ + u8_t i; + + LL_ASSERT(irkmatch_id < peer_irk_count); + i = peer_irk_rl_ids[irkmatch_id]; + LL_ASSERT(i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE); + LL_ASSERT(rl[i].taken); + + return i == rl_idx; +} +#endif + +struct ll_filter *ctrl_filter_get(bool whitelist) +{ + if (whitelist) { + return &wl; + } +#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY) + return &rl_filter; +#else + LL_ASSERT(0); +#endif } u32_t ll_wl_size_get(void) @@ -279,7 +317,8 @@ static void filter_wl_update(void) for (i = 0; i < WL_SIZE; i++) { int j = wl_peers[i].rl_idx; - if (!rl_enable || j == IDX_NONE || !rl[j].pirk || rl[j].dev) { + if (!rl_enable || j == RL_IDX_NONE || !rl[j].pirk || + rl[j].dev) { filter_insert(&wl, i, wl_peers[i].id_addr_type, wl_peers[i].id_addr.val); } @@ -331,12 +370,12 @@ void ll_filters_scan_update(u8_t scan_fp) int ll_rl_find(u8_t id_addr_type, u8_t *id_addr) { - int i, free = -IDX_NONE; + int i, free = -RL_IDX_NONE; for (i = 0; i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; i++) { if (LIST_MATCH(rl, i, id_addr_type, id_addr)) { return i; - } else if (!rl[i].taken && free == -IDX_NONE) { + } else if (!rl[i].taken && free == -RL_IDX_NONE) { free = -i; } } @@ -344,6 +383,32 @@ int ll_rl_find(u8_t id_addr_type, u8_t *id_addr) return free; } +bool ctrl_rl_allowed(u8_t id_addr_type, u8_t *id_addr) +{ + int i, j; + + if (!rl_enable) { + return true; + } + + for (i = 0; i < CONFIG_BLUETOOTH_CONTROLLER_RL_SIZE; i++) { + if (rl[i].taken && (rl[i].id_addr_type == id_addr_type)) { + u8_t *addr = rl[i].id_addr.val; + for (j = 0; j < BDADDR_SIZE; j++) { + if (addr[j] != id_addr[j]) { + break; + } + } + + if (j == BDADDR_SIZE) { + return !rl[i].pirk || rl[i].dev; + } + } + } + + return false; +} + bool ctrl_rl_enabled(void) { return rl_enable; @@ -421,7 +486,6 @@ static void rpa_adv_refresh(void) pdu->chan_sel = 0; } - idx = ll_rl_find(ll_adv->id_addr_type, ll_adv->id_addr); LL_ASSERT(idx >= 0); ll_rl_pdu_adv_update(idx, pdu); @@ -555,7 +619,7 @@ u32_t ll_rl_add(bt_addr_le_t *id_addr, const u8_t pirk[16], i = ll_rl_find(id_addr->type, id_addr->a.val); if (i >= 0) { return BT_HCI_ERR_INVALID_PARAM; - } else if (i == -IDX_NONE) { + } else if (i == -RL_IDX_NONE) { return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; } @@ -628,7 +692,7 @@ u32_t ll_rl_remove(bt_addr_le_t *id_addr) /* Check if referenced by a whitelist entry */ j = wl_peers_find(id_addr->type, id_addr->a.val); if (j >= 0) { - wl_peers[j].rl_idx = IDX_NONE; + wl_peers[j].rl_idx = RL_IDX_NONE; } rl[i].taken = 0; return 0; diff --git a/subsys/bluetooth/controller/ll_sw/ll_filter.h b/subsys/bluetooth/controller/ll_sw/ll_filter.h index 3dbd01f2ca9..ce54478fed3 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_filter.h +++ b/subsys/bluetooth/controller/ll_sw/ll_filter.h @@ -6,6 +6,8 @@ #define WL_SIZE 8 +#define RL_IDX_NONE 0xF + struct ll_filter { u8_t enable_bitmask; u8_t addr_type_bitmask; @@ -16,10 +18,14 @@ void ll_filter_reset(bool init); void ll_filters_adv_update(u8_t adv_fp); void ll_filters_scan_update(u8_t scan_fp); -struct ll_filter *ctrl_filter_get(void); +struct ll_filter *ctrl_filter_get(bool whitelist); +u8_t *ctrl_irks_get(u8_t *count); +bool ctrl_irk_whitelisted(u8_t irkmatch_id); +bool ctrl_rl_idx_match(u8_t irkmatch_id, u8_t rl_idx); bool ctrl_rl_enabled(void); void ll_rl_rpa_update(bool timeout); int ll_rl_find(u8_t id_addr_type, u8_t *id_addr); +bool ctrl_rl_allowed(u8_t id_addr_type, u8_t *id_addr); void ll_rl_pdu_adv_update(int idx, struct pdu_adv *pdu);