net/bluetooth/msft.c

   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
  19 struct msft_cp_read_supported_features {
  20         __u8   sub_opcode;
  21 } __packed;
  22
  23 struct 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
  33 struct msft_le_monitor_advertisement_pattern {
  34         __u8 length;
  35         __u8 data_type;
  36         __u8 start_byte;
  37         __u8 pattern[];
  38 };
  39
  40 struct msft_le_monitor_advertisement_pattern_data {
  41         __u8 count;
  42         __u8 data[];
  43 };
  44
  45 struct 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
  55 struct 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
  62 struct msft_cp_le_cancel_monitor_advertisement {
  63         __u8 sub_opcode;
  64         __u8 handle;
  65 } __packed;
  66
  67 struct 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
  73 struct msft_cp_le_set_advertisement_filter_enable {
  74         __u8 sub_opcode;
  75         __u8 enable;
  76 } __packed;
  77
  78 struct msft_rp_le_set_advertisement_filter_enable {
  79         __u8 status;
  80         __u8 sub_opcode;
  81 } __packed;
  82
  83 struct msft_monitor_advertisement_handle_data {
  84         __u8  msft_handle;
  85         __u16 mgmt_handle;
  86         struct list_head list;
  87 };
  88
  89 struct msft_data {
  90         __u64 features;
  91         __u8  evt_prefix_len;
  92         __u8  *evt_prefix;
  93         struct list_head handle_map;
  94         __u16 pending_add_handle;
  95         __u16 pending_remove_handle;
  96         __u8 reregistering;
  97         __u8 suspending;
  98         __u8 filter_enabled;
  99 };
 100
 101 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
 102                                       struct adv_monitor *monitor);
 103 static int __msft_remove_monitor(struct hci_dev *hdev,
 104                                  struct adv_monitor *monitor, u16 handle);
 105
 106 bool msft_monitor_supported(struct hci_dev *hdev)
 107 {
 108         return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
 109 }
 110
 111 static bool read_supported_features(struct hci_dev *hdev,
 112                                     struct msft_data *msft)
 113 {
 114         struct msft_cp_read_supported_features cp;
 115         struct msft_rp_read_supported_features *rp;
 116         struct sk_buff *skb;
 117
 118         cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
 119
 120         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 121                              HCI_CMD_TIMEOUT);
 122         if (IS_ERR(skb)) {
 123                 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
 124                            PTR_ERR(skb));
 125                 return false;
 126         }
 127
 128         if (skb->len < sizeof(*rp)) {
 129                 bt_dev_err(hdev, "MSFT supported features length mismatch");
 130                 goto failed;
 131         }
 132
 133         rp = (struct msft_rp_read_supported_features *)skb->data;
 134
 135         if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
 136                 goto failed;
 137
 138         if (rp->evt_prefix_len > 0) {
 139                 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
 140                                            GFP_KERNEL);
 141                 if (!msft->evt_prefix)
 142                         goto failed;
 143         }
 144
 145         msft->evt_prefix_len = rp->evt_prefix_len;
 146         msft->features = __le64_to_cpu(rp->features);
 147
 148         if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
 149                 hdev->msft_curve_validity = true;
 150
 151         kfree_skb(skb);
 152         return true;
 153
 154 failed:
 155         kfree_skb(skb);
 156         return false;
 157 }
 158
 159 /* This function requires the caller holds hdev->lock */
 160 static void reregister_monitor(struct hci_dev *hdev, int handle)
 161 {
 162         struct adv_monitor *monitor;
 163         struct msft_data *msft = hdev->msft_data;
 164         int err;
 165
 166         while (1) {
 167                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 168                 if (!monitor) {
 169                         /* All monitors have been reregistered */
 170                         msft->reregistering = false;
 171                         hci_update_background_scan(hdev);
 172                         return;
 173                 }
 174
 175                 msft->pending_add_handle = (u16)handle;
 176                 err = __msft_add_monitor_pattern(hdev, monitor);
 177
 178                 /* If success, we return and wait for monitor added callback */
 179                 if (!err)
 180                         return;
 181
 182                 /* Otherwise remove the monitor and keep registering */
 183                 hci_free_adv_monitor(hdev, monitor);
 184                 handle++;
 185         }
 186 }
 187
 188 /* This function requires the caller holds hdev->lock */
 189 static void remove_monitor_on_suspend(struct hci_dev *hdev, int handle)
 190 {
 191         struct adv_monitor *monitor;
 192         struct msft_data *msft = hdev->msft_data;
 193         int err;
 194
 195         while (1) {
 196                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 197                 if (!monitor) {
 198                         /* All monitors have been removed */
 199                         msft->suspending = false;
 200                         hci_update_background_scan(hdev);
 201                         return;
 202                 }
 203
 204                 msft->pending_remove_handle = (u16)handle;
 205                 err = __msft_remove_monitor(hdev, monitor, handle);
 206
 207                 /* If success, return and wait for monitor removed callback */
 208                 if (!err)
 209                         return;
 210
 211                 /* Otherwise free the monitor and keep removing */
 212                 hci_free_adv_monitor(hdev, monitor);
 213                 handle++;
 214         }
 215 }
 216
 217 /* This function requires the caller holds hdev->lock */
 218 void msft_suspend(struct hci_dev *hdev)
 219 {
 220         struct msft_data *msft = hdev->msft_data;
 221
 222         if (!msft)
 223                 return;
 224
 225         if (msft_monitor_supported(hdev)) {
 226                 msft->suspending = true;
 227                 /* Quitely remove all monitors on suspend to avoid waking up
 228                  * the system.
 229                  */
 230                 remove_monitor_on_suspend(hdev, 0);
 231         }
 232 }
 233
 234 /* This function requires the caller holds hdev->lock */
 235 void msft_resume(struct hci_dev *hdev)
 236 {
 237         struct msft_data *msft = hdev->msft_data;
 238
 239         if (!msft)
 240                 return;
 241
 242         if (msft_monitor_supported(hdev)) {
 243                 msft->reregistering = true;
 244                 /* Monitors are removed on suspend, so we need to add all
 245                  * monitors on resume.
 246                  */
 247                 reregister_monitor(hdev, 0);
 248         }
 249 }
 250
 251 void msft_do_open(struct hci_dev *hdev)
 252 {
 253         struct msft_data *msft = hdev->msft_data;
 254
 255         if (hdev->msft_opcode == HCI_OP_NOP)
 256                 return;
 257
 258         if (!msft) {
 259                 bt_dev_err(hdev, "MSFT extension not registered");
 260                 return;
 261         }
 262
 263         bt_dev_dbg(hdev, "Initialize MSFT extension");
 264
 265         /* Reset existing MSFT data before re-reading */
 266         kfree(msft->evt_prefix);
 267         msft->evt_prefix = NULL;
 268         msft->evt_prefix_len = 0;
 269         msft->features = 0;
 270
 271         if (!read_supported_features(hdev, msft)) {
 272                 hdev->msft_data = NULL;
 273                 kfree(msft);
 274                 return;
 275         }
 276
 277         if (msft_monitor_supported(hdev)) {
 278                 msft->reregistering = true;
 279                 msft_set_filter_enable(hdev, true);
 280                 /* Monitors get removed on power off, so we need to explicitly
 281                  * tell the controller to re-monitor.
 282                  */
 283                 reregister_monitor(hdev, 0);
 284         }
 285 }
 286
 287 void msft_do_close(struct hci_dev *hdev)
 288 {
 289         struct msft_data *msft = hdev->msft_data;
 290         struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
 291         struct adv_monitor *monitor;
 292
 293         if (!msft)
 294                 return;
 295
 296         bt_dev_dbg(hdev, "Cleanup of MSFT extension");
 297
 298         /* The controller will silently remove all monitors on power off.
 299          * Therefore, remove handle_data mapping and reset monitor state.
 300          */
 301         list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
 302                 monitor = idr_find(&hdev->adv_monitors_idr,
 303                                    handle_data->mgmt_handle);
 304
 305                 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 306                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 307
 308                 list_del(&handle_data->list);
 309                 kfree(handle_data);
 310         }
 311 }
 312
 313 void msft_register(struct hci_dev *hdev)
 314 {
 315         struct msft_data *msft = NULL;
 316
 317         bt_dev_dbg(hdev, "Register MSFT extension");
 318
 319         msft = kzalloc(sizeof(*msft), GFP_KERNEL);
 320         if (!msft) {
 321                 bt_dev_err(hdev, "Failed to register MSFT extension");
 322                 return;
 323         }
 324
 325         INIT_LIST_HEAD(&msft->handle_map);
 326         hdev->msft_data = msft;
 327 }
 328
 329 void msft_unregister(struct hci_dev *hdev)
 330 {
 331         struct msft_data *msft = hdev->msft_data;
 332
 333         if (!msft)
 334                 return;
 335
 336         bt_dev_dbg(hdev, "Unregister MSFT extension");
 337
 338         hdev->msft_data = NULL;
 339
 340         kfree(msft->evt_prefix);
 341         kfree(msft);
 342 }
 343
 344 void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
 345 {
 346         struct msft_data *msft = hdev->msft_data;
 347         u8 event;
 348
 349         if (!msft)
 350                 return;
 351
 352         /* When the extension has defined an event prefix, check that it
 353          * matches, and otherwise just return.
 354          */
 355         if (msft->evt_prefix_len > 0) {
 356                 if (skb->len < msft->evt_prefix_len)
 357                         return;
 358
 359                 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
 360                         return;
 361
 362                 skb_pull(skb, msft->evt_prefix_len);
 363         }
 364
 365         /* Every event starts at least with an event code and the rest of
 366          * the data is variable and depends on the event code.
 367          */
 368         if (skb->len < 1)
 369                 return;
 370
 371         event = *skb->data;
 372         skb_pull(skb, 1);
 373
 374         bt_dev_dbg(hdev, "MSFT vendor event %u", event);
 375 }
 376
 377 __u64 msft_get_features(struct hci_dev *hdev)
 378 {
 379         struct msft_data *msft = hdev->msft_data;
 380
 381         return msft ? msft->features : 0;
 382 }
 383
 384 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
 385  * is_mgmt = false matches the handle used by the msft controller.
 386  * This function requires the caller holds hdev->lock
 387  */
 388 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
 389                                 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
 390 {
 391         struct msft_monitor_advertisement_handle_data *entry;
 392         struct msft_data *msft = hdev->msft_data;
 393
 394         list_for_each_entry(entry, &msft->handle_map, list) {
 395                 if (is_mgmt && entry->mgmt_handle == handle)
 396                         return entry;
 397                 if (!is_mgmt && entry->msft_handle == handle)
 398                         return entry;
 399         }
 400
 401         return NULL;
 402 }
 403
 404 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
 405                                              u8 status, u16 opcode,
 406                                              struct sk_buff *skb)
 407 {
 408         struct msft_rp_le_monitor_advertisement *rp;
 409         struct adv_monitor *monitor;
 410         struct msft_monitor_advertisement_handle_data *handle_data;
 411         struct msft_data *msft = hdev->msft_data;
 412
 413         hci_dev_lock(hdev);
 414
 415         monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
 416         if (!monitor) {
 417                 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
 418                            msft->pending_add_handle);
 419                 status = HCI_ERROR_UNSPECIFIED;
 420                 goto unlock;
 421         }
 422
 423         if (status)
 424                 goto unlock;
 425
 426         rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
 427         if (skb->len < sizeof(*rp)) {
 428                 status = HCI_ERROR_UNSPECIFIED;
 429                 goto unlock;
 430         }
 431
 432         handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
 433         if (!handle_data) {
 434                 status = HCI_ERROR_UNSPECIFIED;
 435                 goto unlock;
 436         }
 437
 438         handle_data->mgmt_handle = monitor->handle;
 439         handle_data->msft_handle = rp->handle;
 440         INIT_LIST_HEAD(&handle_data->list);
 441         list_add(&handle_data->list, &msft->handle_map);
 442
 443         monitor->state = ADV_MONITOR_STATE_OFFLOADED;
 444
 445 unlock:
 446         if (status && monitor)
 447                 hci_free_adv_monitor(hdev, monitor);
 448
 449         /* If in restart/reregister sequence, keep registering. */
 450         if (msft->reregistering)
 451                 reregister_monitor(hdev, msft->pending_add_handle + 1);
 452
 453         hci_dev_unlock(hdev);
 454
 455         if (!msft->reregistering)
 456                 hci_add_adv_patterns_monitor_complete(hdev, status);
 457 }
 458
 459 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 460                                                     u8 status, u16 opcode,
 461                                                     struct sk_buff *skb)
 462 {
 463         struct msft_cp_le_cancel_monitor_advertisement *cp;
 464         struct msft_rp_le_cancel_monitor_advertisement *rp;
 465         struct adv_monitor *monitor;
 466         struct msft_monitor_advertisement_handle_data *handle_data;
 467         struct msft_data *msft = hdev->msft_data;
 468         int err;
 469         bool pending;
 470
 471         if (status)
 472                 goto done;
 473
 474         rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
 475         if (skb->len < sizeof(*rp)) {
 476                 status = HCI_ERROR_UNSPECIFIED;
 477                 goto done;
 478         }
 479
 480         hci_dev_lock(hdev);
 481
 482         cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
 483         handle_data = msft_find_handle_data(hdev, cp->handle, false);
 484
 485         if (handle_data) {
 486                 monitor = idr_find(&hdev->adv_monitors_idr,
 487                                    handle_data->mgmt_handle);
 488
 489                 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 490                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 491
 492                 /* Do not free the monitor if it is being removed due to
 493                  * suspend. It will be re-monitored on resume.
 494                  */
 495                 if (monitor && !msft->suspending)
 496                         hci_free_adv_monitor(hdev, monitor);
 497
 498                 list_del(&handle_data->list);
 499                 kfree(handle_data);
 500         }
 501
 502         /* If in suspend/remove sequence, keep removing. */
 503         if (msft->suspending)
 504                 remove_monitor_on_suspend(hdev,
 505                                           msft->pending_remove_handle + 1);
 506
 507         /* If remove all monitors is required, we need to continue the process
 508          * here because the earlier it was paused when waiting for the
 509          * response from controller.
 510          */
 511         if (msft->pending_remove_handle == 0) {
 512                 pending = hci_remove_all_adv_monitor(hdev, &err);
 513                 if (pending) {
 514                         hci_dev_unlock(hdev);
 515                         return;
 516                 }
 517
 518                 if (err)
 519                         status = HCI_ERROR_UNSPECIFIED;
 520         }
 521
 522         hci_dev_unlock(hdev);
 523
 524 done:
 525         if (!msft->suspending)
 526                 hci_remove_adv_monitor_complete(hdev, status);
 527 }
 528
 529 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
 530                                                        u8 status, u16 opcode,
 531                                                        struct sk_buff *skb)
 532 {
 533         struct msft_cp_le_set_advertisement_filter_enable *cp;
 534         struct msft_rp_le_set_advertisement_filter_enable *rp;
 535         struct msft_data *msft = hdev->msft_data;
 536
 537         rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
 538         if (skb->len < sizeof(*rp))
 539                 return;
 540
 541         /* Error 0x0C would be returned if the filter enabled status is
 542          * already set to whatever we were trying to set.
 543          * Although the default state should be disabled, some controller set
 544          * the initial value to enabled. Because there is no way to know the
 545          * actual initial value before sending this command, here we also treat
 546          * error 0x0C as success.
 547          */
 548         if (status != 0x00 && status != 0x0C)
 549                 return;
 550
 551         hci_dev_lock(hdev);
 552
 553         cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
 554         msft->filter_enabled = cp->enable;
 555
 556         if (status == 0x0C)
 557                 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
 558                             cp->enable ? "on" : "off");
 559
 560         hci_dev_unlock(hdev);
 561 }
 562
 563 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
 564 {
 565         struct adv_rssi_thresholds *r = &monitor->rssi;
 566
 567         if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 568             r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
 569             r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 570             r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
 571                 return false;
 572
 573         /* High_threshold_timeout is not supported,
 574          * once high_threshold is reached, events are immediately reported.
 575          */
 576         if (r->high_threshold_timeout != 0)
 577                 return false;
 578
 579         if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
 580                 return false;
 581
 582         /* Sampling period from 0x00 to 0xFF are all allowed */
 583         return true;
 584 }
 585
 586 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
 587 {
 588         return msft_monitor_rssi_valid(monitor);
 589         /* No additional check needed for pattern-based monitor */
 590 }
 591
 592 /* This function requires the caller holds hdev->lock */
 593 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
 594                                       struct adv_monitor *monitor)
 595 {
 596         struct msft_cp_le_monitor_advertisement *cp;
 597         struct msft_le_monitor_advertisement_pattern_data *pattern_data;
 598         struct msft_le_monitor_advertisement_pattern *pattern;
 599         struct adv_pattern *entry;
 600         struct hci_request req;
 601         struct msft_data *msft = hdev->msft_data;
 602         size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
 603         ptrdiff_t offset = 0;
 604         u8 pattern_count = 0;
 605         int err = 0;
 606
 607         if (!msft_monitor_pattern_valid(monitor))
 608                 return -EINVAL;
 609
 610         list_for_each_entry(entry, &monitor->patterns, list) {
 611                 pattern_count++;
 612                 total_size += sizeof(*pattern) + entry->length;
 613         }
 614
 615         cp = kmalloc(total_size, GFP_KERNEL);
 616         if (!cp)
 617                 return -ENOMEM;
 618
 619         cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
 620         cp->rssi_high = monitor->rssi.high_threshold;
 621         cp->rssi_low = monitor->rssi.low_threshold;
 622         cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
 623         cp->rssi_sampling_period = monitor->rssi.sampling_period;
 624
 625         cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
 626
 627         pattern_data = (void *)cp->data;
 628         pattern_data->count = pattern_count;
 629
 630         list_for_each_entry(entry, &monitor->patterns, list) {
 631                 pattern = (void *)(pattern_data->data + offset);
 632                 /* the length also includes data_type and offset */
 633                 pattern->length = entry->length + 2;
 634                 pattern->data_type = entry->ad_type;
 635                 pattern->start_byte = entry->offset;
 636                 memcpy(pattern->pattern, entry->value, entry->length);
 637                 offset += sizeof(*pattern) + entry->length;
 638         }
 639
 640         hci_req_init(&req, hdev);
 641         hci_req_add(&req, hdev->msft_opcode, total_size, cp);
 642         err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
 643         kfree(cp);
 644
 645         if (!err)
 646                 msft->pending_add_handle = monitor->handle;
 647
 648         return err;
 649 }
 650
 651 /* This function requires the caller holds hdev->lock */
 652 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
 653 {
 654         struct msft_data *msft = hdev->msft_data;
 655
 656         if (!msft)
 657                 return -EOPNOTSUPP;
 658
 659         if (msft->reregistering || msft->suspending)
 660                 return -EBUSY;
 661
 662         return __msft_add_monitor_pattern(hdev, monitor);
 663 }
 664
 665 /* This function requires the caller holds hdev->lock */
 666 static int __msft_remove_monitor(struct hci_dev *hdev,
 667                                  struct adv_monitor *monitor, u16 handle)
 668 {
 669         struct msft_cp_le_cancel_monitor_advertisement cp;
 670         struct msft_monitor_advertisement_handle_data *handle_data;
 671         struct hci_request req;
 672         struct msft_data *msft = hdev->msft_data;
 673         int err = 0;
 674
 675         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 676
 677         /* If no matched handle, just remove without telling controller */
 678         if (!handle_data)
 679                 return -ENOENT;
 680
 681         cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
 682         cp.handle = handle_data->msft_handle;
 683
 684         hci_req_init(&req, hdev);
 685         hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
 686         err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
 687
 688         if (!err)
 689                 msft->pending_remove_handle = handle;
 690
 691         return err;
 692 }
 693
 694 /* This function requires the caller holds hdev->lock */
 695 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
 696                         u16 handle)
 697 {
 698         struct msft_data *msft = hdev->msft_data;
 699
 700         if (!msft)
 701                 return -EOPNOTSUPP;
 702
 703         if (msft->reregistering || msft->suspending)
 704                 return -EBUSY;
 705
 706         return __msft_remove_monitor(hdev, monitor, handle);
 707 }
 708
 709 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
 710 {
 711         struct hci_dev *hdev = req->hdev;
 712         struct msft_cp_le_set_advertisement_filter_enable cp;
 713
 714         cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
 715         cp.enable = enable;
 716
 717         hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
 718 }
 719
 720 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
 721 {
 722         struct hci_request req;
 723         struct msft_data *msft = hdev->msft_data;
 724         int err;
 725
 726         if (!msft)
 727                 return -EOPNOTSUPP;
 728
 729         hci_req_init(&req, hdev);
 730         msft_req_add_set_filter_enable(&req, enable);
 731         err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
 732
 733         return err;
 734 }
 735
 736 bool msft_curve_validity(struct hci_dev *hdev)
 737 {
 738         return hdev->msft_curve_validity;
 739 }