net/bluetooth/msft.c

0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * Copyright (C) 2020 Google Corporation
0004  */
0005
0006 #include <net/bluetooth/bluetooth.h>
0007 #include <net/bluetooth/hci_core.h>
0008 #include <net/bluetooth/mgmt.h>
0009
0010 #include "hci_request.h"
0011 #include "mgmt_util.h"
0012 #include "msft.h"
0013
0014 #define MSFT_RSSI_THRESHOLD_VALUE_MIN       -127
0015 #define MSFT_RSSI_THRESHOLD_VALUE_MAX       20
0016 #define MSFT_RSSI_LOW_TIMEOUT_MAX       0x3C
0017
0018 #define MSFT_OP_READ_SUPPORTED_FEATURES     0x00
0019 struct msft_cp_read_supported_features {
0020     __u8   sub_opcode;
0021 } __packed;
0022
0023 struct msft_rp_read_supported_features {
0024     __u8   status;
0025     __u8   sub_opcode;
0026     __le64 features;
0027     __u8   evt_prefix_len;
0028     __u8   evt_prefix[];
0029 } __packed;
0030
0031 #define MSFT_OP_LE_MONITOR_ADVERTISEMENT    0x03
0032 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01
0033 struct msft_le_monitor_advertisement_pattern {
0034     __u8 length;
0035     __u8 data_type;
0036     __u8 start_byte;
0037     __u8 pattern[];
0038 };
0039
0040 struct msft_le_monitor_advertisement_pattern_data {
0041     __u8 count;
0042     __u8 data[];
0043 };
0044
0045 struct msft_cp_le_monitor_advertisement {
0046     __u8 sub_opcode;
0047     __s8 rssi_high;
0048     __s8 rssi_low;
0049     __u8 rssi_low_interval;
0050     __u8 rssi_sampling_period;
0051     __u8 cond_type;
0052     __u8 data[];
0053 } __packed;
0054
0055 struct msft_rp_le_monitor_advertisement {
0056     __u8 status;
0057     __u8 sub_opcode;
0058     __u8 handle;
0059 } __packed;
0060
0061 #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04
0062 struct msft_cp_le_cancel_monitor_advertisement {
0063     __u8 sub_opcode;
0064     __u8 handle;
0065 } __packed;
0066
0067 struct msft_rp_le_cancel_monitor_advertisement {
0068     __u8 status;
0069     __u8 sub_opcode;
0070 } __packed;
0071
0072 #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE  0x05
0073 struct msft_cp_le_set_advertisement_filter_enable {
0074     __u8 sub_opcode;
0075     __u8 enable;
0076 } __packed;
0077
0078 struct msft_rp_le_set_advertisement_filter_enable {
0079     __u8 status;
0080     __u8 sub_opcode;
0081 } __packed;
0082
0083 #define MSFT_EV_LE_MONITOR_DEVICE   0x02
0084 struct msft_ev_le_monitor_device {
0085     __u8     addr_type;
0086     bdaddr_t bdaddr;
0087     __u8     monitor_handle;
0088     __u8     monitor_state;
0089 } __packed;
0090
0091 struct msft_monitor_advertisement_handle_data {
0092     __u8  msft_handle;
0093     __u16 mgmt_handle;
0094     struct list_head list;
0095 };
0096
0097 struct msft_data {
0098     __u64 features;
0099     __u8  evt_prefix_len;
0100     __u8  *evt_prefix;
0101     struct list_head handle_map;
0102     __u8 resuming;
0103     __u8 suspending;
0104     __u8 filter_enabled;
0105 };
0106
0107 bool msft_monitor_supported(struct hci_dev *hdev)
0108 {
0109     return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
0110 }
0111
0112 static bool read_supported_features(struct hci_dev *hdev,
0113                     struct msft_data *msft)
0114 {
0115     struct msft_cp_read_supported_features cp;
0116     struct msft_rp_read_supported_features *rp;
0117     struct sk_buff *skb;
0118
0119     cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
0120
0121     skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
0122                  HCI_CMD_TIMEOUT);
0123     if (IS_ERR_OR_NULL(skb)) {
0124         if (!skb)
0125             skb = ERR_PTR(-EIO);
0126
0127         bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
0128                PTR_ERR(skb));
0129         return false;
0130     }
0131
0132     if (skb->len < sizeof(*rp)) {
0133         bt_dev_err(hdev, "MSFT supported features length mismatch");
0134         goto failed;
0135     }
0136
0137     rp = (struct msft_rp_read_supported_features *)skb->data;
0138
0139     if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
0140         goto failed;
0141
0142     if (rp->evt_prefix_len > 0) {
0143         msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
0144                        GFP_KERNEL);
0145         if (!msft->evt_prefix)
0146             goto failed;
0147     }
0148
0149     msft->evt_prefix_len = rp->evt_prefix_len;
0150     msft->features = __le64_to_cpu(rp->features);
0151
0152     if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
0153         hdev->msft_curve_validity = true;
0154
0155     kfree_skb(skb);
0156     return true;
0157
0158 failed:
0159     kfree_skb(skb);
0160     return false;
0161 }
0162
0163 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
0164  * is_mgmt = false matches the handle used by the msft controller.
0165  * This function requires the caller holds hdev->lock
0166  */
0167 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
0168                 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
0169 {
0170     struct msft_monitor_advertisement_handle_data *entry;
0171     struct msft_data *msft = hdev->msft_data;
0172
0173     list_for_each_entry(entry, &msft->handle_map, list) {
0174         if (is_mgmt && entry->mgmt_handle == handle)
0175             return entry;
0176         if (!is_mgmt && entry->msft_handle == handle)
0177             return entry;
0178     }
0179
0180     return NULL;
0181 }
0182
0183 /* This function requires the caller holds hdev->lock */
0184 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
0185                    bdaddr_t *bdaddr, __u8 addr_type,
0186                    bool notify)
0187 {
0188     struct monitored_device *dev, *tmp;
0189     int count = 0;
0190
0191     list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
0192         /* mgmt_handle == 0 indicates remove all devices, whereas,
0193          * bdaddr == NULL indicates remove all devices matching the
0194          * mgmt_handle.
0195          */
0196         if ((!mgmt_handle || dev->handle == mgmt_handle) &&
0197             (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
0198                  addr_type == dev->addr_type))) {
0199             if (notify && dev->notified) {
0200                 mgmt_adv_monitor_device_lost(hdev, dev->handle,
0201                                  &dev->bdaddr,
0202                                  dev->addr_type);
0203             }
0204
0205             list_del(&dev->list);
0206             kfree(dev);
0207             count++;
0208         }
0209     }
0210
0211     return count;
0212 }
0213
0214 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
0215                         struct adv_monitor *monitor,
0216                         struct sk_buff *skb)
0217 {
0218     struct msft_rp_le_monitor_advertisement *rp;
0219     struct msft_monitor_advertisement_handle_data *handle_data;
0220     struct msft_data *msft = hdev->msft_data;
0221     int status = 0;
0222
0223     hci_dev_lock(hdev);
0224
0225     rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
0226     if (skb->len < sizeof(*rp)) {
0227         status = HCI_ERROR_UNSPECIFIED;
0228         goto unlock;
0229     }
0230
0231     status = rp->status;
0232     if (status)
0233         goto unlock;
0234
0235     handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
0236     if (!handle_data) {
0237         status = HCI_ERROR_UNSPECIFIED;
0238         goto unlock;
0239     }
0240
0241     handle_data->mgmt_handle = monitor->handle;
0242     handle_data->msft_handle = rp->handle;
0243     INIT_LIST_HEAD(&handle_data->list);
0244     list_add(&handle_data->list, &msft->handle_map);
0245
0246     monitor->state = ADV_MONITOR_STATE_OFFLOADED;
0247
0248 unlock:
0249     if (status)
0250         hci_free_adv_monitor(hdev, monitor);
0251
0252     hci_dev_unlock(hdev);
0253
0254     return status;
0255 }
0256
0257 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
0258                            u16 opcode,
0259                            struct adv_monitor *monitor,
0260                            struct sk_buff *skb)
0261 {
0262     struct msft_rp_le_cancel_monitor_advertisement *rp;
0263     struct msft_monitor_advertisement_handle_data *handle_data;
0264     struct msft_data *msft = hdev->msft_data;
0265     int status = 0;
0266
0267     rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
0268     if (skb->len < sizeof(*rp)) {
0269         status = HCI_ERROR_UNSPECIFIED;
0270         goto done;
0271     }
0272
0273     status = rp->status;
0274     if (status)
0275         goto done;
0276
0277     hci_dev_lock(hdev);
0278
0279     handle_data = msft_find_handle_data(hdev, monitor->handle, true);
0280
0281     if (handle_data) {
0282         if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)
0283             monitor->state = ADV_MONITOR_STATE_REGISTERED;
0284
0285         /* Do not free the monitor if it is being removed due to
0286          * suspend. It will be re-monitored on resume.
0287          */
0288         if (!msft->suspending) {
0289             hci_free_adv_monitor(hdev, monitor);
0290
0291             /* Clear any monitored devices by this Adv Monitor */
0292             msft_monitor_device_del(hdev, handle_data->mgmt_handle,
0293                         NULL, 0, false);
0294         }
0295
0296         list_del(&handle_data->list);
0297         kfree(handle_data);
0298     }
0299
0300     hci_dev_unlock(hdev);
0301
0302 done:
0303     return status;
0304 }
0305
0306 /* This function requires the caller holds hci_req_sync_lock */
0307 static int msft_remove_monitor_sync(struct hci_dev *hdev,
0308                     struct adv_monitor *monitor)
0309 {
0310     struct msft_cp_le_cancel_monitor_advertisement cp;
0311     struct msft_monitor_advertisement_handle_data *handle_data;
0312     struct sk_buff *skb;
0313
0314     handle_data = msft_find_handle_data(hdev, monitor->handle, true);
0315
0316     /* If no matched handle, just remove without telling controller */
0317     if (!handle_data)
0318         return -ENOENT;
0319
0320     cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
0321     cp.handle = handle_data->msft_handle;
0322
0323     skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
0324                  HCI_CMD_TIMEOUT);
0325     if (IS_ERR_OR_NULL(skb)) {
0326         if (!skb)
0327             return -EIO;
0328         return PTR_ERR(skb);
0329     }
0330
0331     return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
0332                                monitor, skb);
0333 }
0334
0335 /* This function requires the caller holds hci_req_sync_lock */
0336 int msft_suspend_sync(struct hci_dev *hdev)
0337 {
0338     struct msft_data *msft = hdev->msft_data;
0339     struct adv_monitor *monitor;
0340     int handle = 0;
0341
0342     if (!msft || !msft_monitor_supported(hdev))
0343         return 0;
0344
0345     msft->suspending = true;
0346
0347     while (1) {
0348         monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
0349         if (!monitor)
0350             break;
0351
0352         msft_remove_monitor_sync(hdev, monitor);
0353
0354         handle++;
0355     }
0356
0357     /* All monitors have been removed */
0358     msft->suspending = false;
0359
0360     return 0;
0361 }
0362
0363 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
0364 {
0365     struct adv_rssi_thresholds *r = &monitor->rssi;
0366
0367     if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
0368         r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
0369         r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
0370         r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
0371         return false;
0372
0373     /* High_threshold_timeout is not supported,
0374      * once high_threshold is reached, events are immediately reported.
0375      */
0376     if (r->high_threshold_timeout != 0)
0377         return false;
0378
0379     if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
0380         return false;
0381
0382     /* Sampling period from 0x00 to 0xFF are all allowed */
0383     return true;
0384 }
0385
0386 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
0387 {
0388     return msft_monitor_rssi_valid(monitor);
0389     /* No additional check needed for pattern-based monitor */
0390 }
0391
0392 static int msft_add_monitor_sync(struct hci_dev *hdev,
0393                  struct adv_monitor *monitor)
0394 {
0395     struct msft_cp_le_monitor_advertisement *cp;
0396     struct msft_le_monitor_advertisement_pattern_data *pattern_data;
0397     struct msft_le_monitor_advertisement_pattern *pattern;
0398     struct adv_pattern *entry;
0399     size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
0400     ptrdiff_t offset = 0;
0401     u8 pattern_count = 0;
0402     struct sk_buff *skb;
0403
0404     if (!msft_monitor_pattern_valid(monitor))
0405         return -EINVAL;
0406
0407     list_for_each_entry(entry, &monitor->patterns, list) {
0408         pattern_count++;
0409         total_size += sizeof(*pattern) + entry->length;
0410     }
0411
0412     cp = kmalloc(total_size, GFP_KERNEL);
0413     if (!cp)
0414         return -ENOMEM;
0415
0416     cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
0417     cp->rssi_high = monitor->rssi.high_threshold;
0418     cp->rssi_low = monitor->rssi.low_threshold;
0419     cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
0420     cp->rssi_sampling_period = monitor->rssi.sampling_period;
0421
0422     cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
0423
0424     pattern_data = (void *)cp->data;
0425     pattern_data->count = pattern_count;
0426
0427     list_for_each_entry(entry, &monitor->patterns, list) {
0428         pattern = (void *)(pattern_data->data + offset);
0429         /* the length also includes data_type and offset */
0430         pattern->length = entry->length + 2;
0431         pattern->data_type = entry->ad_type;
0432         pattern->start_byte = entry->offset;
0433         memcpy(pattern->pattern, entry->value, entry->length);
0434         offset += sizeof(*pattern) + entry->length;
0435     }
0436
0437     skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
0438                  HCI_CMD_TIMEOUT);
0439     kfree(cp);
0440
0441     if (IS_ERR_OR_NULL(skb)) {
0442         if (!skb)
0443             return -EIO;
0444         return PTR_ERR(skb);
0445     }
0446
0447     return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
0448                         monitor, skb);
0449 }
0450
0451 /* This function requires the caller holds hci_req_sync_lock */
0452 static void reregister_monitor(struct hci_dev *hdev)
0453 {
0454     struct adv_monitor *monitor;
0455     struct msft_data *msft = hdev->msft_data;
0456     int handle = 0;
0457
0458     if (!msft)
0459         return;
0460
0461     msft->resuming = true;
0462
0463     while (1) {
0464         monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
0465         if (!monitor)
0466             break;
0467
0468         msft_add_monitor_sync(hdev, monitor);
0469
0470         handle++;
0471     }
0472
0473     /* All monitors have been reregistered */
0474     msft->resuming = false;
0475 }
0476
0477 /* This function requires the caller holds hci_req_sync_lock */
0478 int msft_resume_sync(struct hci_dev *hdev)
0479 {
0480     struct msft_data *msft = hdev->msft_data;
0481
0482     if (!msft || !msft_monitor_supported(hdev))
0483         return 0;
0484
0485     hci_dev_lock(hdev);
0486
0487     /* Clear already tracked devices on resume. Once the monitors are
0488      * reregistered, devices in range will be found again after resume.
0489      */
0490     hdev->advmon_pend_notify = false;
0491     msft_monitor_device_del(hdev, 0, NULL, 0, true);
0492
0493     hci_dev_unlock(hdev);
0494
0495     reregister_monitor(hdev);
0496
0497     return 0;
0498 }
0499
0500 /* This function requires the caller holds hci_req_sync_lock */
0501 void msft_do_open(struct hci_dev *hdev)
0502 {
0503     struct msft_data *msft = hdev->msft_data;
0504
0505     if (hdev->msft_opcode == HCI_OP_NOP)
0506         return;
0507
0508     if (!msft) {
0509         bt_dev_err(hdev, "MSFT extension not registered");
0510         return;
0511     }
0512
0513     bt_dev_dbg(hdev, "Initialize MSFT extension");
0514
0515     /* Reset existing MSFT data before re-reading */
0516     kfree(msft->evt_prefix);
0517     msft->evt_prefix = NULL;
0518     msft->evt_prefix_len = 0;
0519     msft->features = 0;
0520
0521     if (!read_supported_features(hdev, msft)) {
0522         hdev->msft_data = NULL;
0523         kfree(msft);
0524         return;
0525     }
0526
0527     if (msft_monitor_supported(hdev)) {
0528         msft->resuming = true;
0529         msft_set_filter_enable(hdev, true);
0530         /* Monitors get removed on power off, so we need to explicitly
0531          * tell the controller to re-monitor.
0532          */
0533         reregister_monitor(hdev);
0534     }
0535 }
0536
0537 void msft_do_close(struct hci_dev *hdev)
0538 {
0539     struct msft_data *msft = hdev->msft_data;
0540     struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
0541     struct adv_monitor *monitor;
0542
0543     if (!msft)
0544         return;
0545
0546     bt_dev_dbg(hdev, "Cleanup of MSFT extension");
0547
0548     /* The controller will silently remove all monitors on power off.
0549      * Therefore, remove handle_data mapping and reset monitor state.
0550      */
0551     list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
0552         monitor = idr_find(&hdev->adv_monitors_idr,
0553                    handle_data->mgmt_handle);
0554
0555         if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
0556             monitor->state = ADV_MONITOR_STATE_REGISTERED;
0557
0558         list_del(&handle_data->list);
0559         kfree(handle_data);
0560     }
0561
0562     hci_dev_lock(hdev);
0563
0564     /* Clear any devices that are being monitored and notify device lost */
0565     hdev->advmon_pend_notify = false;
0566     msft_monitor_device_del(hdev, 0, NULL, 0, true);
0567
0568     hci_dev_unlock(hdev);
0569 }
0570
0571 void msft_register(struct hci_dev *hdev)
0572 {
0573     struct msft_data *msft = NULL;
0574
0575     bt_dev_dbg(hdev, "Register MSFT extension");
0576
0577     msft = kzalloc(sizeof(*msft), GFP_KERNEL);
0578     if (!msft) {
0579         bt_dev_err(hdev, "Failed to register MSFT extension");
0580         return;
0581     }
0582
0583     INIT_LIST_HEAD(&msft->handle_map);
0584     hdev->msft_data = msft;
0585 }
0586
0587 void msft_unregister(struct hci_dev *hdev)
0588 {
0589     struct msft_data *msft = hdev->msft_data;
0590
0591     if (!msft)
0592         return;
0593
0594     bt_dev_dbg(hdev, "Unregister MSFT extension");
0595
0596     hdev->msft_data = NULL;
0597
0598     kfree(msft->evt_prefix);
0599     kfree(msft);
0600 }
0601
0602 /* This function requires the caller holds hdev->lock */
0603 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
0604                   __u8 addr_type, __u16 mgmt_handle)
0605 {
0606     struct monitored_device *dev;
0607
0608     dev = kmalloc(sizeof(*dev), GFP_KERNEL);
0609     if (!dev) {
0610         bt_dev_err(hdev, "MSFT vendor event %u: no memory",
0611                MSFT_EV_LE_MONITOR_DEVICE);
0612         return;
0613     }
0614
0615     bacpy(&dev->bdaddr, bdaddr);
0616     dev->addr_type = addr_type;
0617     dev->handle = mgmt_handle;
0618     dev->notified = false;
0619
0620     INIT_LIST_HEAD(&dev->list);
0621     list_add(&dev->list, &hdev->monitored_devices);
0622     hdev->advmon_pend_notify = true;
0623 }
0624
0625 /* This function requires the caller holds hdev->lock */
0626 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
0627                  __u8 addr_type, __u16 mgmt_handle)
0628 {
0629     if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
0630                      true)) {
0631         bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
0632                MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
0633     }
0634 }
0635
0636 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
0637                u8 ev, size_t len)
0638 {
0639     void *data;
0640
0641     data = skb_pull_data(skb, len);
0642     if (!data)
0643         bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
0644
0645     return data;
0646 }
0647
0648 /* This function requires the caller holds hdev->lock */
0649 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
0650 {
0651     struct msft_ev_le_monitor_device *ev;
0652     struct msft_monitor_advertisement_handle_data *handle_data;
0653     u8 addr_type;
0654
0655     ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
0656     if (!ev)
0657         return;
0658
0659     bt_dev_dbg(hdev,
0660            "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
0661            MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
0662            ev->monitor_state, &ev->bdaddr);
0663
0664     handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
0665     if (!handle_data)
0666         return;
0667
0668     switch (ev->addr_type) {
0669     case ADDR_LE_DEV_PUBLIC:
0670         addr_type = BDADDR_LE_PUBLIC;
0671         break;
0672
0673     case ADDR_LE_DEV_RANDOM:
0674         addr_type = BDADDR_LE_RANDOM;
0675         break;
0676
0677     default:
0678         bt_dev_err(hdev,
0679                "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
0680                MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
0681         return;
0682     }
0683
0684     if (ev->monitor_state)
0685         msft_device_found(hdev, &ev->bdaddr, addr_type,
0686                   handle_data->mgmt_handle);
0687     else
0688         msft_device_lost(hdev, &ev->bdaddr, addr_type,
0689                  handle_data->mgmt_handle);
0690 }
0691
0692 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
0693 {
0694     struct msft_data *msft = hdev->msft_data;
0695     u8 *evt_prefix;
0696     u8 *evt;
0697
0698     if (!msft)
0699         return;
0700
0701     /* When the extension has defined an event prefix, check that it
0702      * matches, and otherwise just return.
0703      */
0704     if (msft->evt_prefix_len > 0) {
0705         evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
0706         if (!evt_prefix)
0707             return;
0708
0709         if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
0710             return;
0711     }
0712
0713     /* Every event starts at least with an event code and the rest of
0714      * the data is variable and depends on the event code.
0715      */
0716     if (skb->len < 1)
0717         return;
0718
0719     evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
0720     if (!evt)
0721         return;
0722
0723     hci_dev_lock(hdev);
0724
0725     switch (*evt) {
0726     case MSFT_EV_LE_MONITOR_DEVICE:
0727         msft_monitor_device_evt(hdev, skb);
0728         break;
0729
0730     default:
0731         bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
0732         break;
0733     }
0734
0735     hci_dev_unlock(hdev);
0736 }
0737
0738 __u64 msft_get_features(struct hci_dev *hdev)
0739 {
0740     struct msft_data *msft = hdev->msft_data;
0741
0742     return msft ? msft->features : 0;
0743 }
0744
0745 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
0746                                u8 status, u16 opcode,
0747                                struct sk_buff *skb)
0748 {
0749     struct msft_cp_le_set_advertisement_filter_enable *cp;
0750     struct msft_rp_le_set_advertisement_filter_enable *rp;
0751     struct msft_data *msft = hdev->msft_data;
0752
0753     rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
0754     if (skb->len < sizeof(*rp))
0755         return;
0756
0757     /* Error 0x0C would be returned if the filter enabled status is
0758      * already set to whatever we were trying to set.
0759      * Although the default state should be disabled, some controller set
0760      * the initial value to enabled. Because there is no way to know the
0761      * actual initial value before sending this command, here we also treat
0762      * error 0x0C as success.
0763      */
0764     if (status != 0x00 && status != 0x0C)
0765         return;
0766
0767     hci_dev_lock(hdev);
0768
0769     cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
0770     msft->filter_enabled = cp->enable;
0771
0772     if (status == 0x0C)
0773         bt_dev_warn(hdev, "MSFT filter_enable is already %s",
0774                 cp->enable ? "on" : "off");
0775
0776     hci_dev_unlock(hdev);
0777 }
0778
0779 /* This function requires the caller holds hci_req_sync_lock */
0780 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
0781 {
0782     struct msft_data *msft = hdev->msft_data;
0783
0784     if (!msft)
0785         return -EOPNOTSUPP;
0786
0787     if (msft->resuming || msft->suspending)
0788         return -EBUSY;
0789
0790     return msft_add_monitor_sync(hdev, monitor);
0791 }
0792
0793 /* This function requires the caller holds hci_req_sync_lock */
0794 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
0795 {
0796     struct msft_data *msft = hdev->msft_data;
0797
0798     if (!msft)
0799         return -EOPNOTSUPP;
0800
0801     if (msft->resuming || msft->suspending)
0802         return -EBUSY;
0803
0804     return msft_remove_monitor_sync(hdev, monitor);
0805 }
0806
0807 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
0808 {
0809     struct hci_dev *hdev = req->hdev;
0810     struct msft_cp_le_set_advertisement_filter_enable cp;
0811
0812     cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
0813     cp.enable = enable;
0814
0815     hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
0816 }
0817
0818 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
0819 {
0820     struct hci_request req;
0821     struct msft_data *msft = hdev->msft_data;
0822     int err;
0823
0824     if (!msft)
0825         return -EOPNOTSUPP;
0826
0827     hci_req_init(&req, hdev);
0828     msft_req_add_set_filter_enable(&req, enable);
0829     err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
0830
0831     return err;
0832 }
0833
0834 bool msft_curve_validity(struct hci_dev *hdev)
0835 {
0836     return hdev->msft_curve_validity;
0837 }