diff --git a/include/net/bluetooth/hci.h b/include/net/bluetooth/hci.h index c5f6b82b9d11..7f5f00ff53da 100644 --- a/include/net/bluetooth/hci.h +++ b/include/net/bluetooth/hci.h @@ -1940,6 +1940,16 @@ struct hci_rp_le_read_transmit_power { __s8 max_le_tx_power; } __packed; +#define HCI_NETWORK_PRIVACY 0x00 +#define HCI_DEVICE_PRIVACY 0x01 + +#define HCI_OP_LE_SET_PRIVACY_MODE 0x204e +struct hci_cp_le_set_privacy_mode { + __u8 bdaddr_type; + bdaddr_t bdaddr; + __u8 mode; +} __packed; + #define HCI_OP_LE_READ_BUFFER_SIZE_V2 0x2060 struct hci_rp_le_read_buffer_size_v2 { __u8 status; diff --git a/include/net/bluetooth/hci_core.h b/include/net/bluetooth/hci_core.h index cf24af649c7f..4d69dcfebd63 100644 --- a/include/net/bluetooth/hci_core.h +++ b/include/net/bluetooth/hci_core.h @@ -757,6 +757,7 @@ struct hci_conn_params { struct hci_conn *conn; bool explicit_connect; DECLARE_BITMAP(flags, __HCI_CONN_NUM_FLAGS); + u8 privacy_mode; }; extern struct list_head hci_dev_list; diff --git a/net/bluetooth/hci_event.c b/net/bluetooth/hci_event.c index df1aca5f28da..c15289b59c3c 100644 --- a/net/bluetooth/hci_event.c +++ b/net/bluetooth/hci_event.c @@ -1489,6 +1489,33 @@ static u8 hci_cc_le_read_transmit_power(struct hci_dev *hdev, void *data, return rp->status; } +static u8 hci_cc_le_set_privacy_mode(struct hci_dev *hdev, void *data, + struct sk_buff *skb) +{ + struct hci_ev_status *rp = data; + struct hci_cp_le_set_privacy_mode *cp; + struct hci_conn_params *params; + + bt_dev_dbg(hdev, "status 0x%2.2x", rp->status); + + if (rp->status) + return rp->status; + + cp = hci_sent_cmd_data(hdev, HCI_OP_LE_SET_PRIVACY_MODE); + if (!cp) + return rp->status; + + hci_dev_lock(hdev); + + params = hci_conn_params_lookup(hdev, &cp->bdaddr, cp->bdaddr_type); + if (params) + params->privacy_mode = cp->mode; + + hci_dev_unlock(hdev); + + return rp->status; +} + static u8 hci_cc_le_set_adv_enable(struct hci_dev *hdev, void *data, struct sk_buff *skb) { @@ -3887,7 +3914,8 @@ static const struct hci_cc { HCI_CC_STATUS(HCI_OP_LE_REMOVE_ADV_SET, hci_cc_le_remove_adv_set), HCI_CC_STATUS(HCI_OP_LE_CLEAR_ADV_SETS, hci_cc_le_clear_adv_sets), HCI_CC(HCI_OP_LE_READ_TRANSMIT_POWER, hci_cc_le_read_transmit_power, - sizeof(struct hci_rp_le_read_transmit_power)) + sizeof(struct hci_rp_le_read_transmit_power)), + HCI_CC_STATUS(HCI_OP_LE_SET_PRIVACY_MODE, hci_cc_le_set_privacy_mode) }; static u8 hci_cc_func(struct hci_dev *hdev, const struct hci_cc *cc, diff --git a/net/bluetooth/hci_sync.c b/net/bluetooth/hci_sync.c index c1b01718a797..3d28ca7ebe45 100644 --- a/net/bluetooth/hci_sync.c +++ b/net/bluetooth/hci_sync.c @@ -1597,8 +1597,40 @@ done: sizeof(cp), &cp, HCI_CMD_TIMEOUT); } +/* Set Device Privacy Mode. */ +static int hci_le_set_privacy_mode_sync(struct hci_dev *hdev, + struct hci_conn_params *params) +{ + struct hci_cp_le_set_privacy_mode cp; + struct smp_irk *irk; + + /* If device privacy mode has already been set there is nothing to do */ + if (params->privacy_mode == HCI_DEVICE_PRIVACY) + return 0; + + /* Check if HCI_CONN_FLAG_DEVICE_PRIVACY has been set as it also + * indicates that LL Privacy has been enabled and + * HCI_OP_LE_SET_PRIVACY_MODE is supported. + */ + if (!test_bit(HCI_CONN_FLAG_DEVICE_PRIVACY, params->flags)) + return 0; + + irk = hci_find_irk_by_addr(hdev, ¶ms->addr, params->addr_type); + if (!irk) + return 0; + + memset(&cp, 0, sizeof(cp)); + cp.bdaddr_type = irk->addr_type; + bacpy(&cp.bdaddr, &irk->bdaddr); + cp.mode = HCI_DEVICE_PRIVACY; + + return __hci_cmd_sync_status(hdev, HCI_OP_LE_SET_PRIVACY_MODE, + sizeof(cp), &cp, HCI_CMD_TIMEOUT); +} + /* Adds connection to allow list if needed, if the device uses RPA (has IRK) - * this attempts to program the device in the resolving list as well. + * this attempts to program the device in the resolving list as well and + * properly set the privacy mode. */ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, struct hci_conn_params *params, @@ -1607,11 +1639,6 @@ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, struct hci_cp_le_add_to_accept_list cp; int err; - /* Already in accept list */ - if (hci_bdaddr_list_lookup(&hdev->le_accept_list, ¶ms->addr, - params->addr_type)) - return 0; - /* Select filter policy to accept all advertising */ if (*num_entries >= hdev->le_accept_list_size) return -ENOSPC; @@ -1637,6 +1664,18 @@ static int hci_le_add_accept_list_sync(struct hci_dev *hdev, return err; } + /* Set Privacy Mode */ + err = hci_le_set_privacy_mode_sync(hdev, params); + if (err) { + bt_dev_err(hdev, "Unable to set privacy mode: %d", err); + return err; + } + + /* Check if already in accept list */ + if (hci_bdaddr_list_lookup(&hdev->le_accept_list, ¶ms->addr, + params->addr_type)) + return 0; + *num_entries += 1; cp.bdaddr_type = params->addr_type; bacpy(&cp.bdaddr, ¶ms->addr);