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