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