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