Linux kernel mirror (for testing)
git.kernel.org/pub/scm/linux/kernel/git/torvalds/linux.git
tjh.dev
kernel
os
linux
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 __u16 pending_add_handle;
103 __u16 pending_remove_handle;
104 __u8 resuming;
105 __u8 suspending;
106 __u8 filter_enabled;
107};
108
109static int __msft_add_monitor_pattern(struct hci_dev *hdev,
110 struct adv_monitor *monitor);
111static int __msft_remove_monitor(struct hci_dev *hdev,
112 struct adv_monitor *monitor, u16 handle);
113
114bool msft_monitor_supported(struct hci_dev *hdev)
115{
116 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
117}
118
119static bool read_supported_features(struct hci_dev *hdev,
120 struct msft_data *msft)
121{
122 struct msft_cp_read_supported_features cp;
123 struct msft_rp_read_supported_features *rp;
124 struct sk_buff *skb;
125
126 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
127
128 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
129 HCI_CMD_TIMEOUT);
130 if (IS_ERR(skb)) {
131 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
132 PTR_ERR(skb));
133 return false;
134 }
135
136 if (skb->len < sizeof(*rp)) {
137 bt_dev_err(hdev, "MSFT supported features length mismatch");
138 goto failed;
139 }
140
141 rp = (struct msft_rp_read_supported_features *)skb->data;
142
143 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
144 goto failed;
145
146 if (rp->evt_prefix_len > 0) {
147 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
148 GFP_KERNEL);
149 if (!msft->evt_prefix)
150 goto failed;
151 }
152
153 msft->evt_prefix_len = rp->evt_prefix_len;
154 msft->features = __le64_to_cpu(rp->features);
155
156 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
157 hdev->msft_curve_validity = true;
158
159 kfree_skb(skb);
160 return true;
161
162failed:
163 kfree_skb(skb);
164 return false;
165}
166
167static void reregister_monitor(struct hci_dev *hdev, int handle)
168{
169 struct adv_monitor *monitor;
170 struct msft_data *msft = hdev->msft_data;
171 int err;
172
173 while (1) {
174 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
175 if (!monitor) {
176 /* All monitors have been resumed */
177 msft->resuming = false;
178 hci_update_passive_scan(hdev);
179 return;
180 }
181
182 msft->pending_add_handle = (u16)handle;
183 err = __msft_add_monitor_pattern(hdev, monitor);
184
185 /* If success, we return and wait for monitor added callback */
186 if (!err)
187 return;
188
189 /* Otherwise remove the monitor and keep registering */
190 hci_free_adv_monitor(hdev, monitor);
191 handle++;
192 }
193}
194
195/* is_mgmt = true matches the handle exposed to userspace via mgmt.
196 * is_mgmt = false matches the handle used by the msft controller.
197 * This function requires the caller holds hdev->lock
198 */
199static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
200 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
201{
202 struct msft_monitor_advertisement_handle_data *entry;
203 struct msft_data *msft = hdev->msft_data;
204
205 list_for_each_entry(entry, &msft->handle_map, list) {
206 if (is_mgmt && entry->mgmt_handle == handle)
207 return entry;
208 if (!is_mgmt && entry->msft_handle == handle)
209 return entry;
210 }
211
212 return NULL;
213}
214
215/* This function requires the caller holds hdev->lock */
216static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
217 bdaddr_t *bdaddr, __u8 addr_type,
218 bool notify)
219{
220 struct monitored_device *dev, *tmp;
221 int count = 0;
222
223 list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
224 /* mgmt_handle == 0 indicates remove all devices, whereas,
225 * bdaddr == NULL indicates remove all devices matching the
226 * mgmt_handle.
227 */
228 if ((!mgmt_handle || dev->handle == mgmt_handle) &&
229 (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
230 addr_type == dev->addr_type))) {
231 if (notify && dev->notified) {
232 mgmt_adv_monitor_device_lost(hdev, dev->handle,
233 &dev->bdaddr,
234 dev->addr_type);
235 }
236
237 list_del(&dev->list);
238 kfree(dev);
239 count++;
240 }
241 }
242
243 return count;
244}
245
246static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
247 u8 status, u16 opcode,
248 struct sk_buff *skb)
249{
250 struct msft_rp_le_monitor_advertisement *rp;
251 struct adv_monitor *monitor;
252 struct msft_monitor_advertisement_handle_data *handle_data;
253 struct msft_data *msft = hdev->msft_data;
254
255 hci_dev_lock(hdev);
256
257 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
258 if (!monitor) {
259 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
260 msft->pending_add_handle);
261 status = HCI_ERROR_UNSPECIFIED;
262 goto unlock;
263 }
264
265 if (status)
266 goto unlock;
267
268 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
269 if (skb->len < sizeof(*rp)) {
270 status = HCI_ERROR_UNSPECIFIED;
271 goto unlock;
272 }
273
274 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
275 if (!handle_data) {
276 status = HCI_ERROR_UNSPECIFIED;
277 goto unlock;
278 }
279
280 handle_data->mgmt_handle = monitor->handle;
281 handle_data->msft_handle = rp->handle;
282 INIT_LIST_HEAD(&handle_data->list);
283 list_add(&handle_data->list, &msft->handle_map);
284
285 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
286
287unlock:
288 if (status && monitor)
289 hci_free_adv_monitor(hdev, monitor);
290
291 hci_dev_unlock(hdev);
292
293 if (!msft->resuming)
294 hci_add_adv_patterns_monitor_complete(hdev, status);
295}
296
297static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
298 u8 status, u16 opcode,
299 struct sk_buff *skb)
300{
301 struct msft_cp_le_cancel_monitor_advertisement *cp;
302 struct msft_rp_le_cancel_monitor_advertisement *rp;
303 struct adv_monitor *monitor;
304 struct msft_monitor_advertisement_handle_data *handle_data;
305 struct msft_data *msft = hdev->msft_data;
306 int err;
307 bool pending;
308
309 if (status)
310 goto done;
311
312 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
313 if (skb->len < sizeof(*rp)) {
314 status = HCI_ERROR_UNSPECIFIED;
315 goto done;
316 }
317
318 hci_dev_lock(hdev);
319
320 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
321 handle_data = msft_find_handle_data(hdev, cp->handle, false);
322
323 if (handle_data) {
324 monitor = idr_find(&hdev->adv_monitors_idr,
325 handle_data->mgmt_handle);
326
327 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
328 monitor->state = ADV_MONITOR_STATE_REGISTERED;
329
330 /* Do not free the monitor if it is being removed due to
331 * suspend. It will be re-monitored on resume.
332 */
333 if (monitor && !msft->suspending) {
334 hci_free_adv_monitor(hdev, monitor);
335
336 /* Clear any monitored devices by this Adv Monitor */
337 msft_monitor_device_del(hdev, handle_data->mgmt_handle,
338 NULL, 0, false);
339 }
340
341 list_del(&handle_data->list);
342 kfree(handle_data);
343 }
344
345 /* If remove all monitors is required, we need to continue the process
346 * here because the earlier it was paused when waiting for the
347 * response from controller.
348 */
349 if (msft->pending_remove_handle == 0) {
350 pending = hci_remove_all_adv_monitor(hdev, &err);
351 if (pending) {
352 hci_dev_unlock(hdev);
353 return;
354 }
355
356 if (err)
357 status = HCI_ERROR_UNSPECIFIED;
358 }
359
360 hci_dev_unlock(hdev);
361
362done:
363 if (!msft->suspending)
364 hci_remove_adv_monitor_complete(hdev, status);
365}
366
367static int msft_remove_monitor_sync(struct hci_dev *hdev,
368 struct adv_monitor *monitor)
369{
370 struct msft_cp_le_cancel_monitor_advertisement cp;
371 struct msft_monitor_advertisement_handle_data *handle_data;
372 struct sk_buff *skb;
373 u8 status;
374
375 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
376
377 /* If no matched handle, just remove without telling controller */
378 if (!handle_data)
379 return -ENOENT;
380
381 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
382 cp.handle = handle_data->msft_handle;
383
384 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
385 HCI_CMD_TIMEOUT);
386 if (IS_ERR(skb))
387 return PTR_ERR(skb);
388
389 status = skb->data[0];
390 skb_pull(skb, 1);
391
392 msft_le_cancel_monitor_advertisement_cb(hdev, status, hdev->msft_opcode,
393 skb);
394
395 return status;
396}
397
398/* This function requires the caller holds hci_req_sync_lock */
399int msft_suspend_sync(struct hci_dev *hdev)
400{
401 struct msft_data *msft = hdev->msft_data;
402 struct adv_monitor *monitor;
403 int handle = 0;
404
405 if (!msft || !msft_monitor_supported(hdev))
406 return 0;
407
408 msft->suspending = true;
409
410 while (1) {
411 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
412 if (!monitor)
413 break;
414
415 msft_remove_monitor_sync(hdev, monitor);
416
417 handle++;
418 }
419
420 /* All monitors have been removed */
421 msft->suspending = false;
422
423 return 0;
424}
425
426static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
427{
428 struct adv_rssi_thresholds *r = &monitor->rssi;
429
430 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
431 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
432 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
433 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
434 return false;
435
436 /* High_threshold_timeout is not supported,
437 * once high_threshold is reached, events are immediately reported.
438 */
439 if (r->high_threshold_timeout != 0)
440 return false;
441
442 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
443 return false;
444
445 /* Sampling period from 0x00 to 0xFF are all allowed */
446 return true;
447}
448
449static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
450{
451 return msft_monitor_rssi_valid(monitor);
452 /* No additional check needed for pattern-based monitor */
453}
454
455static int msft_add_monitor_sync(struct hci_dev *hdev,
456 struct adv_monitor *monitor)
457{
458 struct msft_cp_le_monitor_advertisement *cp;
459 struct msft_le_monitor_advertisement_pattern_data *pattern_data;
460 struct msft_le_monitor_advertisement_pattern *pattern;
461 struct adv_pattern *entry;
462 size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
463 ptrdiff_t offset = 0;
464 u8 pattern_count = 0;
465 struct sk_buff *skb;
466 u8 status;
467
468 if (!msft_monitor_pattern_valid(monitor))
469 return -EINVAL;
470
471 list_for_each_entry(entry, &monitor->patterns, list) {
472 pattern_count++;
473 total_size += sizeof(*pattern) + entry->length;
474 }
475
476 cp = kmalloc(total_size, GFP_KERNEL);
477 if (!cp)
478 return -ENOMEM;
479
480 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
481 cp->rssi_high = monitor->rssi.high_threshold;
482 cp->rssi_low = monitor->rssi.low_threshold;
483 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
484 cp->rssi_sampling_period = monitor->rssi.sampling_period;
485
486 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
487
488 pattern_data = (void *)cp->data;
489 pattern_data->count = pattern_count;
490
491 list_for_each_entry(entry, &monitor->patterns, list) {
492 pattern = (void *)(pattern_data->data + offset);
493 /* the length also includes data_type and offset */
494 pattern->length = entry->length + 2;
495 pattern->data_type = entry->ad_type;
496 pattern->start_byte = entry->offset;
497 memcpy(pattern->pattern, entry->value, entry->length);
498 offset += sizeof(*pattern) + entry->length;
499 }
500
501 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
502 HCI_CMD_TIMEOUT);
503 kfree(cp);
504
505 if (IS_ERR(skb))
506 return PTR_ERR(skb);
507
508 status = skb->data[0];
509 skb_pull(skb, 1);
510
511 msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb);
512
513 return status;
514}
515
516/* This function requires the caller holds hci_req_sync_lock */
517int msft_resume_sync(struct hci_dev *hdev)
518{
519 struct msft_data *msft = hdev->msft_data;
520 struct adv_monitor *monitor;
521 int handle = 0;
522
523 if (!msft || !msft_monitor_supported(hdev))
524 return 0;
525
526 hci_dev_lock(hdev);
527
528 /* Clear already tracked devices on resume. Once the monitors are
529 * reregistered, devices in range will be found again after resume.
530 */
531 hdev->advmon_pend_notify = false;
532 msft_monitor_device_del(hdev, 0, NULL, 0, true);
533
534 hci_dev_unlock(hdev);
535
536 msft->resuming = true;
537
538 while (1) {
539 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
540 if (!monitor)
541 break;
542
543 msft_add_monitor_sync(hdev, monitor);
544
545 handle++;
546 }
547
548 /* All monitors have been resumed */
549 msft->resuming = false;
550
551 return 0;
552}
553
554void msft_do_open(struct hci_dev *hdev)
555{
556 struct msft_data *msft = hdev->msft_data;
557
558 if (hdev->msft_opcode == HCI_OP_NOP)
559 return;
560
561 if (!msft) {
562 bt_dev_err(hdev, "MSFT extension not registered");
563 return;
564 }
565
566 bt_dev_dbg(hdev, "Initialize MSFT extension");
567
568 /* Reset existing MSFT data before re-reading */
569 kfree(msft->evt_prefix);
570 msft->evt_prefix = NULL;
571 msft->evt_prefix_len = 0;
572 msft->features = 0;
573
574 if (!read_supported_features(hdev, msft)) {
575 hdev->msft_data = NULL;
576 kfree(msft);
577 return;
578 }
579
580 if (msft_monitor_supported(hdev)) {
581 msft->resuming = true;
582 msft_set_filter_enable(hdev, true);
583 /* Monitors get removed on power off, so we need to explicitly
584 * tell the controller to re-monitor.
585 */
586 reregister_monitor(hdev, 0);
587 }
588}
589
590void msft_do_close(struct hci_dev *hdev)
591{
592 struct msft_data *msft = hdev->msft_data;
593 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
594 struct adv_monitor *monitor;
595
596 if (!msft)
597 return;
598
599 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
600
601 /* The controller will silently remove all monitors on power off.
602 * Therefore, remove handle_data mapping and reset monitor state.
603 */
604 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
605 monitor = idr_find(&hdev->adv_monitors_idr,
606 handle_data->mgmt_handle);
607
608 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
609 monitor->state = ADV_MONITOR_STATE_REGISTERED;
610
611 list_del(&handle_data->list);
612 kfree(handle_data);
613 }
614
615 hci_dev_lock(hdev);
616
617 /* Clear any devices that are being monitored and notify device lost */
618 hdev->advmon_pend_notify = false;
619 msft_monitor_device_del(hdev, 0, NULL, 0, true);
620
621 hci_dev_unlock(hdev);
622}
623
624void msft_register(struct hci_dev *hdev)
625{
626 struct msft_data *msft = NULL;
627
628 bt_dev_dbg(hdev, "Register MSFT extension");
629
630 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
631 if (!msft) {
632 bt_dev_err(hdev, "Failed to register MSFT extension");
633 return;
634 }
635
636 INIT_LIST_HEAD(&msft->handle_map);
637 hdev->msft_data = msft;
638}
639
640void msft_unregister(struct hci_dev *hdev)
641{
642 struct msft_data *msft = hdev->msft_data;
643
644 if (!msft)
645 return;
646
647 bt_dev_dbg(hdev, "Unregister MSFT extension");
648
649 hdev->msft_data = NULL;
650
651 kfree(msft->evt_prefix);
652 kfree(msft);
653}
654
655/* This function requires the caller holds hdev->lock */
656static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
657 __u8 addr_type, __u16 mgmt_handle)
658{
659 struct monitored_device *dev;
660
661 dev = kmalloc(sizeof(*dev), GFP_KERNEL);
662 if (!dev) {
663 bt_dev_err(hdev, "MSFT vendor event %u: no memory",
664 MSFT_EV_LE_MONITOR_DEVICE);
665 return;
666 }
667
668 bacpy(&dev->bdaddr, bdaddr);
669 dev->addr_type = addr_type;
670 dev->handle = mgmt_handle;
671 dev->notified = false;
672
673 INIT_LIST_HEAD(&dev->list);
674 list_add(&dev->list, &hdev->monitored_devices);
675 hdev->advmon_pend_notify = true;
676}
677
678/* This function requires the caller holds hdev->lock */
679static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
680 __u8 addr_type, __u16 mgmt_handle)
681{
682 if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
683 true)) {
684 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
685 MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
686 }
687}
688
689static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
690 u8 ev, size_t len)
691{
692 void *data;
693
694 data = skb_pull_data(skb, len);
695 if (!data)
696 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
697
698 return data;
699}
700
701/* This function requires the caller holds hdev->lock */
702static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
703{
704 struct msft_ev_le_monitor_device *ev;
705 struct msft_monitor_advertisement_handle_data *handle_data;
706 u8 addr_type;
707
708 ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
709 if (!ev)
710 return;
711
712 bt_dev_dbg(hdev,
713 "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
714 MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
715 ev->monitor_state, &ev->bdaddr);
716
717 handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
718 if (!handle_data)
719 return;
720
721 switch (ev->addr_type) {
722 case ADDR_LE_DEV_PUBLIC:
723 addr_type = BDADDR_LE_PUBLIC;
724 break;
725
726 case ADDR_LE_DEV_RANDOM:
727 addr_type = BDADDR_LE_RANDOM;
728 break;
729
730 default:
731 bt_dev_err(hdev,
732 "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
733 MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
734 return;
735 }
736
737 if (ev->monitor_state)
738 msft_device_found(hdev, &ev->bdaddr, addr_type,
739 handle_data->mgmt_handle);
740 else
741 msft_device_lost(hdev, &ev->bdaddr, addr_type,
742 handle_data->mgmt_handle);
743}
744
745void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
746{
747 struct msft_data *msft = hdev->msft_data;
748 u8 *evt_prefix;
749 u8 *evt;
750
751 if (!msft)
752 return;
753
754 /* When the extension has defined an event prefix, check that it
755 * matches, and otherwise just return.
756 */
757 if (msft->evt_prefix_len > 0) {
758 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
759 if (!evt_prefix)
760 return;
761
762 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
763 return;
764 }
765
766 /* Every event starts at least with an event code and the rest of
767 * the data is variable and depends on the event code.
768 */
769 if (skb->len < 1)
770 return;
771
772 evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
773 if (!evt)
774 return;
775
776 hci_dev_lock(hdev);
777
778 switch (*evt) {
779 case MSFT_EV_LE_MONITOR_DEVICE:
780 msft_monitor_device_evt(hdev, skb);
781 break;
782
783 default:
784 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
785 break;
786 }
787
788 hci_dev_unlock(hdev);
789}
790
791__u64 msft_get_features(struct hci_dev *hdev)
792{
793 struct msft_data *msft = hdev->msft_data;
794
795 return msft ? msft->features : 0;
796}
797
798static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
799 u8 status, u16 opcode,
800 struct sk_buff *skb)
801{
802 struct msft_cp_le_set_advertisement_filter_enable *cp;
803 struct msft_rp_le_set_advertisement_filter_enable *rp;
804 struct msft_data *msft = hdev->msft_data;
805
806 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
807 if (skb->len < sizeof(*rp))
808 return;
809
810 /* Error 0x0C would be returned if the filter enabled status is
811 * already set to whatever we were trying to set.
812 * Although the default state should be disabled, some controller set
813 * the initial value to enabled. Because there is no way to know the
814 * actual initial value before sending this command, here we also treat
815 * error 0x0C as success.
816 */
817 if (status != 0x00 && status != 0x0C)
818 return;
819
820 hci_dev_lock(hdev);
821
822 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
823 msft->filter_enabled = cp->enable;
824
825 if (status == 0x0C)
826 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
827 cp->enable ? "on" : "off");
828
829 hci_dev_unlock(hdev);
830}
831
832/* This function requires the caller holds hdev->lock */
833static int __msft_add_monitor_pattern(struct hci_dev *hdev,
834 struct adv_monitor *monitor)
835{
836 struct msft_cp_le_monitor_advertisement *cp;
837 struct msft_le_monitor_advertisement_pattern_data *pattern_data;
838 struct msft_le_monitor_advertisement_pattern *pattern;
839 struct adv_pattern *entry;
840 struct hci_request req;
841 struct msft_data *msft = hdev->msft_data;
842 size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
843 ptrdiff_t offset = 0;
844 u8 pattern_count = 0;
845 int err = 0;
846
847 if (!msft_monitor_pattern_valid(monitor))
848 return -EINVAL;
849
850 list_for_each_entry(entry, &monitor->patterns, list) {
851 pattern_count++;
852 total_size += sizeof(*pattern) + entry->length;
853 }
854
855 cp = kmalloc(total_size, GFP_KERNEL);
856 if (!cp)
857 return -ENOMEM;
858
859 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
860 cp->rssi_high = monitor->rssi.high_threshold;
861 cp->rssi_low = monitor->rssi.low_threshold;
862 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
863 cp->rssi_sampling_period = monitor->rssi.sampling_period;
864
865 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
866
867 pattern_data = (void *)cp->data;
868 pattern_data->count = pattern_count;
869
870 list_for_each_entry(entry, &monitor->patterns, list) {
871 pattern = (void *)(pattern_data->data + offset);
872 /* the length also includes data_type and offset */
873 pattern->length = entry->length + 2;
874 pattern->data_type = entry->ad_type;
875 pattern->start_byte = entry->offset;
876 memcpy(pattern->pattern, entry->value, entry->length);
877 offset += sizeof(*pattern) + entry->length;
878 }
879
880 hci_req_init(&req, hdev);
881 hci_req_add(&req, hdev->msft_opcode, total_size, cp);
882 err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
883 kfree(cp);
884
885 if (!err)
886 msft->pending_add_handle = monitor->handle;
887
888 return err;
889}
890
891/* This function requires the caller holds hdev->lock */
892int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
893{
894 struct msft_data *msft = hdev->msft_data;
895
896 if (!msft)
897 return -EOPNOTSUPP;
898
899 if (msft->resuming || msft->suspending)
900 return -EBUSY;
901
902 return __msft_add_monitor_pattern(hdev, monitor);
903}
904
905/* This function requires the caller holds hdev->lock */
906static int __msft_remove_monitor(struct hci_dev *hdev,
907 struct adv_monitor *monitor, u16 handle)
908{
909 struct msft_cp_le_cancel_monitor_advertisement cp;
910 struct msft_monitor_advertisement_handle_data *handle_data;
911 struct hci_request req;
912 struct msft_data *msft = hdev->msft_data;
913 int err = 0;
914
915 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
916
917 /* If no matched handle, just remove without telling controller */
918 if (!handle_data)
919 return -ENOENT;
920
921 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
922 cp.handle = handle_data->msft_handle;
923
924 hci_req_init(&req, hdev);
925 hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
926 err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
927
928 if (!err)
929 msft->pending_remove_handle = handle;
930
931 return err;
932}
933
934/* This function requires the caller holds hdev->lock */
935int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
936 u16 handle)
937{
938 struct msft_data *msft = hdev->msft_data;
939
940 if (!msft)
941 return -EOPNOTSUPP;
942
943 if (msft->resuming || msft->suspending)
944 return -EBUSY;
945
946 return __msft_remove_monitor(hdev, monitor, handle);
947}
948
949void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
950{
951 struct hci_dev *hdev = req->hdev;
952 struct msft_cp_le_set_advertisement_filter_enable cp;
953
954 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
955 cp.enable = enable;
956
957 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
958}
959
960int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
961{
962 struct hci_request req;
963 struct msft_data *msft = hdev->msft_data;
964 int err;
965
966 if (!msft)
967 return -EOPNOTSUPP;
968
969 hci_req_init(&req, hdev);
970 msft_req_add_set_filter_enable(&req, enable);
971 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
972
973 return err;
974}
975
976bool msft_curve_validity(struct hci_dev *hdev)
977{
978 return hdev->msft_curve_validity;
979}