cachepc-linux

Fork of AMDESE/linux with modifications for CachePC side-channel attack
git clone https://git.sinitax.com/sinitax/cachepc-linux
Log | Files | Refs | README | LICENSE | sfeed.txt

msft.c (24471B)


      1// SPDX-License-Identifier: GPL-2.0-only
      2/*
      3 * Copyright (C) 2020 Google Corporation
      4 */
      5
      6#include <net/bluetooth/bluetooth.h>
      7#include <net/bluetooth/hci_core.h>
      8#include <net/bluetooth/mgmt.h>
      9
     10#include "hci_request.h"
     11#include "mgmt_util.h"
     12#include "msft.h"
     13
     14#define MSFT_RSSI_THRESHOLD_VALUE_MIN		-127
     15#define MSFT_RSSI_THRESHOLD_VALUE_MAX		20
     16#define MSFT_RSSI_LOW_TIMEOUT_MAX		0x3C
     17
     18#define MSFT_OP_READ_SUPPORTED_FEATURES		0x00
     19struct msft_cp_read_supported_features {
     20	__u8   sub_opcode;
     21} __packed;
     22
     23struct msft_rp_read_supported_features {
     24	__u8   status;
     25	__u8   sub_opcode;
     26	__le64 features;
     27	__u8   evt_prefix_len;
     28	__u8   evt_prefix[];
     29} __packed;
     30
     31#define MSFT_OP_LE_MONITOR_ADVERTISEMENT	0x03
     32#define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN	0x01
     33struct msft_le_monitor_advertisement_pattern {
     34	__u8 length;
     35	__u8 data_type;
     36	__u8 start_byte;
     37	__u8 pattern[];
     38};
     39
     40struct msft_le_monitor_advertisement_pattern_data {
     41	__u8 count;
     42	__u8 data[];
     43};
     44
     45struct msft_cp_le_monitor_advertisement {
     46	__u8 sub_opcode;
     47	__s8 rssi_high;
     48	__s8 rssi_low;
     49	__u8 rssi_low_interval;
     50	__u8 rssi_sampling_period;
     51	__u8 cond_type;
     52	__u8 data[];
     53} __packed;
     54
     55struct msft_rp_le_monitor_advertisement {
     56	__u8 status;
     57	__u8 sub_opcode;
     58	__u8 handle;
     59} __packed;
     60
     61#define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT	0x04
     62struct msft_cp_le_cancel_monitor_advertisement {
     63	__u8 sub_opcode;
     64	__u8 handle;
     65} __packed;
     66
     67struct msft_rp_le_cancel_monitor_advertisement {
     68	__u8 status;
     69	__u8 sub_opcode;
     70} __packed;
     71
     72#define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE	0x05
     73struct msft_cp_le_set_advertisement_filter_enable {
     74	__u8 sub_opcode;
     75	__u8 enable;
     76} __packed;
     77
     78struct msft_rp_le_set_advertisement_filter_enable {
     79	__u8 status;
     80	__u8 sub_opcode;
     81} __packed;
     82
     83#define MSFT_EV_LE_MONITOR_DEVICE	0x02
     84struct msft_ev_le_monitor_device {
     85	__u8     addr_type;
     86	bdaddr_t bdaddr;
     87	__u8     monitor_handle;
     88	__u8     monitor_state;
     89} __packed;
     90
     91struct msft_monitor_advertisement_handle_data {
     92	__u8  msft_handle;
     93	__u16 mgmt_handle;
     94	struct list_head list;
     95};
     96
     97struct msft_data {
     98	__u64 features;
     99	__u8  evt_prefix_len;
    100	__u8  *evt_prefix;
    101	struct list_head handle_map;
    102	__u16 pending_add_handle;
    103	__u16 pending_remove_handle;
    104	__u8 resuming;
    105	__u8 suspending;
    106	__u8 filter_enabled;
    107};
    108
    109static int __msft_add_monitor_pattern(struct hci_dev *hdev,
    110				      struct adv_monitor *monitor);
    111static int __msft_remove_monitor(struct hci_dev *hdev,
    112				 struct adv_monitor *monitor, u16 handle);
    113
    114bool msft_monitor_supported(struct hci_dev *hdev)
    115{
    116	return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
    117}
    118
    119static bool read_supported_features(struct hci_dev *hdev,
    120				    struct msft_data *msft)
    121{
    122	struct msft_cp_read_supported_features cp;
    123	struct msft_rp_read_supported_features *rp;
    124	struct sk_buff *skb;
    125
    126	cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
    127
    128	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
    129			     HCI_CMD_TIMEOUT);
    130	if (IS_ERR(skb)) {
    131		bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
    132			   PTR_ERR(skb));
    133		return false;
    134	}
    135
    136	if (skb->len < sizeof(*rp)) {
    137		bt_dev_err(hdev, "MSFT supported features length mismatch");
    138		goto failed;
    139	}
    140
    141	rp = (struct msft_rp_read_supported_features *)skb->data;
    142
    143	if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
    144		goto failed;
    145
    146	if (rp->evt_prefix_len > 0) {
    147		msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
    148					   GFP_KERNEL);
    149		if (!msft->evt_prefix)
    150			goto failed;
    151	}
    152
    153	msft->evt_prefix_len = rp->evt_prefix_len;
    154	msft->features = __le64_to_cpu(rp->features);
    155
    156	if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
    157		hdev->msft_curve_validity = true;
    158
    159	kfree_skb(skb);
    160	return true;
    161
    162failed:
    163	kfree_skb(skb);
    164	return false;
    165}
    166
    167static void reregister_monitor(struct hci_dev *hdev, int handle)
    168{
    169	struct adv_monitor *monitor;
    170	struct msft_data *msft = hdev->msft_data;
    171	int err;
    172
    173	while (1) {
    174		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
    175		if (!monitor) {
    176			/* All monitors have been resumed */
    177			msft->resuming = false;
    178			hci_update_passive_scan(hdev);
    179			return;
    180		}
    181
    182		msft->pending_add_handle = (u16)handle;
    183		err = __msft_add_monitor_pattern(hdev, monitor);
    184
    185		/* If success, we return and wait for monitor added callback */
    186		if (!err)
    187			return;
    188
    189		/* Otherwise remove the monitor and keep registering */
    190		hci_free_adv_monitor(hdev, monitor);
    191		handle++;
    192	}
    193}
    194
    195/* is_mgmt = true matches the handle exposed to userspace via mgmt.
    196 * is_mgmt = false matches the handle used by the msft controller.
    197 * This function requires the caller holds hdev->lock
    198 */
    199static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
    200				(struct hci_dev *hdev, u16 handle, bool is_mgmt)
    201{
    202	struct msft_monitor_advertisement_handle_data *entry;
    203	struct msft_data *msft = hdev->msft_data;
    204
    205	list_for_each_entry(entry, &msft->handle_map, list) {
    206		if (is_mgmt && entry->mgmt_handle == handle)
    207			return entry;
    208		if (!is_mgmt && entry->msft_handle == handle)
    209			return entry;
    210	}
    211
    212	return NULL;
    213}
    214
    215/* This function requires the caller holds hdev->lock */
    216static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
    217				   bdaddr_t *bdaddr, __u8 addr_type,
    218				   bool notify)
    219{
    220	struct monitored_device *dev, *tmp;
    221	int count = 0;
    222
    223	list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
    224		/* mgmt_handle == 0 indicates remove all devices, whereas,
    225		 * bdaddr == NULL indicates remove all devices matching the
    226		 * mgmt_handle.
    227		 */
    228		if ((!mgmt_handle || dev->handle == mgmt_handle) &&
    229		    (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
    230				 addr_type == dev->addr_type))) {
    231			if (notify && dev->notified) {
    232				mgmt_adv_monitor_device_lost(hdev, dev->handle,
    233							     &dev->bdaddr,
    234							     dev->addr_type);
    235			}
    236
    237			list_del(&dev->list);
    238			kfree(dev);
    239			count++;
    240		}
    241	}
    242
    243	return count;
    244}
    245
    246static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
    247					     u8 status, u16 opcode,
    248					     struct sk_buff *skb)
    249{
    250	struct msft_rp_le_monitor_advertisement *rp;
    251	struct adv_monitor *monitor;
    252	struct msft_monitor_advertisement_handle_data *handle_data;
    253	struct msft_data *msft = hdev->msft_data;
    254
    255	hci_dev_lock(hdev);
    256
    257	monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
    258	if (!monitor) {
    259		bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
    260			   msft->pending_add_handle);
    261		status = HCI_ERROR_UNSPECIFIED;
    262		goto unlock;
    263	}
    264
    265	if (status)
    266		goto unlock;
    267
    268	rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
    269	if (skb->len < sizeof(*rp)) {
    270		status = HCI_ERROR_UNSPECIFIED;
    271		goto unlock;
    272	}
    273
    274	handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
    275	if (!handle_data) {
    276		status = HCI_ERROR_UNSPECIFIED;
    277		goto unlock;
    278	}
    279
    280	handle_data->mgmt_handle = monitor->handle;
    281	handle_data->msft_handle = rp->handle;
    282	INIT_LIST_HEAD(&handle_data->list);
    283	list_add(&handle_data->list, &msft->handle_map);
    284
    285	monitor->state = ADV_MONITOR_STATE_OFFLOADED;
    286
    287unlock:
    288	if (status && monitor)
    289		hci_free_adv_monitor(hdev, monitor);
    290
    291	hci_dev_unlock(hdev);
    292
    293	if (!msft->resuming)
    294		hci_add_adv_patterns_monitor_complete(hdev, status);
    295}
    296
    297static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
    298						    u8 status, u16 opcode,
    299						    struct sk_buff *skb)
    300{
    301	struct msft_cp_le_cancel_monitor_advertisement *cp;
    302	struct msft_rp_le_cancel_monitor_advertisement *rp;
    303	struct adv_monitor *monitor;
    304	struct msft_monitor_advertisement_handle_data *handle_data;
    305	struct msft_data *msft = hdev->msft_data;
    306	int err;
    307	bool pending;
    308
    309	if (status)
    310		goto done;
    311
    312	rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
    313	if (skb->len < sizeof(*rp)) {
    314		status = HCI_ERROR_UNSPECIFIED;
    315		goto done;
    316	}
    317
    318	hci_dev_lock(hdev);
    319
    320	cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
    321	handle_data = msft_find_handle_data(hdev, cp->handle, false);
    322
    323	if (handle_data) {
    324		monitor = idr_find(&hdev->adv_monitors_idr,
    325				   handle_data->mgmt_handle);
    326
    327		if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
    328			monitor->state = ADV_MONITOR_STATE_REGISTERED;
    329
    330		/* Do not free the monitor if it is being removed due to
    331		 * suspend. It will be re-monitored on resume.
    332		 */
    333		if (monitor && !msft->suspending) {
    334			hci_free_adv_monitor(hdev, monitor);
    335
    336			/* Clear any monitored devices by this Adv Monitor */
    337			msft_monitor_device_del(hdev, handle_data->mgmt_handle,
    338						NULL, 0, false);
    339		}
    340
    341		list_del(&handle_data->list);
    342		kfree(handle_data);
    343	}
    344
    345	/* If remove all monitors is required, we need to continue the process
    346	 * here because the earlier it was paused when waiting for the
    347	 * response from controller.
    348	 */
    349	if (msft->pending_remove_handle == 0) {
    350		pending = hci_remove_all_adv_monitor(hdev, &err);
    351		if (pending) {
    352			hci_dev_unlock(hdev);
    353			return;
    354		}
    355
    356		if (err)
    357			status = HCI_ERROR_UNSPECIFIED;
    358	}
    359
    360	hci_dev_unlock(hdev);
    361
    362done:
    363	if (!msft->suspending)
    364		hci_remove_adv_monitor_complete(hdev, status);
    365}
    366
    367static int msft_remove_monitor_sync(struct hci_dev *hdev,
    368				    struct adv_monitor *monitor)
    369{
    370	struct msft_cp_le_cancel_monitor_advertisement cp;
    371	struct msft_monitor_advertisement_handle_data *handle_data;
    372	struct sk_buff *skb;
    373	u8 status;
    374
    375	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
    376
    377	/* If no matched handle, just remove without telling controller */
    378	if (!handle_data)
    379		return -ENOENT;
    380
    381	cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
    382	cp.handle = handle_data->msft_handle;
    383
    384	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
    385			     HCI_CMD_TIMEOUT);
    386	if (IS_ERR(skb))
    387		return PTR_ERR(skb);
    388
    389	status = skb->data[0];
    390	skb_pull(skb, 1);
    391
    392	msft_le_cancel_monitor_advertisement_cb(hdev, status, hdev->msft_opcode,
    393						skb);
    394
    395	return status;
    396}
    397
    398/* This function requires the caller holds hci_req_sync_lock */
    399int msft_suspend_sync(struct hci_dev *hdev)
    400{
    401	struct msft_data *msft = hdev->msft_data;
    402	struct adv_monitor *monitor;
    403	int handle = 0;
    404
    405	if (!msft || !msft_monitor_supported(hdev))
    406		return 0;
    407
    408	msft->suspending = true;
    409
    410	while (1) {
    411		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
    412		if (!monitor)
    413			break;
    414
    415		msft_remove_monitor_sync(hdev, monitor);
    416
    417		handle++;
    418	}
    419
    420	/* All monitors have been removed */
    421	msft->suspending = false;
    422
    423	return 0;
    424}
    425
    426static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
    427{
    428	struct adv_rssi_thresholds *r = &monitor->rssi;
    429
    430	if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
    431	    r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
    432	    r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
    433	    r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
    434		return false;
    435
    436	/* High_threshold_timeout is not supported,
    437	 * once high_threshold is reached, events are immediately reported.
    438	 */
    439	if (r->high_threshold_timeout != 0)
    440		return false;
    441
    442	if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
    443		return false;
    444
    445	/* Sampling period from 0x00 to 0xFF are all allowed */
    446	return true;
    447}
    448
    449static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
    450{
    451	return msft_monitor_rssi_valid(monitor);
    452	/* No additional check needed for pattern-based monitor */
    453}
    454
    455static int msft_add_monitor_sync(struct hci_dev *hdev,
    456				 struct adv_monitor *monitor)
    457{
    458	struct msft_cp_le_monitor_advertisement *cp;
    459	struct msft_le_monitor_advertisement_pattern_data *pattern_data;
    460	struct msft_le_monitor_advertisement_pattern *pattern;
    461	struct adv_pattern *entry;
    462	size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
    463	ptrdiff_t offset = 0;
    464	u8 pattern_count = 0;
    465	struct sk_buff *skb;
    466	u8 status;
    467
    468	if (!msft_monitor_pattern_valid(monitor))
    469		return -EINVAL;
    470
    471	list_for_each_entry(entry, &monitor->patterns, list) {
    472		pattern_count++;
    473		total_size += sizeof(*pattern) + entry->length;
    474	}
    475
    476	cp = kmalloc(total_size, GFP_KERNEL);
    477	if (!cp)
    478		return -ENOMEM;
    479
    480	cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
    481	cp->rssi_high = monitor->rssi.high_threshold;
    482	cp->rssi_low = monitor->rssi.low_threshold;
    483	cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
    484	cp->rssi_sampling_period = monitor->rssi.sampling_period;
    485
    486	cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
    487
    488	pattern_data = (void *)cp->data;
    489	pattern_data->count = pattern_count;
    490
    491	list_for_each_entry(entry, &monitor->patterns, list) {
    492		pattern = (void *)(pattern_data->data + offset);
    493		/* the length also includes data_type and offset */
    494		pattern->length = entry->length + 2;
    495		pattern->data_type = entry->ad_type;
    496		pattern->start_byte = entry->offset;
    497		memcpy(pattern->pattern, entry->value, entry->length);
    498		offset += sizeof(*pattern) + entry->length;
    499	}
    500
    501	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
    502			     HCI_CMD_TIMEOUT);
    503	kfree(cp);
    504
    505	if (IS_ERR(skb))
    506		return PTR_ERR(skb);
    507
    508	status = skb->data[0];
    509	skb_pull(skb, 1);
    510
    511	msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb);
    512
    513	return status;
    514}
    515
    516/* This function requires the caller holds hci_req_sync_lock */
    517int msft_resume_sync(struct hci_dev *hdev)
    518{
    519	struct msft_data *msft = hdev->msft_data;
    520	struct adv_monitor *monitor;
    521	int handle = 0;
    522
    523	if (!msft || !msft_monitor_supported(hdev))
    524		return 0;
    525
    526	hci_dev_lock(hdev);
    527
    528	/* Clear already tracked devices on resume. Once the monitors are
    529	 * reregistered, devices in range will be found again after resume.
    530	 */
    531	hdev->advmon_pend_notify = false;
    532	msft_monitor_device_del(hdev, 0, NULL, 0, true);
    533
    534	hci_dev_unlock(hdev);
    535
    536	msft->resuming = true;
    537
    538	while (1) {
    539		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
    540		if (!monitor)
    541			break;
    542
    543		msft_add_monitor_sync(hdev, monitor);
    544
    545		handle++;
    546	}
    547
    548	/* All monitors have been resumed */
    549	msft->resuming = false;
    550
    551	return 0;
    552}
    553
    554void msft_do_open(struct hci_dev *hdev)
    555{
    556	struct msft_data *msft = hdev->msft_data;
    557
    558	if (hdev->msft_opcode == HCI_OP_NOP)
    559		return;
    560
    561	if (!msft) {
    562		bt_dev_err(hdev, "MSFT extension not registered");
    563		return;
    564	}
    565
    566	bt_dev_dbg(hdev, "Initialize MSFT extension");
    567
    568	/* Reset existing MSFT data before re-reading */
    569	kfree(msft->evt_prefix);
    570	msft->evt_prefix = NULL;
    571	msft->evt_prefix_len = 0;
    572	msft->features = 0;
    573
    574	if (!read_supported_features(hdev, msft)) {
    575		hdev->msft_data = NULL;
    576		kfree(msft);
    577		return;
    578	}
    579
    580	if (msft_monitor_supported(hdev)) {
    581		msft->resuming = true;
    582		msft_set_filter_enable(hdev, true);
    583		/* Monitors get removed on power off, so we need to explicitly
    584		 * tell the controller to re-monitor.
    585		 */
    586		reregister_monitor(hdev, 0);
    587	}
    588}
    589
    590void msft_do_close(struct hci_dev *hdev)
    591{
    592	struct msft_data *msft = hdev->msft_data;
    593	struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
    594	struct adv_monitor *monitor;
    595
    596	if (!msft)
    597		return;
    598
    599	bt_dev_dbg(hdev, "Cleanup of MSFT extension");
    600
    601	/* The controller will silently remove all monitors on power off.
    602	 * Therefore, remove handle_data mapping and reset monitor state.
    603	 */
    604	list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
    605		monitor = idr_find(&hdev->adv_monitors_idr,
    606				   handle_data->mgmt_handle);
    607
    608		if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
    609			monitor->state = ADV_MONITOR_STATE_REGISTERED;
    610
    611		list_del(&handle_data->list);
    612		kfree(handle_data);
    613	}
    614
    615	hci_dev_lock(hdev);
    616
    617	/* Clear any devices that are being monitored and notify device lost */
    618	hdev->advmon_pend_notify = false;
    619	msft_monitor_device_del(hdev, 0, NULL, 0, true);
    620
    621	hci_dev_unlock(hdev);
    622}
    623
    624void msft_register(struct hci_dev *hdev)
    625{
    626	struct msft_data *msft = NULL;
    627
    628	bt_dev_dbg(hdev, "Register MSFT extension");
    629
    630	msft = kzalloc(sizeof(*msft), GFP_KERNEL);
    631	if (!msft) {
    632		bt_dev_err(hdev, "Failed to register MSFT extension");
    633		return;
    634	}
    635
    636	INIT_LIST_HEAD(&msft->handle_map);
    637	hdev->msft_data = msft;
    638}
    639
    640void msft_unregister(struct hci_dev *hdev)
    641{
    642	struct msft_data *msft = hdev->msft_data;
    643
    644	if (!msft)
    645		return;
    646
    647	bt_dev_dbg(hdev, "Unregister MSFT extension");
    648
    649	hdev->msft_data = NULL;
    650
    651	kfree(msft->evt_prefix);
    652	kfree(msft);
    653}
    654
    655/* This function requires the caller holds hdev->lock */
    656static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
    657			      __u8 addr_type, __u16 mgmt_handle)
    658{
    659	struct monitored_device *dev;
    660
    661	dev = kmalloc(sizeof(*dev), GFP_KERNEL);
    662	if (!dev) {
    663		bt_dev_err(hdev, "MSFT vendor event %u: no memory",
    664			   MSFT_EV_LE_MONITOR_DEVICE);
    665		return;
    666	}
    667
    668	bacpy(&dev->bdaddr, bdaddr);
    669	dev->addr_type = addr_type;
    670	dev->handle = mgmt_handle;
    671	dev->notified = false;
    672
    673	INIT_LIST_HEAD(&dev->list);
    674	list_add(&dev->list, &hdev->monitored_devices);
    675	hdev->advmon_pend_notify = true;
    676}
    677
    678/* This function requires the caller holds hdev->lock */
    679static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
    680			     __u8 addr_type, __u16 mgmt_handle)
    681{
    682	if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
    683				     true)) {
    684		bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
    685			   MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
    686	}
    687}
    688
    689static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
    690			   u8 ev, size_t len)
    691{
    692	void *data;
    693
    694	data = skb_pull_data(skb, len);
    695	if (!data)
    696		bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
    697
    698	return data;
    699}
    700
    701/* This function requires the caller holds hdev->lock */
    702static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
    703{
    704	struct msft_ev_le_monitor_device *ev;
    705	struct msft_monitor_advertisement_handle_data *handle_data;
    706	u8 addr_type;
    707
    708	ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
    709	if (!ev)
    710		return;
    711
    712	bt_dev_dbg(hdev,
    713		   "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
    714		   MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
    715		   ev->monitor_state, &ev->bdaddr);
    716
    717	handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
    718	if (!handle_data)
    719		return;
    720
    721	switch (ev->addr_type) {
    722	case ADDR_LE_DEV_PUBLIC:
    723		addr_type = BDADDR_LE_PUBLIC;
    724		break;
    725
    726	case ADDR_LE_DEV_RANDOM:
    727		addr_type = BDADDR_LE_RANDOM;
    728		break;
    729
    730	default:
    731		bt_dev_err(hdev,
    732			   "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
    733			   MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
    734		return;
    735	}
    736
    737	if (ev->monitor_state)
    738		msft_device_found(hdev, &ev->bdaddr, addr_type,
    739				  handle_data->mgmt_handle);
    740	else
    741		msft_device_lost(hdev, &ev->bdaddr, addr_type,
    742				 handle_data->mgmt_handle);
    743}
    744
    745void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
    746{
    747	struct msft_data *msft = hdev->msft_data;
    748	u8 *evt_prefix;
    749	u8 *evt;
    750
    751	if (!msft)
    752		return;
    753
    754	/* When the extension has defined an event prefix, check that it
    755	 * matches, and otherwise just return.
    756	 */
    757	if (msft->evt_prefix_len > 0) {
    758		evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
    759		if (!evt_prefix)
    760			return;
    761
    762		if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
    763			return;
    764	}
    765
    766	/* Every event starts at least with an event code and the rest of
    767	 * the data is variable and depends on the event code.
    768	 */
    769	if (skb->len < 1)
    770		return;
    771
    772	evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
    773	if (!evt)
    774		return;
    775
    776	hci_dev_lock(hdev);
    777
    778	switch (*evt) {
    779	case MSFT_EV_LE_MONITOR_DEVICE:
    780		msft_monitor_device_evt(hdev, skb);
    781		break;
    782
    783	default:
    784		bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
    785		break;
    786	}
    787
    788	hci_dev_unlock(hdev);
    789}
    790
    791__u64 msft_get_features(struct hci_dev *hdev)
    792{
    793	struct msft_data *msft = hdev->msft_data;
    794
    795	return msft ? msft->features : 0;
    796}
    797
    798static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
    799						       u8 status, u16 opcode,
    800						       struct sk_buff *skb)
    801{
    802	struct msft_cp_le_set_advertisement_filter_enable *cp;
    803	struct msft_rp_le_set_advertisement_filter_enable *rp;
    804	struct msft_data *msft = hdev->msft_data;
    805
    806	rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
    807	if (skb->len < sizeof(*rp))
    808		return;
    809
    810	/* Error 0x0C would be returned if the filter enabled status is
    811	 * already set to whatever we were trying to set.
    812	 * Although the default state should be disabled, some controller set
    813	 * the initial value to enabled. Because there is no way to know the
    814	 * actual initial value before sending this command, here we also treat
    815	 * error 0x0C as success.
    816	 */
    817	if (status != 0x00 && status != 0x0C)
    818		return;
    819
    820	hci_dev_lock(hdev);
    821
    822	cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
    823	msft->filter_enabled = cp->enable;
    824
    825	if (status == 0x0C)
    826		bt_dev_warn(hdev, "MSFT filter_enable is already %s",
    827			    cp->enable ? "on" : "off");
    828
    829	hci_dev_unlock(hdev);
    830}
    831
    832/* This function requires the caller holds hdev->lock */
    833static int __msft_add_monitor_pattern(struct hci_dev *hdev,
    834				      struct adv_monitor *monitor)
    835{
    836	struct msft_cp_le_monitor_advertisement *cp;
    837	struct msft_le_monitor_advertisement_pattern_data *pattern_data;
    838	struct msft_le_monitor_advertisement_pattern *pattern;
    839	struct adv_pattern *entry;
    840	struct hci_request req;
    841	struct msft_data *msft = hdev->msft_data;
    842	size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
    843	ptrdiff_t offset = 0;
    844	u8 pattern_count = 0;
    845	int err = 0;
    846
    847	if (!msft_monitor_pattern_valid(monitor))
    848		return -EINVAL;
    849
    850	list_for_each_entry(entry, &monitor->patterns, list) {
    851		pattern_count++;
    852		total_size += sizeof(*pattern) + entry->length;
    853	}
    854
    855	cp = kmalloc(total_size, GFP_KERNEL);
    856	if (!cp)
    857		return -ENOMEM;
    858
    859	cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
    860	cp->rssi_high = monitor->rssi.high_threshold;
    861	cp->rssi_low = monitor->rssi.low_threshold;
    862	cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
    863	cp->rssi_sampling_period = monitor->rssi.sampling_period;
    864
    865	cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
    866
    867	pattern_data = (void *)cp->data;
    868	pattern_data->count = pattern_count;
    869
    870	list_for_each_entry(entry, &monitor->patterns, list) {
    871		pattern = (void *)(pattern_data->data + offset);
    872		/* the length also includes data_type and offset */
    873		pattern->length = entry->length + 2;
    874		pattern->data_type = entry->ad_type;
    875		pattern->start_byte = entry->offset;
    876		memcpy(pattern->pattern, entry->value, entry->length);
    877		offset += sizeof(*pattern) + entry->length;
    878	}
    879
    880	hci_req_init(&req, hdev);
    881	hci_req_add(&req, hdev->msft_opcode, total_size, cp);
    882	err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
    883	kfree(cp);
    884
    885	if (!err)
    886		msft->pending_add_handle = monitor->handle;
    887
    888	return err;
    889}
    890
    891/* This function requires the caller holds hdev->lock */
    892int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
    893{
    894	struct msft_data *msft = hdev->msft_data;
    895
    896	if (!msft)
    897		return -EOPNOTSUPP;
    898
    899	if (msft->resuming || msft->suspending)
    900		return -EBUSY;
    901
    902	return __msft_add_monitor_pattern(hdev, monitor);
    903}
    904
    905/* This function requires the caller holds hdev->lock */
    906static int __msft_remove_monitor(struct hci_dev *hdev,
    907				 struct adv_monitor *monitor, u16 handle)
    908{
    909	struct msft_cp_le_cancel_monitor_advertisement cp;
    910	struct msft_monitor_advertisement_handle_data *handle_data;
    911	struct hci_request req;
    912	struct msft_data *msft = hdev->msft_data;
    913	int err = 0;
    914
    915	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
    916
    917	/* If no matched handle, just remove without telling controller */
    918	if (!handle_data)
    919		return -ENOENT;
    920
    921	cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
    922	cp.handle = handle_data->msft_handle;
    923
    924	hci_req_init(&req, hdev);
    925	hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
    926	err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
    927
    928	if (!err)
    929		msft->pending_remove_handle = handle;
    930
    931	return err;
    932}
    933
    934/* This function requires the caller holds hdev->lock */
    935int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
    936			u16 handle)
    937{
    938	struct msft_data *msft = hdev->msft_data;
    939
    940	if (!msft)
    941		return -EOPNOTSUPP;
    942
    943	if (msft->resuming || msft->suspending)
    944		return -EBUSY;
    945
    946	return __msft_remove_monitor(hdev, monitor, handle);
    947}
    948
    949void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
    950{
    951	struct hci_dev *hdev = req->hdev;
    952	struct msft_cp_le_set_advertisement_filter_enable cp;
    953
    954	cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
    955	cp.enable = enable;
    956
    957	hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
    958}
    959
    960int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
    961{
    962	struct hci_request req;
    963	struct msft_data *msft = hdev->msft_data;
    964	int err;
    965
    966	if (!msft)
    967		return -EOPNOTSUPP;
    968
    969	hci_req_init(&req, hdev);
    970	msft_req_add_set_filter_enable(&req, enable);
    971	err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
    972
    973	return err;
    974}
    975
    976bool msft_curve_validity(struct hci_dev *hdev)
    977{
    978	return hdev->msft_curve_validity;
    979}