1 // SPDX-License-Identifier: GPL-2.0-only
3 * Copyright (C) 2020 Google Corporation
6 #include <net/bluetooth/bluetooth.h>
7 #include <net/bluetooth/hci_core.h>
8 #include <net/bluetooth/mgmt.h>
10 #include "hci_request.h"
11 #include "mgmt_util.h"
14 #define MSFT_RSSI_THRESHOLD_VALUE_MIN -127
15 #define MSFT_RSSI_THRESHOLD_VALUE_MAX 20
16 #define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C
18 #define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
19 struct msft_cp_read_supported_features
{
23 struct msft_rp_read_supported_features
{
31 #define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03
32 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01
33 struct msft_le_monitor_advertisement_pattern
{
40 struct msft_le_monitor_advertisement_pattern_data
{
45 struct msft_cp_le_monitor_advertisement
{
49 __u8 rssi_low_interval
;
50 __u8 rssi_sampling_period
;
55 struct msft_rp_le_monitor_advertisement
{
61 #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04
62 struct msft_cp_le_cancel_monitor_advertisement
{
67 struct msft_rp_le_cancel_monitor_advertisement
{
72 #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05
73 struct msft_cp_le_set_advertisement_filter_enable
{
78 struct msft_rp_le_set_advertisement_filter_enable
{
83 #define MSFT_EV_LE_MONITOR_DEVICE 0x02
84 struct msft_ev_le_monitor_device
{
91 struct msft_monitor_advertisement_handle_data
{
96 __u8 rssi_low_interval
;
97 __u8 rssi_sampling_period
;
99 struct list_head list
;
102 enum monitor_addr_filter_state
{
109 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR 0x04
110 struct msft_monitor_addr_filter_data
{
112 __u8 pattern_handle
; /* address filters pertain to */
117 __u8 rssi_low_interval
;
118 __u8 rssi_sampling_period
;
121 struct list_head list
;
128 struct list_head handle_map
;
129 struct list_head address_filters
;
133 /* To synchronize add/remove address filter and monitor device event.*/
134 struct mutex filter_lock
;
137 bool msft_monitor_supported(struct hci_dev
*hdev
)
139 return !!(msft_get_features(hdev
) & MSFT_FEATURE_MASK_LE_ADV_MONITOR
);
142 static bool read_supported_features(struct hci_dev
*hdev
,
143 struct msft_data
*msft
)
145 struct msft_cp_read_supported_features cp
;
146 struct msft_rp_read_supported_features
*rp
;
149 cp
.sub_opcode
= MSFT_OP_READ_SUPPORTED_FEATURES
;
151 skb
= __hci_cmd_sync(hdev
, hdev
->msft_opcode
, sizeof(cp
), &cp
,
154 bt_dev_err(hdev
, "Failed to read MSFT supported features (%ld)",
159 if (skb
->len
< sizeof(*rp
)) {
160 bt_dev_err(hdev
, "MSFT supported features length mismatch");
164 rp
= (struct msft_rp_read_supported_features
*)skb
->data
;
166 if (rp
->sub_opcode
!= MSFT_OP_READ_SUPPORTED_FEATURES
)
169 if (rp
->evt_prefix_len
> 0) {
170 msft
->evt_prefix
= kmemdup(rp
->evt_prefix
, rp
->evt_prefix_len
,
172 if (!msft
->evt_prefix
)
176 msft
->evt_prefix_len
= rp
->evt_prefix_len
;
177 msft
->features
= __le64_to_cpu(rp
->features
);
179 if (msft
->features
& MSFT_FEATURE_MASK_CURVE_VALIDITY
)
180 hdev
->msft_curve_validity
= true;
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
194 static struct msft_monitor_advertisement_handle_data
*msft_find_handle_data
195 (struct hci_dev
*hdev
, u16 handle
, bool is_mgmt
)
197 struct msft_monitor_advertisement_handle_data
*entry
;
198 struct msft_data
*msft
= hdev
->msft_data
;
200 list_for_each_entry(entry
, &msft
->handle_map
, list
) {
201 if (is_mgmt
&& entry
->mgmt_handle
== handle
)
203 if (!is_mgmt
&& entry
->msft_handle
== handle
)
210 /* This function requires the caller holds msft->filter_lock */
211 static struct msft_monitor_addr_filter_data
*msft_find_address_data
212 (struct hci_dev
*hdev
, u8 addr_type
, bdaddr_t
*addr
,
215 struct msft_monitor_addr_filter_data
*entry
;
216 struct msft_data
*msft
= hdev
->msft_data
;
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
))
228 /* This function requires the caller holds hdev->lock */
229 static int msft_monitor_device_del(struct hci_dev
*hdev
, __u16 mgmt_handle
,
230 bdaddr_t
*bdaddr
, __u8 addr_type
,
233 struct monitored_device
*dev
, *tmp
;
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
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
,
250 list_del(&dev
->list
);
259 static int msft_le_monitor_advertisement_cb(struct hci_dev
*hdev
, u16 opcode
,
260 struct adv_monitor
*monitor
,
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
;
270 rp
= (struct msft_rp_le_monitor_advertisement
*)skb
->data
;
271 if (skb
->len
< sizeof(*rp
)) {
272 status
= HCI_ERROR_UNSPECIFIED
;
280 handle_data
= kmalloc(sizeof(*handle_data
), GFP_KERNEL
);
282 status
= HCI_ERROR_UNSPECIFIED
;
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
);
292 monitor
->state
= ADV_MONITOR_STATE_OFFLOADED
;
296 hci_free_adv_monitor(hdev
, monitor
);
298 hci_dev_unlock(hdev
);
303 /* This function requires the caller holds hci_req_sync_lock */
304 static void msft_remove_addr_filters_sync(struct hci_dev
*hdev
, u8 handle
)
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
;
312 INIT_LIST_HEAD(&head
);
314 /* Cancel all corresponding address monitors */
315 mutex_lock(&msft
->filter_lock
);
317 list_for_each_entry_safe(address_filter
, n
, &msft
->address_filters
,
319 if (address_filter
->pattern_handle
!= handle
)
322 list_del(&address_filter
->list
);
324 /* Keep the address filter and let
325 * msft_add_address_filter_sync() remove and free the address
328 if (address_filter
->state
== AF_STATE_ADDING
) {
329 address_filter
->state
= AF_STATE_REMOVING
;
333 /* Keep the address filter and let
334 * msft_cancel_address_filter_sync() remove and free the address
337 if (address_filter
->state
== AF_STATE_REMOVING
)
340 list_add_tail(&address_filter
->list
, &head
);
343 mutex_unlock(&msft
->filter_lock
);
345 list_for_each_entry_safe(address_filter
, n
, &head
, list
) {
346 list_del(&address_filter
->list
);
348 cp
.sub_opcode
= MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT
;
349 cp
.handle
= address_filter
->msft_handle
;
351 skb
= __hci_cmd_sync(hdev
, hdev
->msft_opcode
, sizeof(cp
), &cp
,
354 kfree(address_filter
);
360 bt_dev_dbg(hdev
, "MSFT: Canceled device %pMR address filter",
361 &address_filter
->bdaddr
);
363 kfree(address_filter
);
367 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev
*hdev
,
369 struct adv_monitor
*monitor
,
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
;
378 rp
= (struct msft_rp_le_cancel_monitor_advertisement
*)skb
->data
;
379 if (skb
->len
< sizeof(*rp
)) {
380 status
= HCI_ERROR_UNSPECIFIED
;
390 handle_data
= msft_find_handle_data(hdev
, monitor
->handle
, true);
393 if (monitor
->state
== ADV_MONITOR_STATE_OFFLOADED
)
394 monitor
->state
= ADV_MONITOR_STATE_REGISTERED
;
396 /* Do not free the monitor if it is being removed due to
397 * suspend. It will be re-monitored on resume.
399 if (!msft
->suspending
) {
400 hci_free_adv_monitor(hdev
, monitor
);
402 /* Clear any monitored devices by this Adv Monitor */
403 msft_monitor_device_del(hdev
, handle_data
->mgmt_handle
,
407 msft_handle
= handle_data
->msft_handle
;
409 list_del(&handle_data
->list
);
412 hci_dev_unlock(hdev
);
414 msft_remove_addr_filters_sync(hdev
, msft_handle
);
416 hci_dev_unlock(hdev
);
423 /* This function requires the caller holds hci_req_sync_lock */
424 static int msft_remove_monitor_sync(struct hci_dev
*hdev
,
425 struct adv_monitor
*monitor
)
427 struct msft_cp_le_cancel_monitor_advertisement cp
;
428 struct msft_monitor_advertisement_handle_data
*handle_data
;
431 handle_data
= msft_find_handle_data(hdev
, monitor
->handle
, true);
433 /* If no matched handle, just remove without telling controller */
437 cp
.sub_opcode
= MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT
;
438 cp
.handle
= handle_data
->msft_handle
;
440 skb
= __hci_cmd_sync(hdev
, hdev
->msft_opcode
, sizeof(cp
), &cp
,
445 return msft_le_cancel_monitor_advertisement_cb(hdev
, hdev
->msft_opcode
,
449 /* This function requires the caller holds hci_req_sync_lock */
450 int msft_suspend_sync(struct hci_dev
*hdev
)
452 struct msft_data
*msft
= hdev
->msft_data
;
453 struct adv_monitor
*monitor
;
456 if (!msft
|| !msft_monitor_supported(hdev
))
459 msft
->suspending
= true;
462 monitor
= idr_get_next(&hdev
->adv_monitors_idr
, &handle
);
466 msft_remove_monitor_sync(hdev
, monitor
);
471 /* All monitors have been removed */
472 msft
->suspending
= false;
477 static bool msft_monitor_rssi_valid(struct adv_monitor
*monitor
)
479 struct adv_rssi_thresholds
*r
= &monitor
->rssi
;
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
)
487 /* High_threshold_timeout is not supported,
488 * once high_threshold is reached, events are immediately reported.
490 if (r
->high_threshold_timeout
!= 0)
493 if (r
->low_threshold_timeout
> MSFT_RSSI_LOW_TIMEOUT_MAX
)
496 /* Sampling period from 0x00 to 0xFF are all allowed */
500 static bool msft_monitor_pattern_valid(struct adv_monitor
*monitor
)
502 return msft_monitor_rssi_valid(monitor
);
503 /* No additional check needed for pattern-based monitor */
506 static int msft_add_monitor_sync(struct hci_dev
*hdev
,
507 struct adv_monitor
*monitor
)
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;
520 if (!msft_monitor_pattern_valid(monitor
))
523 list_for_each_entry(entry
, &monitor
->patterns
, list
) {
525 total_size
+= sizeof(*pattern
) + entry
->length
;
528 cp
= kmalloc(total_size
, GFP_KERNEL
);
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
;
538 cp
->cond_type
= MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN
;
540 pattern_data
= (void *)cp
->data
;
541 pattern_data
->count
= pattern_count
;
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
;
553 skb
= __hci_cmd_sync(hdev
, hdev
->msft_opcode
, total_size
, cp
,
561 err
= msft_le_monitor_advertisement_cb(hdev
, hdev
->msft_opcode
,
566 handle_data
= msft_find_handle_data(hdev
, monitor
->handle
, true);
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
;
582 /* This function requires the caller holds hci_req_sync_lock */
583 static void reregister_monitor(struct hci_dev
*hdev
)
585 struct adv_monitor
*monitor
;
586 struct msft_data
*msft
= hdev
->msft_data
;
592 msft
->resuming
= true;
595 monitor
= idr_get_next(&hdev
->adv_monitors_idr
, &handle
);
599 msft_add_monitor_sync(hdev
, monitor
);
604 /* All monitors have been reregistered */
605 msft
->resuming
= false;
608 /* This function requires the caller holds hci_req_sync_lock */
609 int msft_resume_sync(struct hci_dev
*hdev
)
611 struct msft_data
*msft
= hdev
->msft_data
;
613 if (!msft
|| !msft_monitor_supported(hdev
))
618 /* Clear already tracked devices on resume. Once the monitors are
619 * reregistered, devices in range will be found again after resume.
621 hdev
->advmon_pend_notify
= false;
622 msft_monitor_device_del(hdev
, 0, NULL
, 0, true);
624 hci_dev_unlock(hdev
);
626 reregister_monitor(hdev
);
631 /* This function requires the caller holds hci_req_sync_lock */
632 void msft_do_open(struct hci_dev
*hdev
)
634 struct msft_data
*msft
= hdev
->msft_data
;
636 if (hdev
->msft_opcode
== HCI_OP_NOP
)
640 bt_dev_err(hdev
, "MSFT extension not registered");
644 bt_dev_dbg(hdev
, "Initialize MSFT extension");
646 /* Reset existing MSFT data before re-reading */
647 kfree(msft
->evt_prefix
);
648 msft
->evt_prefix
= NULL
;
649 msft
->evt_prefix_len
= 0;
652 if (!read_supported_features(hdev
, msft
)) {
653 hdev
->msft_data
= NULL
;
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.
664 reregister_monitor(hdev
);
668 void msft_do_close(struct hci_dev
*hdev
)
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
;
678 bt_dev_dbg(hdev
, "Cleanup of MSFT extension");
680 /* The controller will silently remove all monitors on power off.
681 * Therefore, remove handle_data mapping and reset monitor state.
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
);
687 if (monitor
&& monitor
->state
== ADV_MONITOR_STATE_OFFLOADED
)
688 monitor
->state
= ADV_MONITOR_STATE_REGISTERED
;
690 list_del(&handle_data
->list
);
694 mutex_lock(&msft
->filter_lock
);
695 list_for_each_entry_safe(address_filter
, n
, &msft
->address_filters
,
697 list_del(&address_filter
->list
);
698 kfree(address_filter
);
700 mutex_unlock(&msft
->filter_lock
);
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);
708 hci_dev_unlock(hdev
);
711 static int msft_cancel_address_filter_sync(struct hci_dev
*hdev
, void *data
)
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
;
720 bt_dev_err(hdev
, "MSFT: msft data is freed");
724 /* The address filter has been removed by hci dev close */
725 if (!test_bit(HCI_UP
, &hdev
->flags
))
728 mutex_lock(&msft
->filter_lock
);
729 list_del(&address_filter
->list
);
730 mutex_unlock(&msft
->filter_lock
);
732 cp
.sub_opcode
= MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT
;
733 cp
.handle
= address_filter
->msft_handle
;
735 skb
= __hci_cmd_sync(hdev
, hdev
->msft_opcode
, sizeof(cp
), &cp
,
738 bt_dev_err(hdev
, "MSFT: Failed to cancel address (%pMR) filter",
739 &address_filter
->bdaddr
);
745 bt_dev_dbg(hdev
, "MSFT: Canceled device %pMR address filter",
746 &address_filter
->bdaddr
);
749 kfree(address_filter
);
754 void msft_register(struct hci_dev
*hdev
)
756 struct msft_data
*msft
= NULL
;
758 bt_dev_dbg(hdev
, "Register MSFT extension");
760 msft
= kzalloc(sizeof(*msft
), GFP_KERNEL
);
762 bt_dev_err(hdev
, "Failed to register MSFT extension");
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
);
772 void msft_release(struct hci_dev
*hdev
)
774 struct msft_data
*msft
= hdev
->msft_data
;
779 bt_dev_dbg(hdev
, "Unregister MSFT extension");
781 hdev
->msft_data
= NULL
;
783 kfree(msft
->evt_prefix
);
784 mutex_destroy(&msft
->filter_lock
);
788 /* This function requires the caller holds hdev->lock */
789 static void msft_device_found(struct hci_dev
*hdev
, bdaddr_t
*bdaddr
,
790 __u8 addr_type
, __u16 mgmt_handle
)
792 struct monitored_device
*dev
;
794 dev
= kmalloc(sizeof(*dev
), GFP_KERNEL
);
796 bt_dev_err(hdev
, "MSFT vendor event %u: no memory",
797 MSFT_EV_LE_MONITOR_DEVICE
);
801 bacpy(&dev
->bdaddr
, bdaddr
);
802 dev
->addr_type
= addr_type
;
803 dev
->handle
= mgmt_handle
;
804 dev
->notified
= false;
806 INIT_LIST_HEAD(&dev
->list
);
807 list_add(&dev
->list
, &hdev
->monitored_devices
);
808 hdev
->advmon_pend_notify
= true;
811 /* This function requires the caller holds hdev->lock */
812 static void msft_device_lost(struct hci_dev
*hdev
, bdaddr_t
*bdaddr
,
813 __u8 addr_type
, __u16 mgmt_handle
)
815 if (!msft_monitor_device_del(hdev
, mgmt_handle
, bdaddr
, addr_type
,
817 bt_dev_err(hdev
, "MSFT vendor event %u: dev %pMR not in list",
818 MSFT_EV_LE_MONITOR_DEVICE
, bdaddr
);
822 static void *msft_skb_pull(struct hci_dev
*hdev
, struct sk_buff
*skb
,
827 data
= skb_pull_data(skb
, len
);
829 bt_dev_err(hdev
, "Malformed MSFT vendor event: 0x%02x", ev
);
834 static int msft_add_address_filter_sync(struct hci_dev
*hdev
, void *data
)
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
;
845 bt_dev_err(hdev
, "MSFT: msft data is freed");
849 /* The address filter has been removed by hci dev close */
850 if (!test_bit(HCI_UP
, &hdev
->flags
))
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.
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
);
870 sizeof(address_filter
->addr_type
) +
871 sizeof(address_filter
->bdaddr
);
872 cp
= kzalloc(size
, GFP_KERNEL
);
874 bt_dev_err(hdev
, "MSFT: Alloc cmd param err");
879 cp
->sub_opcode
= MSFT_OP_LE_MONITOR_ADVERTISEMENT
;
880 cp
->rssi_high
= address_filter
->rssi_high
;
881 cp
->rssi_low
= address_filter
->rssi_low
;
882 cp
->rssi_low_interval
= address_filter
->rssi_low_interval
;
883 cp
->rssi_sampling_period
= address_filter
->rssi_sampling_period
;
884 cp
->cond_type
= MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR
;
885 cp
->data
[0] = address_filter
->addr_type
;
886 memcpy(&cp
->data
[1], &address_filter
->bdaddr
,
887 sizeof(address_filter
->bdaddr
));
889 skb
= __hci_cmd_sync(hdev
, hdev
->msft_opcode
, size
, cp
,
894 bt_dev_err(hdev
, "Failed to enable address %pMR filter",
895 &address_filter
->bdaddr
);
901 rp
= skb_pull_data(skb
, sizeof(*rp
));
902 if (!rp
|| rp
->sub_opcode
!= MSFT_OP_LE_MONITOR_ADVERTISEMENT
||
907 mutex_lock(&msft
->filter_lock
);
910 bt_dev_warn(hdev
, "MSFT: Remove address (%pMR) filter",
911 &address_filter
->bdaddr
);
912 list_del(&address_filter
->list
);
913 kfree(address_filter
);
915 address_filter
->state
= AF_STATE_ADDED
;
916 address_filter
->msft_handle
= rp
->handle
;
917 bt_dev_dbg(hdev
, "MSFT: Address %pMR filter enabled",
918 &address_filter
->bdaddr
);
920 mutex_unlock(&msft
->filter_lock
);
927 /* This function requires the caller holds msft->filter_lock */
928 static struct msft_monitor_addr_filter_data
*msft_add_address_filter
929 (struct hci_dev
*hdev
, u8 addr_type
, bdaddr_t
*bdaddr
,
930 struct msft_monitor_advertisement_handle_data
*handle_data
)
932 struct msft_monitor_addr_filter_data
*address_filter
= NULL
;
933 struct msft_data
*msft
= hdev
->msft_data
;
936 address_filter
= kzalloc(sizeof(*address_filter
), GFP_KERNEL
);
940 address_filter
->state
= AF_STATE_ADDING
;
941 address_filter
->msft_handle
= 0xff;
942 address_filter
->pattern_handle
= handle_data
->msft_handle
;
943 address_filter
->mgmt_handle
= handle_data
->mgmt_handle
;
944 address_filter
->rssi_high
= handle_data
->rssi_high
;
945 address_filter
->rssi_low
= handle_data
->rssi_low
;
946 address_filter
->rssi_low_interval
= handle_data
->rssi_low_interval
;
947 address_filter
->rssi_sampling_period
= handle_data
->rssi_sampling_period
;
948 address_filter
->addr_type
= addr_type
;
949 bacpy(&address_filter
->bdaddr
, bdaddr
);
951 /* With the above AF_STATE_ADDING, duplicated address filter can be
952 * avoided when receiving monitor device event (found/lost) frequently
953 * for the same device.
955 list_add_tail(&address_filter
->list
, &msft
->address_filters
);
957 err
= hci_cmd_sync_queue(hdev
, msft_add_address_filter_sync
,
958 address_filter
, NULL
);
960 bt_dev_err(hdev
, "MSFT: Add address %pMR filter err", bdaddr
);
961 list_del(&address_filter
->list
);
962 kfree(address_filter
);
966 bt_dev_dbg(hdev
, "MSFT: Add device %pMR address filter",
967 &address_filter
->bdaddr
);
969 return address_filter
;
972 /* This function requires the caller holds hdev->lock */
973 static void msft_monitor_device_evt(struct hci_dev
*hdev
, struct sk_buff
*skb
)
975 struct msft_monitor_addr_filter_data
*n
, *address_filter
= NULL
;
976 struct msft_ev_le_monitor_device
*ev
;
977 struct msft_monitor_advertisement_handle_data
*handle_data
;
978 struct msft_data
*msft
= hdev
->msft_data
;
979 u16 mgmt_handle
= 0xffff;
982 ev
= msft_skb_pull(hdev
, skb
, MSFT_EV_LE_MONITOR_DEVICE
, sizeof(*ev
));
987 "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
988 MSFT_EV_LE_MONITOR_DEVICE
, ev
->monitor_handle
,
989 ev
->monitor_state
, &ev
->bdaddr
);
991 handle_data
= msft_find_handle_data(hdev
, ev
->monitor_handle
, false);
993 if (!test_bit(HCI_QUIRK_USE_MSFT_EXT_ADDRESS_FILTER
, &hdev
->quirks
)) {
996 mgmt_handle
= handle_data
->mgmt_handle
;
1001 /* Don't report any device found/lost event from pattern
1002 * monitors. Pattern monitor always has its address filters for
1006 address_filter
= msft_find_address_data(hdev
, ev
->addr_type
,
1008 handle_data
->msft_handle
);
1012 if (ev
->monitor_state
&& handle_data
->cond_type
==
1013 MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN
)
1014 msft_add_address_filter(hdev
, ev
->addr_type
,
1015 &ev
->bdaddr
, handle_data
);
1020 /* This device event is not from pattern monitor.
1021 * Report it if there is a corresponding address_filter for it.
1023 list_for_each_entry(n
, &msft
->address_filters
, list
) {
1024 if (n
->state
== AF_STATE_ADDED
&&
1025 n
->msft_handle
== ev
->monitor_handle
) {
1026 mgmt_handle
= n
->mgmt_handle
;
1032 if (!address_filter
) {
1033 bt_dev_warn(hdev
, "MSFT: Unexpected device event %pMR, %u, %u",
1034 &ev
->bdaddr
, ev
->monitor_handle
, ev
->monitor_state
);
1039 switch (ev
->addr_type
) {
1040 case ADDR_LE_DEV_PUBLIC
:
1041 addr_type
= BDADDR_LE_PUBLIC
;
1044 case ADDR_LE_DEV_RANDOM
:
1045 addr_type
= BDADDR_LE_RANDOM
;
1050 "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
1051 MSFT_EV_LE_MONITOR_DEVICE
, ev
->addr_type
);
1055 if (ev
->monitor_state
) {
1056 msft_device_found(hdev
, &ev
->bdaddr
, addr_type
, mgmt_handle
);
1058 if (address_filter
&& address_filter
->state
== AF_STATE_ADDED
) {
1059 address_filter
->state
= AF_STATE_REMOVING
;
1060 hci_cmd_sync_queue(hdev
,
1061 msft_cancel_address_filter_sync
,
1065 msft_device_lost(hdev
, &ev
->bdaddr
, addr_type
, mgmt_handle
);
1069 void msft_vendor_evt(struct hci_dev
*hdev
, void *data
, struct sk_buff
*skb
)
1071 struct msft_data
*msft
= hdev
->msft_data
;
1078 /* When the extension has defined an event prefix, check that it
1079 * matches, and otherwise just return.
1081 if (msft
->evt_prefix_len
> 0) {
1082 evt_prefix
= msft_skb_pull(hdev
, skb
, 0, msft
->evt_prefix_len
);
1086 if (memcmp(evt_prefix
, msft
->evt_prefix
, msft
->evt_prefix_len
))
1090 /* Every event starts at least with an event code and the rest of
1091 * the data is variable and depends on the event code.
1096 evt
= msft_skb_pull(hdev
, skb
, 0, sizeof(*evt
));
1103 case MSFT_EV_LE_MONITOR_DEVICE
:
1104 mutex_lock(&msft
->filter_lock
);
1105 msft_monitor_device_evt(hdev
, skb
);
1106 mutex_unlock(&msft
->filter_lock
);
1110 bt_dev_dbg(hdev
, "MSFT vendor event 0x%02x", *evt
);
1114 hci_dev_unlock(hdev
);
1117 __u64
msft_get_features(struct hci_dev
*hdev
)
1119 struct msft_data
*msft
= hdev
->msft_data
;
1121 return msft
? msft
->features
: 0;
1124 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev
*hdev
,
1128 struct msft_cp_le_set_advertisement_filter_enable
*cp
= user_data
;
1129 struct msft_data
*msft
= hdev
->msft_data
;
1131 /* Error 0x0C would be returned if the filter enabled status is
1132 * already set to whatever we were trying to set.
1133 * Although the default state should be disabled, some controller set
1134 * the initial value to enabled. Because there is no way to know the
1135 * actual initial value before sending this command, here we also treat
1136 * error 0x0C as success.
1138 if (status
!= 0x00 && status
!= 0x0C)
1143 msft
->filter_enabled
= cp
->enable
;
1146 bt_dev_warn(hdev
, "MSFT filter_enable is already %s",
1147 cp
->enable
? "on" : "off");
1149 hci_dev_unlock(hdev
);
1152 /* This function requires the caller holds hci_req_sync_lock */
1153 int msft_add_monitor_pattern(struct hci_dev
*hdev
, struct adv_monitor
*monitor
)
1155 struct msft_data
*msft
= hdev
->msft_data
;
1160 if (msft
->resuming
|| msft
->suspending
)
1163 return msft_add_monitor_sync(hdev
, monitor
);
1166 /* This function requires the caller holds hci_req_sync_lock */
1167 int msft_remove_monitor(struct hci_dev
*hdev
, struct adv_monitor
*monitor
)
1169 struct msft_data
*msft
= hdev
->msft_data
;
1174 if (msft
->resuming
|| msft
->suspending
)
1177 return msft_remove_monitor_sync(hdev
, monitor
);
1180 int msft_set_filter_enable(struct hci_dev
*hdev
, bool enable
)
1182 struct msft_cp_le_set_advertisement_filter_enable cp
;
1183 struct msft_data
*msft
= hdev
->msft_data
;
1189 cp
.sub_opcode
= MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE
;
1191 err
= __hci_cmd_sync_status(hdev
, hdev
->msft_opcode
, sizeof(cp
), &cp
,
1194 msft_le_set_advertisement_filter_enable_cb(hdev
, &cp
, err
);
1199 bool msft_curve_validity(struct hci_dev
*hdev
)
1201 return hdev
->msft_curve_validity
;