Bluetooth: controller: Add inital support for Controller-based privacy

This initial commit adds the following:

* Handling of privacy HCI commands
* New Link Layer filter module for both whitelist and resolving list
* Advertising RPA generation with timeouts

Follow-up commits will expand the functionality.

Signed-off-by: Carles Cufi <carles.cufi@nordicsemi.no>

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)

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;

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

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);

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,

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 */
+	/* reset whitelist and resolving list */
-	ll_wl_clear();
+	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
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY)
-	 * PDU buffer, here. So that Scan Response has same AdvA.
+	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) ||

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)

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);

subsys/bluetooth/controller/ll_sw/ll_adv.c

@@ -16,17 +16,17 @@
 #include "ctrl.h"
 #include "ll.h"
 
-static struct {
+#include "hal/debug.h"
-	u8_t  chl_map:3;
-	u8_t  filter_policy:2;
 
-#if defined(CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT)
+#include "ll_filter.h"
-	u8_t  phy_p:3;
+#include "ll_adv.h"
-	u32_t interval;
+
-#else /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */
+static struct ll_adv_set ll_adv;
-	u16_t interval;
+
-#endif /* !CONFIG_BLUETOOTH_CONTROLLER_ADV_EXT */
+struct ll_adv_set *ll_adv_set_get(void)
-} ll_adv;
+{
+	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],
+		bool priv = false;
-		       ll_addr_get(pdu_adv->tx_addr, NULL), BDADDR_SIZE);
+#if defined(CONFIG_BLUETOOTH_CONTROLLER_PRIVACY)
-		memcpy(&pdu_scan->payload.scan_rsp.addr[0],
+		if (ctrl_rl_enabled()) {
-		       ll_addr_get(pdu_adv->tx_addr, NULL), BDADDR_SIZE);
+			/*@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);

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);

subsys/bluetooth/controller/ll_sw/ll_filter.c

@@ -10,10 +10,12 @@
 #include <bluetooth/hci.h>
 
 #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_clear();
-	wl.addr_type_bitmask = 0;
-	wl.anon = 0;
 
 	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 */
+
+}

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);

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).

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

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

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