projects/linux·blob master · refs

1206 lines · 30.0 KiB

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