diff --git a/subsys/bluetooth/controller/hci/hci.c b/subsys/bluetooth/controller/hci/hci.c index 7f3168950eb..f80241425b3 100644 --- a/subsys/bluetooth/controller/hci/hci.c +++ b/subsys/bluetooth/controller/hci/hci.c @@ -205,9 +205,8 @@ static void read_supported_commands(struct net_buf *buf, struct net_buf *evt) /* All LE commands in this octet. */ rp->commands[26] = 0xFF; /* All LE commands in this octet, - * except LE Remove Device From White List */ - rp->commands[27] = 0xFD; + rp->commands[27] = 0xFF; /* LE Start Encryption, LE Long Term Key Req Reply, * LE Long Term Key Req Neg Reply. and * LE Read Supported States. @@ -480,6 +479,18 @@ static void le_add_dev_to_wl(struct net_buf *buf, struct net_buf *evt) ccst->status = (!status) ? 0x00 : BT_HCI_ERR_MEM_CAPACITY_EXCEEDED; } +static void le_rem_dev_from_wl(struct net_buf *buf, struct net_buf *evt) +{ + struct bt_hci_cp_le_rem_dev_from_wl *cmd = (void *)buf->data; + struct bt_hci_evt_cc_status *ccst; + uint32_t status; + + status = radio_filter_remove(cmd->addr.type, &cmd->addr.a.val[0]); + + ccst = cmd_complete(evt, sizeof(*ccst)); + ccst->status = (!status) ? 0x00 : BT_HCI_ERR_CMD_DISALLOWED; +} + static void le_conn_update(struct net_buf *buf, struct net_buf *evt) { struct hci_cp_le_conn_update *cmd = (void *)buf->data; @@ -736,6 +747,10 @@ static int controller_cmd_handle(uint8_t ocf, struct net_buf *cmd, le_add_dev_to_wl(cmd, evt); break; + case BT_OCF(BT_HCI_OP_LE_REM_DEV_FROM_WL): + le_rem_dev_from_wl(cmd, evt); + break; + case BT_OCF(BT_HCI_OP_LE_CONN_UPDATE): le_conn_update(cmd, evt); break; diff --git a/subsys/bluetooth/controller/ll/ctrl.c b/subsys/bluetooth/controller/ll/ctrl.c index 24a73e86c63..788ee45c803 100644 --- a/subsys/bluetooth/controller/ll/ctrl.c +++ b/subsys/bluetooth/controller/ll/ctrl.c @@ -6320,6 +6320,30 @@ uint32_t radio_filter_add(uint8_t addr_type, uint8_t *addr) return 1; } +uint32_t radio_filter_remove(uint8_t addr_type, uint8_t *addr) +{ + uint8_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 radio_irk_clear(void) { _radio.nirk = 0; diff --git a/subsys/bluetooth/controller/ll/ctrl.h b/subsys/bluetooth/controller/ll/ctrl.h index 5fc15bf147e..f9b092d708b 100644 --- a/subsys/bluetooth/controller/ll/ctrl.h +++ b/subsys/bluetooth/controller/ll/ctrl.h @@ -213,6 +213,7 @@ struct radio_adv_data *radio_adv_data_get(void); struct radio_adv_data *radio_scan_data_get(void); void radio_filter_clear(void); uint32_t radio_filter_add(uint8_t addr_type, uint8_t *addr); +uint32_t radio_filter_remove(uint8_t addr_type, uint8_t *addr); void radio_irk_clear(void); uint32_t radio_irk_add(uint8_t *irk); uint32_t radio_adv_enable(uint16_t interval, uint8_t chl_map,