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;