diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c index 1c3d0885f96..60085b271e4 100644 --- a/subsys/bluetooth/controller/hci/hci.c +++ b/subsys/bluetooth/controller/hci/hci.c @@ -494,17 +494,15 @@ static void le_read_wl_size(struct net_buf *buf, struct net_buf **evt) rp = cmd_complete(evt, sizeof(*rp)); rp->status = 0x00; - rp->wl_size = 8; + rp->wl_size = ll_wl_size_get(); } static void le_clear_wl(struct net_buf *buf, struct net_buf **evt) { struct bt_hci_evt_cc_status *ccst; - ll_filter_clear(); - ccst = cmd_complete(evt, sizeof(*ccst)); - ccst->status = 0x00; + ccst->status = ll_wl_clear(); } static void le_add_dev_to_wl(struct net_buf *buf, struct net_buf **evt) @@ -513,10 +511,10 @@ static void le_add_dev_to_wl(struct net_buf *buf, struct net_buf **evt) struct bt_hci_evt_cc_status *ccst; u32_t status; - status = ll_filter_add(cmd->addr.type, &cmd->addr.a.val[0]); + status = ll_wl_add(&cmd->addr); ccst = cmd_complete(evt, sizeof(*ccst)); - ccst->status = (!status) ? 0x00 : BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; + ccst->status = status; } static void le_rem_dev_from_wl(struct net_buf *buf, struct net_buf **evt) @@ -525,10 +523,10 @@ static void le_rem_dev_from_wl(struct net_buf *buf, struct net_buf **evt) struct bt_hci_evt_cc_status *ccst; u32_t status; - status = ll_filter_remove(cmd->addr.type, &cmd->addr.a.val[0]); + status = ll_wl_remove(&cmd->addr); ccst = cmd_complete(evt, sizeof(*ccst)); - ccst->status = (!status) ? 0x00 : BT_HCI_ERR_CMD_DISALLOWED; + ccst->status = status; } static void le_encrypt(struct net_buf *buf, struct net_buf **evt) diff --git a/subsys/bluetooth/controller/include/ll.h b/subsys/bluetooth/controller/include/ll.h index 3f9feb70d1f..2b6eb35af69 100644 --- a/subsys/bluetooth/controller/include/ll.h +++ b/subsys/bluetooth/controller/include/ll.h @@ -22,9 +22,11 @@ u32_t ll_adv_enable(u8_t enable); u32_t ll_scan_params_set(u8_t type, u16_t interval, u16_t window, u8_t own_addr_type, u8_t filter_policy); u32_t ll_scan_enable(u8_t enable); -void ll_filter_clear(void); -u32_t ll_filter_add(u8_t addr_type, u8_t *addr); -u32_t ll_filter_remove(u8_t addr_type, u8_t *addr); + +u32_t ll_wl_size_get(void); +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); void ll_irk_clear(void); u32_t ll_irk_add(u8_t *irk); diff --git a/subsys/bluetooth/controller/ll_sw/Makefile b/subsys/bluetooth/controller/ll_sw/Makefile index 5993b37168a..542099dbf91 100644 --- a/subsys/bluetooth/controller/ll_sw/Makefile +++ b/subsys/bluetooth/controller/ll_sw/Makefile @@ -2,7 +2,7 @@ ccflags-y += -I$(srctree)/subsys/bluetooth/controller/include ccflags-y += -I$(srctree)/subsys/bluetooth/controller ccflags-y += -I$(srctree)/subsys/bluetooth -obj-y += crypto.o ctrl.o ll.o +obj-y += crypto.o ctrl.o ll.o ll_filter.o obj-$(CONFIG_BLUETOOTH_BROADCASTER) += ll_adv.o obj-$(CONFIG_BLUETOOTH_OBSERVER) += ll_scan.o obj-$(CONFIG_BLUETOOTH_CENTRAL) += ll_master.o diff --git a/subsys/bluetooth/controller/ll_sw/ctrl.c b/subsys/bluetooth/controller/ll_sw/ctrl.c index 3baa73ba3c5..7dcee8dbd64 100644 --- a/subsys/bluetooth/controller/ll_sw/ctrl.c +++ b/subsys/bluetooth/controller/ll_sw/ctrl.c @@ -33,6 +33,9 @@ #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) #include "common/log.h" @@ -80,10 +83,6 @@ struct advertiser { u8_t chl_map_current:3; u8_t filter_policy:2; - u8_t filter_enable_bitmask; - u8_t filter_addr_type_bitmask; - u8_t filter_bdaddr[8][BDADDR_SIZE]; - struct radio_adv_data adv_data; struct radio_adv_data scan_data; @@ -105,10 +104,6 @@ struct scanner { u8_t init_addr[BDADDR_SIZE]; u32_t ticks_window; - u8_t filter_enable_bitmask; - u8_t filter_addr_type_bitmask; - u8_t filter_bdaddr[8][BDADDR_SIZE]; - u16_t conn_interval; u16_t conn_latency; u16_t conn_timeout; @@ -131,10 +126,6 @@ static struct { enum role volatile role; enum state state; - u8_t filter_enable_bitmask; - u8_t filter_addr_type_bitmask; - u8_t filter_bdaddr[8][BDADDR_SIZE]; - u8_t nirk; u8_t irk[RADIO_IRK_COUNT_MAX][16]; @@ -443,7 +434,6 @@ void ll_reset(void) } /* reset controller context members */ - _radio.filter_enable_bitmask = 0; _radio.nirk = 0; _radio.advertiser.is_enabled = 0; _radio.advertiser.conn = NULL; @@ -460,6 +450,8 @@ void ll_reset(void) _radio.packet_release_first = 0; _radio.packet_release_last = 0; + /* reset whitelist */ + ll_wl_clear(); /* memory allocations */ common_init(); } @@ -4795,9 +4787,12 @@ static void event_adv(u32_t ticks_at_expire, u32_t remainder, /* Setup Radio Filter */ if (_radio.advertiser.filter_policy) { - radio_filter_configure(_radio.advertiser.filter_enable_bitmask, - _radio.advertiser.filter_addr_type_bitmask, - (u8_t *)_radio.advertiser.filter_bdaddr); + + struct ll_wl *wl = ctrl_wl_get(); + + radio_filter_configure(wl->enable_bitmask, + wl->addr_type_bitmask, + (u8_t *)wl->bdaddr); } radio_tmr_start(1, @@ -4982,10 +4977,12 @@ static void event_scan(u32_t ticks_at_expire, u32_t remainder, u16_t lazy, /* Setup Radio Filter */ if (_radio.scanner.filter_policy) { - radio_filter_configure(_radio.scanner.filter_enable_bitmask, - _radio.scanner.filter_addr_type_bitmask, - (u8_t *)_radio.scanner.filter_bdaddr); + struct ll_wl *wl = ctrl_wl_get(); + + radio_filter_configure(wl->enable_bitmask, + wl->addr_type_bitmask, + (u8_t *)wl->bdaddr); if (_radio.nirk) { radio_ar_configure(_radio.nirk, _radio.irk); } @@ -7745,56 +7742,6 @@ struct radio_adv_data *radio_scan_data_get(void) return &_radio.advertiser.scan_data; } -void ll_filter_clear(void) -{ - _radio.filter_enable_bitmask = 0; - _radio.filter_addr_type_bitmask = 0; -} - -u32_t ll_filter_add(u8_t addr_type, u8_t *addr) -{ - if (_radio.filter_enable_bitmask != 0xFF) { - u8_t index; - - for (index = 0; - (_radio.filter_enable_bitmask & (1 << index)); - index++) { - } - _radio.filter_enable_bitmask |= (1 << index); - _radio.filter_addr_type_bitmask |= - ((addr_type & 0x01) << index); - memcpy(&_radio.filter_bdaddr[index][0], addr, BDADDR_SIZE); - - return 0; - } - - return 1; -} - -u32_t ll_filter_remove(u8_t addr_type, u8_t *addr) -{ - u8_t index; - - if (!_radio.filter_enable_bitmask) { - return 1; - } - - index = 8; - while (index--) { - if ((_radio.filter_enable_bitmask & BIT(index)) && - (((_radio.filter_addr_type_bitmask >> index) & 0x01) == - (addr_type & 0x01)) && - !memcmp(_radio.filter_bdaddr[index], addr, BDADDR_SIZE)) { - _radio.filter_enable_bitmask &= ~BIT(index); - _radio.filter_addr_type_bitmask &= ~BIT(index); - - return 0; - } - } - - return 1; -} - void ll_irk_clear(void) { _radio.nirk = 0; @@ -8172,15 +8119,6 @@ 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 (filter_policy) { - _radio.advertiser.filter_addr_type_bitmask = - _radio.filter_addr_type_bitmask; - memcpy(&_radio.advertiser.filter_bdaddr[0][0], - &_radio.filter_bdaddr[0][0], - sizeof(_radio.advertiser.filter_bdaddr)); - _radio.advertiser.filter_enable_bitmask = - _radio.filter_enable_bitmask; - } _radio.advertiser.hdr.ticks_active_to_start = _radio.ticks_active_to_start; @@ -8335,15 +8273,6 @@ u32_t radio_scan_enable(u8_t type, u8_t init_addr_type, u8_t *init_addr, _radio.scanner.ticks_window = TICKER_US_TO_TICKS((u64_t) window * 625); _radio.scanner.filter_policy = filter_policy; - if (filter_policy) { - _radio.scanner.filter_addr_type_bitmask = - _radio.filter_addr_type_bitmask; - memcpy(&_radio.scanner.filter_bdaddr[0][0], - &_radio.filter_bdaddr[0][0], - sizeof(_radio.scanner.filter_bdaddr)); - _radio.scanner.filter_enable_bitmask = - _radio.filter_enable_bitmask; - } _radio.scanner.hdr.ticks_active_to_start = _radio.ticks_active_to_start; diff --git a/subsys/bluetooth/controller/ll_sw/ll.c b/subsys/bluetooth/controller/ll_sw/ll.c index 32aeacc7863..3b5837281d2 100644 --- a/subsys/bluetooth/controller/ll_sw/ll.c +++ b/subsys/bluetooth/controller/ll_sw/ll.c @@ -14,6 +14,7 @@ #ifdef CONFIG_CLOCK_CONTROL_NRF5 #include #endif +#include #define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_HCI_DRIVER) #include "common/log.h" diff --git a/subsys/bluetooth/controller/ll_sw/ll_adv.c b/subsys/bluetooth/controller/ll_sw/ll_adv.c index e379d3b03c6..e77ef15d2d8 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_adv.c +++ b/subsys/bluetooth/controller/ll_sw/ll_adv.c @@ -8,6 +8,7 @@ #include #include +#include #include "util/util.h" diff --git a/subsys/bluetooth/controller/ll_sw/ll_filter.c b/subsys/bluetooth/controller/ll_sw/ll_filter.c new file mode 100644 index 00000000000..5275538f934 --- /dev/null +++ b/subsys/bluetooth/controller/ll_sw/ll_filter.c @@ -0,0 +1,108 @@ +/* + * Copyright (c) 2017 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#include + +#include +#include + +#include "util/util.h" + +#include "pdu.h" +#include "ctrl.h" +#include "ll.h" +#include "ll_filter.h" + +#define ADDR_TYPE_ANON 0xFF + +#define BT_DBG_ENABLED IS_ENABLED(CONFIG_BLUETOOTH_DEBUG_HCI_DRIVER) +#include "common/log.h" + +static struct ll_wl wl; + +struct ll_wl *ctrl_wl_get(void) +{ + return &wl; +} + +u32_t ll_wl_size_get(void) +{ + return WL_SIZE; +} + +u32_t ll_wl_clear(void) +{ + if (radio_adv_filter_pol_get() || (radio_scan_filter_pol_get() & 0x1)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + wl.enable_bitmask = 0; + wl.addr_type_bitmask = 0; + wl.anon = 0; + + return 0; +} + +u32_t ll_wl_add(bt_addr_le_t *addr) +{ + u8_t index; + + if (radio_adv_filter_pol_get() || (radio_scan_filter_pol_get() & 0x1)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + if (addr->type == ADDR_TYPE_ANON) { + wl.anon = 1; + return 0; + } + + if (wl.enable_bitmask == 0xFF) { + return BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; + } + + for (index = 0; + (wl.enable_bitmask & (1 << index)); + index++) { + } + wl.enable_bitmask |= (1 << index); + wl.addr_type_bitmask |= ((addr->type & 0x01) << index); + memcpy(&wl.bdaddr[index][0], addr->a.val, BDADDR_SIZE); + + return 0; +} + +u32_t ll_wl_remove(bt_addr_le_t *addr) +{ + u8_t index; + + if (radio_adv_filter_pol_get() || (radio_scan_filter_pol_get() & 0x1)) { + return BT_HCI_ERR_CMD_DISALLOWED; + } + + if (addr->type == ADDR_TYPE_ANON) { + wl.anon = 0; + return 0; + } + + if (!wl.enable_bitmask) { + return BT_HCI_ERR_INVALID_PARAM; + } + + index = 8; + while (index--) { + if ((wl.enable_bitmask & BIT(index)) && + (((wl.addr_type_bitmask >> index) & 0x01) == + (addr->type & 0x01)) && + !memcmp(wl.bdaddr[index], addr->a.val, BDADDR_SIZE)) { + wl.enable_bitmask &= ~BIT(index); + wl.addr_type_bitmask &= ~BIT(index); + return 0; + } + } + + return BT_HCI_ERR_INVALID_PARAM; +} + diff --git a/subsys/bluetooth/controller/ll_sw/ll_filter.h b/subsys/bluetooth/controller/ll_sw/ll_filter.h new file mode 100644 index 00000000000..4182f28def8 --- /dev/null +++ b/subsys/bluetooth/controller/ll_sw/ll_filter.h @@ -0,0 +1,17 @@ +/* + * Copyright (c) 2017 Nordic Semiconductor ASA + * + * SPDX-License-Identifier: Apache-2.0 + */ + +#define WL_SIZE 8 + +struct ll_wl { + u8_t enable_bitmask; + u8_t addr_type_bitmask; + u8_t bdaddr[WL_SIZE][BDADDR_SIZE]; + u8_t anon; +}; + +struct ll_wl *ctrl_wl_get(void); + diff --git a/subsys/bluetooth/controller/ll_sw/ll_master.c b/subsys/bluetooth/controller/ll_sw/ll_master.c index 8cc720e6962..f0794232435 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_master.c +++ b/subsys/bluetooth/controller/ll_sw/ll_master.c @@ -6,6 +6,7 @@ */ #include +#include #include "util/util.h" diff --git a/subsys/bluetooth/controller/ll_sw/ll_scan.c b/subsys/bluetooth/controller/ll_sw/ll_scan.c index 3402e98c7ab..7dadb985f4e 100644 --- a/subsys/bluetooth/controller/ll_sw/ll_scan.c +++ b/subsys/bluetooth/controller/ll_sw/ll_scan.c @@ -6,6 +6,7 @@ */ #include +#include #include "util/util.h"