xref: /openbmc/linux/net/bluetooth/msft.c (revision a85a60e6)
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
19 struct msft_cp_read_supported_features {
20 	__u8   sub_opcode;
21 } __packed;
22 
23 struct 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
33 struct msft_le_monitor_advertisement_pattern {
34 	__u8 length;
35 	__u8 data_type;
36 	__u8 start_byte;
37 	__u8 pattern[];
38 };
39 
40 struct msft_le_monitor_advertisement_pattern_data {
41 	__u8 count;
42 	__u8 data[];
43 };
44 
45 struct 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 
55 struct 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
62 struct msft_cp_le_cancel_monitor_advertisement {
63 	__u8 sub_opcode;
64 	__u8 handle;
65 } __packed;
66 
67 struct 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
73 struct msft_cp_le_set_advertisement_filter_enable {
74 	__u8 sub_opcode;
75 	__u8 enable;
76 } __packed;
77 
78 struct 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
84 struct msft_ev_le_monitor_device {
85 	__u8     addr_type;
86 	bdaddr_t bdaddr;
87 	__u8     monitor_handle;
88 	__u8     monitor_state;
89 } __packed;
90 
91 struct msft_monitor_advertisement_handle_data {
92 	__u8  msft_handle;
93 	__u16 mgmt_handle;
94 	__s8 rssi_high;
95 	__s8 rssi_low;
96 	__u8 rssi_low_interval;
97 	__u8 rssi_sampling_period;
98 	__u8 cond_type;
99 	struct list_head list;
100 };
101 
102 enum monitor_addr_filter_state {
103 	AF_STATE_IDLE,
104 	AF_STATE_ADDING,
105 	AF_STATE_ADDED,
106 	AF_STATE_REMOVING,
107 };
108 
109 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR	0x04
110 struct msft_monitor_addr_filter_data {
111 	__u8     msft_handle;
112 	__u8     pattern_handle; /* address filters pertain to */
113 	__u16    mgmt_handle;
114 	int      state;
115 	__s8     rssi_high;
116 	__s8     rssi_low;
117 	__u8     rssi_low_interval;
118 	__u8     rssi_sampling_period;
119 	__u8     addr_type;
120 	bdaddr_t bdaddr;
121 	struct list_head list;
122 };
123 
124 struct msft_data {
125 	__u64 features;
126 	__u8  evt_prefix_len;
127 	__u8  *evt_prefix;
128 	struct list_head handle_map;
129 	struct list_head address_filters;
130 	__u8 resuming;
131 	__u8 suspending;
132 	__u8 filter_enabled;
133 	/* To synchronize add/remove address filter and monitor device event.*/
134 	struct mutex filter_lock;
135 };
136 
msft_monitor_supported(struct hci_dev * hdev)137 bool msft_monitor_supported(struct hci_dev *hdev)
138 {
139 	return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
140 }
141 
read_supported_features(struct hci_dev * hdev,struct msft_data * msft)142 static bool read_supported_features(struct hci_dev *hdev,
143 				    struct msft_data *msft)
144 {
145 	struct msft_cp_read_supported_features cp;
146 	struct msft_rp_read_supported_features *rp;
147 	struct sk_buff *skb;
148 
149 	cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
150 
151 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
152 			     HCI_CMD_TIMEOUT);
153 	if (IS_ERR(skb)) {
154 		bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
155 			   PTR_ERR(skb));
156 		return false;
157 	}
158 
159 	if (skb->len < sizeof(*rp)) {
160 		bt_dev_err(hdev, "MSFT supported features length mismatch");
161 		goto failed;
162 	}
163 
164 	rp = (struct msft_rp_read_supported_features *)skb->data;
165 
166 	if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
167 		goto failed;
168 
169 	if (rp->evt_prefix_len > 0) {
170 		msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
171 					   GFP_KERNEL);
172 		if (!msft->evt_prefix)
173 			goto failed;
174 	}
175 
176 	msft->evt_prefix_len = rp->evt_prefix_len;
177 	msft->features = __le64_to_cpu(rp->features);
178 
179 	if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
180 		hdev->msft_curve_validity = true;
181 
182 	kfree_skb(skb);
183 	return true;
184 
185 failed:
186 	kfree_skb(skb);
187 	return false;
188 }
189 
190 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
191  * is_mgmt = false matches the handle used by the msft controller.
192  * This function requires the caller holds hdev->lock
193  */
msft_find_handle_data(struct hci_dev * hdev,u16 handle,bool is_mgmt)194 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
195 				(struct hci_dev *hdev, u16 handle, bool is_mgmt)
196 {
197 	struct msft_monitor_advertisement_handle_data *entry;
198 	struct msft_data *msft = hdev->msft_data;
199 
200 	list_for_each_entry(entry, &msft->handle_map, list) {
201 		if (is_mgmt && entry->mgmt_handle == handle)
202 			return entry;
203 		if (!is_mgmt && entry->msft_handle == handle)
204 			return entry;
205 	}
206 
207 	return NULL;
208 }
209 
210 /* This function requires the caller holds msft->filter_lock */
msft_find_address_data(struct hci_dev * hdev,u8 addr_type,bdaddr_t * addr,u8 pattern_handle)211 static struct msft_monitor_addr_filter_data *msft_find_address_data
212 			(struct hci_dev *hdev, u8 addr_type, bdaddr_t *addr,
213 			 u8 pattern_handle)
214 {
215 	struct msft_monitor_addr_filter_data *entry;
216 	struct msft_data *msft = hdev->msft_data;
217 
218 	list_for_each_entry(entry, &msft->address_filters, list) {
219 		if (entry->pattern_handle == pattern_handle &&
220 		    addr_type == entry->addr_type &&
221 		    !bacmp(addr, &entry->bdaddr))
222 			return entry;
223 	}
224 
225 	return NULL;
226 }
227 
228 /* This function requires the caller holds hdev->lock */
msft_monitor_device_del(struct hci_dev * hdev,__u16 mgmt_handle,bdaddr_t * bdaddr,__u8 addr_type,bool notify)229 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
230 				   bdaddr_t *bdaddr, __u8 addr_type,
231 				   bool notify)
232 {
233 	struct monitored_device *dev, *tmp;
234 	int count = 0;
235 
236 	list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
237 		/* mgmt_handle == 0 indicates remove all devices, whereas,
238 		 * bdaddr == NULL indicates remove all devices matching the
239 		 * mgmt_handle.
240 		 */
241 		if ((!mgmt_handle || dev->handle == mgmt_handle) &&
242 		    (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
243 				 addr_type == dev->addr_type))) {
244 			if (notify && dev->notified) {
245 				mgmt_adv_monitor_device_lost(hdev, dev->handle,
246 							     &dev->bdaddr,
247 							     dev->addr_type);
248 			}
249 
250 			list_del(&dev->list);
251 			kfree(dev);
252 			count++;
253 		}
254 	}
255 
256 	return count;
257 }
258 
msft_le_monitor_advertisement_cb(struct hci_dev * hdev,u16 opcode,struct adv_monitor * monitor,struct sk_buff * skb)259 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
260 					    struct adv_monitor *monitor,
261 					    struct sk_buff *skb)
262 {
263 	struct msft_rp_le_monitor_advertisement *rp;
264 	struct msft_monitor_advertisement_handle_data *handle_data;
265 	struct msft_data *msft = hdev->msft_data;
266 	int status = 0;
267 
268 	hci_dev_lock(hdev);
269 
270 	rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
271 	if (skb->len < sizeof(*rp)) {
272 		status = HCI_ERROR_UNSPECIFIED;
273 		goto unlock;
274 	}
275 
276 	status = rp->status;
277 	if (status)
278 		goto unlock;
279 
280 	handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
281 	if (!handle_data) {
282 		status = HCI_ERROR_UNSPECIFIED;
283 		goto unlock;
284 	}
285 
286 	handle_data->mgmt_handle = monitor->handle;
287 	handle_data->msft_handle = rp->handle;
288 	handle_data->cond_type   = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
289 	INIT_LIST_HEAD(&handle_data->list);
290 	list_add(&handle_data->list, &msft->handle_map);
291 
292 	monitor->state = ADV_MONITOR_STATE_OFFLOADED;
293 
294 unlock:
295 	if (status)
296 		hci_free_adv_monitor(hdev, monitor);
297 
298 	hci_dev_unlock(hdev);
299 
300 	return status;
301 }
302 
303 /* This function requires the caller holds hci_req_sync_lock */
msft_remove_addr_filters_sync(struct hci_dev * hdev,u8 handle)304 static void msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle)
305 {
306 	struct msft_monitor_addr_filter_data *address_filter, *n;
307 	struct msft_cp_le_cancel_monitor_advertisement cp;
308 	struct msft_data *msft = hdev->msft_data;
309 	struct list_head head;
310 	struct sk_buff *skb;
311 
312 	INIT_LIST_HEAD(&head);
313 
314 	/* Cancel all corresponding address monitors */
315 	mutex_lock(&msft->filter_lock);
316 
317 	list_for_each_entry_safe(address_filter, n, &msft->address_filters,
318 				 list) {
319 		if (address_filter->pattern_handle != handle)
320 			continue;
321 
322 		list_del(&address_filter->list);
323 
324 		/* Keep the address filter and let
325 		 * msft_add_address_filter_sync() remove and free the address
326 		 * filter.
327 		 */
328 		if (address_filter->state == AF_STATE_ADDING) {
329 			address_filter->state = AF_STATE_REMOVING;
330 			continue;
331 		}
332 
333 		/* Keep the address filter and let
334 		 * msft_cancel_address_filter_sync() remove and free the address
335 		 * filter
336 		 */
337 		if (address_filter->state == AF_STATE_REMOVING)
338 			continue;
339 
340 		list_add_tail(&address_filter->list, &head);
341 	}
342 
343 	mutex_unlock(&msft->filter_lock);
344 
345 	list_for_each_entry_safe(address_filter, n, &head, list) {
346 		list_del(&address_filter->list);
347 
348 		cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
349 		cp.handle = address_filter->msft_handle;
350 
351 		skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
352 				     HCI_CMD_TIMEOUT);
353 		if (IS_ERR(skb)) {
354 			kfree(address_filter);
355 			continue;
356 		}
357 
358 		kfree_skb(skb);
359 
360 		bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",
361 			   &address_filter->bdaddr);
362 
363 		kfree(address_filter);
364 	}
365 }
366 
msft_le_cancel_monitor_advertisement_cb(struct hci_dev * hdev,u16 opcode,struct adv_monitor * monitor,struct sk_buff * skb)367 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
368 						   u16 opcode,
369 						   struct adv_monitor *monitor,
370 						   struct sk_buff *skb)
371 {
372 	struct msft_rp_le_cancel_monitor_advertisement *rp;
373 	struct msft_monitor_advertisement_handle_data *handle_data;
374 	struct msft_data *msft = hdev->msft_data;
375 	int status = 0;
376 	u8 msft_handle;
377 
378 	rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
379 	if (skb->len < sizeof(*rp)) {
380 		status = HCI_ERROR_UNSPECIFIED;
381 		goto done;
382 	}
383 
384 	status = rp->status;
385 	if (status)
386 		goto done;
387 
388 	hci_dev_lock(hdev);
389 
390 	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
391 
392 	if (handle_data) {
393 		if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)
394 			monitor->state = ADV_MONITOR_STATE_REGISTERED;
395 
396 		/* Do not free the monitor if it is being removed due to
397 		 * suspend. It will be re-monitored on resume.
398 		 */
399 		if (!msft->suspending) {
400 			hci_free_adv_monitor(hdev, monitor);
401 
402 			/* Clear any monitored devices by this Adv Monitor */
403 			msft_monitor_device_del(hdev, handle_data->mgmt_handle,
404 						NULL, 0, false);
405 		}
406 
407 		msft_handle = handle_data->msft_handle;
408 
409 		list_del(&handle_data->list);
410 		kfree(handle_data);
411 
412 		hci_dev_unlock(hdev);
413 
414 		msft_remove_addr_filters_sync(hdev, msft_handle);
415 	} else {
416 		hci_dev_unlock(hdev);
417 	}
418 
419 done:
420 	return status;
421 }
422 
423 /* This function requires the caller holds hci_req_sync_lock */
msft_remove_monitor_sync(struct hci_dev * hdev,struct adv_monitor * monitor)424 static int msft_remove_monitor_sync(struct hci_dev *hdev,
425 				    struct adv_monitor *monitor)
426 {
427 	struct msft_cp_le_cancel_monitor_advertisement cp;
428 	struct msft_monitor_advertisement_handle_data *handle_data;
429 	struct sk_buff *skb;
430 
431 	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
432 
433 	/* If no matched handle, just remove without telling controller */
434 	if (!handle_data)
435 		return -ENOENT;
436 
437 	cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
438 	cp.handle = handle_data->msft_handle;
439 
440 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
441 			     HCI_CMD_TIMEOUT);
442 	if (IS_ERR(skb))
443 		return PTR_ERR(skb);
444 
445 	return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
446 						       monitor, skb);
447 }
448 
449 /* This function requires the caller holds hci_req_sync_lock */
msft_suspend_sync(struct hci_dev * hdev)450 int msft_suspend_sync(struct hci_dev *hdev)
451 {
452 	struct msft_data *msft = hdev->msft_data;
453 	struct adv_monitor *monitor;
454 	int handle = 0;
455 
456 	if (!msft || !msft_monitor_supported(hdev))
457 		return 0;
458 
459 	msft->suspending = true;
460 
461 	while (1) {
462 		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
463 		if (!monitor)
464 			break;
465 
466 		msft_remove_monitor_sync(hdev, monitor);
467 
468 		handle++;
469 	}
470 
471 	/* All monitors have been removed */
472 	msft->suspending = false;
473 
474 	return 0;
475 }
476 
msft_monitor_rssi_valid(struct adv_monitor * monitor)477 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
478 {
479 	struct adv_rssi_thresholds *r = &monitor->rssi;
480 
481 	if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
482 	    r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
483 	    r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
484 	    r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
485 		return false;
486 
487 	/* High_threshold_timeout is not supported,
488 	 * once high_threshold is reached, events are immediately reported.
489 	 */
490 	if (r->high_threshold_timeout != 0)
491 		return false;
492 
493 	if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
494 		return false;
495 
496 	/* Sampling period from 0x00 to 0xFF are all allowed */
497 	return true;
498 }
499 
msft_monitor_pattern_valid(struct adv_monitor * monitor)500 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
501 {
502 	return msft_monitor_rssi_valid(monitor);
503 	/* No additional check needed for pattern-based monitor */
504 }
505 
msft_add_monitor_sync(struct hci_dev * hdev,struct adv_monitor * monitor)506 static int msft_add_monitor_sync(struct hci_dev *hdev,
507 				 struct adv_monitor *monitor)
508 {
509 	struct msft_cp_le_monitor_advertisement *cp;
510 	struct msft_le_monitor_advertisement_pattern_data *pattern_data;
511 	struct msft_monitor_advertisement_handle_data *handle_data;
512 	struct msft_le_monitor_advertisement_pattern *pattern;
513 	struct adv_pattern *entry;
514 	size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
515 	ptrdiff_t offset = 0;
516 	u8 pattern_count = 0;
517 	struct sk_buff *skb;
518 	int err;
519 
520 	if (!msft_monitor_pattern_valid(monitor))
521 		return -EINVAL;
522 
523 	list_for_each_entry(entry, &monitor->patterns, list) {
524 		pattern_count++;
525 		total_size += sizeof(*pattern) + entry->length;
526 	}
527 
528 	cp = kmalloc(total_size, GFP_KERNEL);
529 	if (!cp)
530 		return -ENOMEM;
531 
532 	cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
533 	cp->rssi_high = monitor->rssi.high_threshold;
534 	cp->rssi_low = monitor->rssi.low_threshold;
535 	cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
536 	cp->rssi_sampling_period = monitor->rssi.sampling_period;
537 
538 	cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
539 
540 	pattern_data = (void *)cp->data;
541 	pattern_data->count = pattern_count;
542 
543 	list_for_each_entry(entry, &monitor->patterns, list) {
544 		pattern = (void *)(pattern_data->data + offset);
545 		/* the length also includes data_type and offset */
546 		pattern->length = entry->length + 2;
547 		pattern->data_type = entry->ad_type;
548 		pattern->start_byte = entry->offset;
549 		memcpy(pattern->pattern, entry->value, entry->length);
550 		offset += sizeof(*pattern) + entry->length;
551 	}
552 
553 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
554 			     HCI_CMD_TIMEOUT);
555 
556 	if (IS_ERR(skb)) {
557 		err = PTR_ERR(skb);
558 		goto out_free;
559 	}
560 
561 	err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
562 					       monitor, skb);
563 	if (err)
564 		goto out_free;
565 
566 	handle_data = msft_find_handle_data(hdev, monitor->handle, true);
567 	if (!handle_data) {
568 		err = -ENODATA;
569 		goto out_free;
570 	}
571 
572 	handle_data->rssi_high	= cp->rssi_high;
573 	handle_data->rssi_low	= cp->rssi_low;
574 	handle_data->rssi_low_interval	  = cp->rssi_low_interval;
575 	handle_data->rssi_sampling_period = cp->rssi_sampling_period;
576 
577 out_free:
578 	kfree(cp);
579 	return err;
580 }
581 
582 /* This function requires the caller holds hci_req_sync_lock */
reregister_monitor(struct hci_dev * hdev)583 static void reregister_monitor(struct hci_dev *hdev)
584 {
585 	struct adv_monitor *monitor;
586 	struct msft_data *msft = hdev->msft_data;
587 	int handle = 0;
588 
589 	if (!msft)
590 		return;
591 
592 	msft->resuming = true;
593 
594 	while (1) {
595 		monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
596 		if (!monitor)
597 			break;
598 
599 		msft_add_monitor_sync(hdev, monitor);
600 
601 		handle++;
602 	}
603 
604 	/* All monitors have been reregistered */
605 	msft->resuming = false;
606 }
607 
608 /* This function requires the caller holds hci_req_sync_lock */
msft_resume_sync(struct hci_dev * hdev)609 int msft_resume_sync(struct hci_dev *hdev)
610 {
611 	struct msft_data *msft = hdev->msft_data;
612 
613 	if (!msft || !msft_monitor_supported(hdev))
614 		return 0;
615 
616 	hci_dev_lock(hdev);
617 
618 	/* Clear already tracked devices on resume. Once the monitors are
619 	 * reregistered, devices in range will be found again after resume.
620 	 */
621 	hdev->advmon_pend_notify = false;
622 	msft_monitor_device_del(hdev, 0, NULL, 0, true);
623 
624 	hci_dev_unlock(hdev);
625 
626 	reregister_monitor(hdev);
627 
628 	return 0;
629 }
630 
631 /* This function requires the caller holds hci_req_sync_lock */
msft_do_open(struct hci_dev * hdev)632 void msft_do_open(struct hci_dev *hdev)
633 {
634 	struct msft_data *msft = hdev->msft_data;
635 
636 	if (hdev->msft_opcode == HCI_OP_NOP)
637 		return;
638 
639 	if (!msft) {
640 		bt_dev_err(hdev, "MSFT extension not registered");
641 		return;
642 	}
643 
644 	bt_dev_dbg(hdev, "Initialize MSFT extension");
645 
646 	/* Reset existing MSFT data before re-reading */
647 	kfree(msft->evt_prefix);
648 	msft->evt_prefix = NULL;
649 	msft->evt_prefix_len = 0;
650 	msft->features = 0;
651 
652 	if (!read_supported_features(hdev, msft)) {
653 		hdev->msft_data = NULL;
654 		kfree(msft);
655 		return;
656 	}
657 
658 	if (msft_monitor_supported(hdev)) {
659 		msft->resuming = true;
660 		msft_set_filter_enable(hdev, true);
661 		/* Monitors get removed on power off, so we need to explicitly
662 		 * tell the controller to re-monitor.
663 		 */
664 		reregister_monitor(hdev);
665 	}
666 }
667 
msft_do_close(struct hci_dev * hdev)668 void msft_do_close(struct hci_dev *hdev)
669 {
670 	struct msft_data *msft = hdev->msft_data;
671 	struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
672 	struct msft_monitor_addr_filter_data *address_filter, *n;
673 	struct adv_monitor *monitor;
674 
675 	if (!msft)
676 		return;
677 
678 	bt_dev_dbg(hdev, "Cleanup of MSFT extension");
679 
680 	/* The controller will silently remove all monitors on power off.
681 	 * Therefore, remove handle_data mapping and reset monitor state.
682 	 */
683 	list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
684 		monitor = idr_find(&hdev->adv_monitors_idr,
685 				   handle_data->mgmt_handle);
686 
687 		if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
688 			monitor->state = ADV_MONITOR_STATE_REGISTERED;
689 
690 		list_del(&handle_data->list);
691 		kfree(handle_data);
692 	}
693 
694 	mutex_lock(&msft->filter_lock);
695 	list_for_each_entry_safe(address_filter, n, &msft->address_filters,
696 				 list) {
697 		list_del(&address_filter->list);
698 		kfree(address_filter);
699 	}
700 	mutex_unlock(&msft->filter_lock);
701 
702 	hci_dev_lock(hdev);
703 
704 	/* Clear any devices that are being monitored and notify device lost */
705 	hdev->advmon_pend_notify = false;
706 	msft_monitor_device_del(hdev, 0, NULL, 0, true);
707 
708 	hci_dev_unlock(hdev);
709 }
710 
msft_cancel_address_filter_sync(struct hci_dev * hdev,void * data)711 static int msft_cancel_address_filter_sync(struct hci_dev *hdev, void *data)
712 {
713 	struct msft_monitor_addr_filter_data *address_filter = data;
714 	struct msft_cp_le_cancel_monitor_advertisement cp;
715 	struct msft_data *msft = hdev->msft_data;
716 	struct sk_buff *skb;
717 	int err = 0;
718 
719 	if (!msft) {
720 		bt_dev_err(hdev, "MSFT: msft data is freed");
721 		return -EINVAL;
722 	}
723 
724 	/* The address filter has been removed by hci dev close */
725 	if (!test_bit(HCI_UP, &hdev->flags))
726 		return 0;
727 
728 	mutex_lock(&msft->filter_lock);
729 	list_del(&address_filter->list);
730 	mutex_unlock(&msft->filter_lock);
731 
732 	cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
733 	cp.handle = address_filter->msft_handle;
734 
735 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
736 			     HCI_CMD_TIMEOUT);
737 	if (IS_ERR(skb)) {
738 		bt_dev_err(hdev, "MSFT: Failed to cancel address (%pMR) filter",
739 			   &address_filter->bdaddr);
740 		err = PTR_ERR(skb);
741 		goto done;
742 	}
743 	kfree_skb(skb);
744 
745 	bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",
746 		   &address_filter->bdaddr);
747 
748 done:
749 	kfree(address_filter);
750 
751 	return err;
752 }
753 
msft_register(struct hci_dev * hdev)754 void msft_register(struct hci_dev *hdev)
755 {
756 	struct msft_data *msft = NULL;
757 
758 	bt_dev_dbg(hdev, "Register MSFT extension");
759 
760 	msft = kzalloc(sizeof(*msft), GFP_KERNEL);
761 	if (!msft) {
762 		bt_dev_err(hdev, "Failed to register MSFT extension");
763 		return;
764 	}
765 
766 	INIT_LIST_HEAD(&msft->handle_map);
767 	INIT_LIST_HEAD(&msft->address_filters);
768 	hdev->msft_data = msft;
769 	mutex_init(&msft->filter_lock);
770 }
771 
msft_release(struct hci_dev * hdev)772 void msft_release(struct hci_dev *hdev)
773 {
774 	struct msft_data *msft = hdev->msft_data;
775 
776 	if (!msft)
777 		return;
778 
779 	bt_dev_dbg(hdev, "Unregister MSFT extension");
780 
781 	hdev->msft_data = NULL;
782 
783 	kfree(msft->evt_prefix);
784 	mutex_destroy(&msft->filter_lock);
785 	kfree(msft);
786 }
787 
788 /* This function requires the caller holds hdev->lock */
msft_device_found(struct hci_dev * hdev,bdaddr_t * bdaddr,__u8 addr_type,__u16 mgmt_handle)789 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
790 			      __u8 addr_type, __u16 mgmt_handle)
791 {
792 	struct monitored_device *dev;
793 
794 	dev = kmalloc(sizeof(*dev), GFP_KERNEL);
795 	if (!dev) {
796 		bt_dev_err(hdev, "MSFT vendor event %u: no memory",
797 			   MSFT_EV_LE_MONITOR_DEVICE);
798 		return;
799 	}
800 
801 	bacpy(&dev->bdaddr, bdaddr);
802 	dev->addr_type = addr_type;
803 	dev->handle = mgmt_handle;
804 	dev->notified = false;
805 
806 	INIT_LIST_HEAD(&dev->list);
807 	list_add(&dev->list, &hdev->monitored_devices);
808 	hdev->advmon_pend_notify = true;
809 }
810 
811 /* This function requires the caller holds hdev->lock */
msft_device_lost(struct hci_dev * hdev,bdaddr_t * bdaddr,__u8 addr_type,__u16 mgmt_handle)812 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
813 			     __u8 addr_type, __u16 mgmt_handle)
814 {
815 	if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
816 				     true)) {
817 		bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
818 			   MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
819 	}
820 }
821 
msft_skb_pull(struct hci_dev * hdev,struct sk_buff * skb,u8 ev,size_t len)822 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
823 			   u8 ev, size_t len)
824 {
825 	void *data;
826 
827 	data = skb_pull_data(skb, len);
828 	if (!data)
829 		bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
830 
831 	return data;
832 }
833 
msft_add_address_filter_sync(struct hci_dev * hdev,void * data)834 static int msft_add_address_filter_sync(struct hci_dev *hdev, void *data)
835 {
836 	struct msft_monitor_addr_filter_data *address_filter = data;
837 	struct msft_rp_le_monitor_advertisement *rp;
838 	struct msft_cp_le_monitor_advertisement *cp;
839 	struct msft_data *msft = hdev->msft_data;
840 	struct sk_buff *skb = NULL;
841 	bool remove = false;
842 	size_t size;
843 
844 	if (!msft) {
845 		bt_dev_err(hdev, "MSFT: msft data is freed");
846 		return -EINVAL;
847 	}
848 
849 	/* The address filter has been removed by hci dev close */
850 	if (!test_bit(HCI_UP, &hdev->flags))
851 		return -ENODEV;
852 
853 	/* We are safe to use the address filter from now on.
854 	 * msft_monitor_device_evt() wouldn't delete this filter because it's
855 	 * not been added by now.
856 	 * And all other functions that requiring hci_req_sync_lock wouldn't
857 	 * touch this filter before this func completes because it's protected
858 	 * by hci_req_sync_lock.
859 	 */
860 
861 	if (address_filter->state == AF_STATE_REMOVING) {
862 		mutex_lock(&msft->filter_lock);
863 		list_del(&address_filter->list);
864 		mutex_unlock(&msft->filter_lock);
865 		kfree(address_filter);
866 		return 0;
867 	}
868 
869 	size = sizeof(*cp) +
870 	       sizeof(address_filter->addr_type) +
871 	       sizeof(address_filter->bdaddr);
872 	cp = kzalloc(size, GFP_KERNEL);
873 	if (!cp) {
874 		bt_dev_err(hdev, "MSFT: Alloc cmd param err");
875 		remove = true;
876 		goto done;
877 	}
878 
879 	cp->sub_opcode           = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
880 	cp->rssi_high		 = address_filter->rssi_high;
881 	cp->rssi_low		 = address_filter->rssi_low;
882 	cp->rssi_low_interval    = address_filter->rssi_low_interval;
883 	cp->rssi_sampling_period = address_filter->rssi_sampling_period;
884 	cp->cond_type            = MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR;
885 	cp->data[0]              = address_filter->addr_type;
886 	memcpy(&cp->data[1], &address_filter->bdaddr,
887 	       sizeof(address_filter->bdaddr));
888 
889 	skb = __hci_cmd_sync(hdev, hdev->msft_opcode, size, cp,
890 			     HCI_CMD_TIMEOUT);
891 	kfree(cp);
892 
893 	if (IS_ERR(skb)) {
894 		bt_dev_err(hdev, "Failed to enable address %pMR filter",
895 			   &address_filter->bdaddr);
896 		skb = NULL;
897 		remove = true;
898 		goto done;
899 	}
900 
901 	rp = skb_pull_data(skb, sizeof(*rp));
902 	if (!rp || rp->sub_opcode != MSFT_OP_LE_MONITOR_ADVERTISEMENT ||
903 	    rp->status)
904 		remove = true;
905 
906 done:
907 	mutex_lock(&msft->filter_lock);
908 
909 	if (remove) {
910 		bt_dev_warn(hdev, "MSFT: Remove address (%pMR) filter",
911 			    &address_filter->bdaddr);
912 		list_del(&address_filter->list);
913 		kfree(address_filter);
914 	} else {
915 		address_filter->state = AF_STATE_ADDED;
916 		address_filter->msft_handle = rp->handle;
917 		bt_dev_dbg(hdev, "MSFT: Address %pMR filter enabled",
918 			   &address_filter->bdaddr);
919 	}
920 	mutex_unlock(&msft->filter_lock);
921 
922 	kfree_skb(skb);
923 
924 	return 0;
925 }
926 
927 /* This function requires the caller holds msft->filter_lock */
msft_add_address_filter(struct hci_dev * hdev,u8 addr_type,bdaddr_t * bdaddr,struct msft_monitor_advertisement_handle_data * handle_data)928 static struct msft_monitor_addr_filter_data *msft_add_address_filter
929 		(struct hci_dev *hdev, u8 addr_type, bdaddr_t *bdaddr,
930 		 struct msft_monitor_advertisement_handle_data *handle_data)
931 {
932 	struct msft_monitor_addr_filter_data *address_filter = NULL;
933 	struct msft_data *msft = hdev->msft_data;
934 	int err;
935 
936 	address_filter = kzalloc(sizeof(*address_filter), GFP_KERNEL);
937 	if (!address_filter)
938 		return NULL;
939 
940 	address_filter->state             = AF_STATE_ADDING;
941 	address_filter->msft_handle       = 0xff;
942 	address_filter->pattern_handle    = handle_data->msft_handle;
943 	address_filter->mgmt_handle       = handle_data->mgmt_handle;
944 	address_filter->rssi_high         = handle_data->rssi_high;
945 	address_filter->rssi_low          = handle_data->rssi_low;
946 	address_filter->rssi_low_interval = handle_data->rssi_low_interval;
947 	address_filter->rssi_sampling_period = handle_data->rssi_sampling_period;
948 	address_filter->addr_type            = addr_type;
949 	bacpy(&address_filter->bdaddr, bdaddr);
950 
951 	/* With the above AF_STATE_ADDING, duplicated address filter can be
952 	 * avoided when receiving monitor device event (found/lost) frequently
953 	 * for the same device.
954 	 */
955 	list_add_tail(&address_filter->list, &msft->address_filters);
956 
957 	err = hci_cmd_sync_queue(hdev, msft_add_address_filter_sync,
958 				 address_filter, NULL);
959 	if (err < 0) {
960 		bt_dev_err(hdev, "MSFT: Add address %pMR filter err", bdaddr);
961 		list_del(&address_filter->list);
962 		kfree(address_filter);
963 		return NULL;
964 	}
965 
966 	bt_dev_dbg(hdev, "MSFT: Add device %pMR address filter",
967 		   &address_filter->bdaddr);
968 
969 	return address_filter;
970 }
971 
972 /* This function requires the caller holds hdev->lock */
msft_monitor_device_evt(struct hci_dev * hdev,struct sk_buff * skb)973 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
974 {
975 	struct msft_monitor_addr_filter_data *n, *address_filter = NULL;
976 	struct msft_ev_le_monitor_device *ev;
977 	struct msft_monitor_advertisement_handle_data *handle_data;
978 	struct msft_data *msft = hdev->msft_data;
979 	u16 mgmt_handle = 0xffff;
980 	u8 addr_type;
981 
982 	ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
983 	if (!ev)
984 		return;
985 
986 	bt_dev_dbg(hdev,
987 		   "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
988 		   MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
989 		   ev->monitor_state, &ev->bdaddr);
990 
991 	handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
992 
993 	if (!test_bit(HCI_QUIRK_USE_MSFT_EXT_ADDRESS_FILTER, &hdev->quirks)) {
994 		if (!handle_data)
995 			return;
996 		mgmt_handle = handle_data->mgmt_handle;
997 		goto report_state;
998 	}
999 
1000 	if (handle_data) {
1001 		/* Don't report any device found/lost event from pattern
1002 		 * monitors. Pattern monitor always has its address filters for
1003 		 * tracking devices.
1004 		 */
1005 
1006 		address_filter = msft_find_address_data(hdev, ev->addr_type,
1007 							&ev->bdaddr,
1008 							handle_data->msft_handle);
1009 		if (address_filter)
1010 			return;
1011 
1012 		if (ev->monitor_state && handle_data->cond_type ==
1013 				MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN)
1014 			msft_add_address_filter(hdev, ev->addr_type,
1015 						&ev->bdaddr, handle_data);
1016 
1017 		return;
1018 	}
1019 
1020 	/* This device event is not from pattern monitor.
1021 	 * Report it if there is a corresponding address_filter for it.
1022 	 */
1023 	list_for_each_entry(n, &msft->address_filters, list) {
1024 		if (n->state == AF_STATE_ADDED &&
1025 		    n->msft_handle == ev->monitor_handle) {
1026 			mgmt_handle = n->mgmt_handle;
1027 			address_filter = n;
1028 			break;
1029 		}
1030 	}
1031 
1032 	if (!address_filter) {
1033 		bt_dev_warn(hdev, "MSFT: Unexpected device event %pMR, %u, %u",
1034 			    &ev->bdaddr, ev->monitor_handle, ev->monitor_state);
1035 		return;
1036 	}
1037 
1038 report_state:
1039 	switch (ev->addr_type) {
1040 	case ADDR_LE_DEV_PUBLIC:
1041 		addr_type = BDADDR_LE_PUBLIC;
1042 		break;
1043 
1044 	case ADDR_LE_DEV_RANDOM:
1045 		addr_type = BDADDR_LE_RANDOM;
1046 		break;
1047 
1048 	default:
1049 		bt_dev_err(hdev,
1050 			   "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
1051 			   MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
1052 		return;
1053 	}
1054 
1055 	if (ev->monitor_state) {
1056 		msft_device_found(hdev, &ev->bdaddr, addr_type, mgmt_handle);
1057 	} else {
1058 		if (address_filter && address_filter->state == AF_STATE_ADDED) {
1059 			address_filter->state = AF_STATE_REMOVING;
1060 			hci_cmd_sync_queue(hdev,
1061 					   msft_cancel_address_filter_sync,
1062 					   address_filter,
1063 					   NULL);
1064 		}
1065 		msft_device_lost(hdev, &ev->bdaddr, addr_type, mgmt_handle);
1066 	}
1067 }
1068 
msft_vendor_evt(struct hci_dev * hdev,void * data,struct sk_buff * skb)1069 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
1070 {
1071 	struct msft_data *msft = hdev->msft_data;
1072 	u8 *evt_prefix;
1073 	u8 *evt;
1074 
1075 	if (!msft)
1076 		return;
1077 
1078 	/* When the extension has defined an event prefix, check that it
1079 	 * matches, and otherwise just return.
1080 	 */
1081 	if (msft->evt_prefix_len > 0) {
1082 		evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
1083 		if (!evt_prefix)
1084 			return;
1085 
1086 		if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
1087 			return;
1088 	}
1089 
1090 	/* Every event starts at least with an event code and the rest of
1091 	 * the data is variable and depends on the event code.
1092 	 */
1093 	if (skb->len < 1)
1094 		return;
1095 
1096 	evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
1097 	if (!evt)
1098 		return;
1099 
1100 	hci_dev_lock(hdev);
1101 
1102 	switch (*evt) {
1103 	case MSFT_EV_LE_MONITOR_DEVICE:
1104 		mutex_lock(&msft->filter_lock);
1105 		msft_monitor_device_evt(hdev, skb);
1106 		mutex_unlock(&msft->filter_lock);
1107 		break;
1108 
1109 	default:
1110 		bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
1111 		break;
1112 	}
1113 
1114 	hci_dev_unlock(hdev);
1115 }
1116 
msft_get_features(struct hci_dev * hdev)1117 __u64 msft_get_features(struct hci_dev *hdev)
1118 {
1119 	struct msft_data *msft = hdev->msft_data;
1120 
1121 	return msft ? msft->features : 0;
1122 }
1123 
msft_le_set_advertisement_filter_enable_cb(struct hci_dev * hdev,void * user_data,u8 status)1124 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
1125 						       void *user_data,
1126 						       u8 status)
1127 {
1128 	struct msft_cp_le_set_advertisement_filter_enable *cp = user_data;
1129 	struct msft_data *msft = hdev->msft_data;
1130 
1131 	/* Error 0x0C would be returned if the filter enabled status is
1132 	 * already set to whatever we were trying to set.
1133 	 * Although the default state should be disabled, some controller set
1134 	 * the initial value to enabled. Because there is no way to know the
1135 	 * actual initial value before sending this command, here we also treat
1136 	 * error 0x0C as success.
1137 	 */
1138 	if (status != 0x00 && status != 0x0C)
1139 		return;
1140 
1141 	hci_dev_lock(hdev);
1142 
1143 	msft->filter_enabled = cp->enable;
1144 
1145 	if (status == 0x0C)
1146 		bt_dev_warn(hdev, "MSFT filter_enable is already %s",
1147 			    cp->enable ? "on" : "off");
1148 
1149 	hci_dev_unlock(hdev);
1150 }
1151 
1152 /* This function requires the caller holds hci_req_sync_lock */
msft_add_monitor_pattern(struct hci_dev * hdev,struct adv_monitor * monitor)1153 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
1154 {
1155 	struct msft_data *msft = hdev->msft_data;
1156 
1157 	if (!msft)
1158 		return -EOPNOTSUPP;
1159 
1160 	if (msft->resuming || msft->suspending)
1161 		return -EBUSY;
1162 
1163 	return msft_add_monitor_sync(hdev, monitor);
1164 }
1165 
1166 /* This function requires the caller holds hci_req_sync_lock */
msft_remove_monitor(struct hci_dev * hdev,struct adv_monitor * monitor)1167 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
1168 {
1169 	struct msft_data *msft = hdev->msft_data;
1170 
1171 	if (!msft)
1172 		return -EOPNOTSUPP;
1173 
1174 	if (msft->resuming || msft->suspending)
1175 		return -EBUSY;
1176 
1177 	return msft_remove_monitor_sync(hdev, monitor);
1178 }
1179 
msft_set_filter_enable(struct hci_dev * hdev,bool enable)1180 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
1181 {
1182 	struct msft_cp_le_set_advertisement_filter_enable cp;
1183 	struct msft_data *msft = hdev->msft_data;
1184 	int err;
1185 
1186 	if (!msft)
1187 		return -EOPNOTSUPP;
1188 
1189 	cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
1190 	cp.enable = enable;
1191 	err = __hci_cmd_sync_status(hdev, hdev->msft_opcode, sizeof(cp), &cp,
1192 				    HCI_CMD_TIMEOUT);
1193 
1194 	msft_le_set_advertisement_filter_enable_cb(hdev, &cp, err);
1195 
1196 	return 0;
1197 }
1198 
msft_curve_validity(struct hci_dev * hdev)1199 bool msft_curve_validity(struct hci_dev *hdev)
1200 {
1201 	return hdev->msft_curve_validity;
1202 }
1203