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