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 #define MSFT_EV_LE_MONITOR_DEVICE       0x02
  84 struct msft_ev_le_monitor_device {
  85         __u8     addr_type;
  86         bdaddr_t bdaddr;
  87         __u8     monitor_handle;
  88         __u8     monitor_state;
  89 } __packed;
  90
  91 struct msft_monitor_advertisement_handle_data {
  92         __u8  msft_handle;
  93         __u16 mgmt_handle;
  94         struct list_head list;
  95 };
  96
  97 struct msft_data {
  98         __u64 features;
  99         __u8  evt_prefix_len;
 100         __u8  *evt_prefix;
 101         struct list_head handle_map;
 102         __u8 resuming;
 103         __u8 suspending;
 104         __u8 filter_enabled;
 105 };
 106
 107 bool msft_monitor_supported(struct hci_dev *hdev)
 108 {
 109         return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
 110 }
 111
 112 static bool read_supported_features(struct hci_dev *hdev,
 113                                     struct msft_data *msft)
 114 {
 115         struct msft_cp_read_supported_features cp;
 116         struct msft_rp_read_supported_features *rp;
 117         struct sk_buff *skb;
 118
 119         cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
 120
 121         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 122                              HCI_CMD_TIMEOUT);
 123         if (IS_ERR(skb)) {
 124                 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
 125                            PTR_ERR(skb));
 126                 return false;
 127         }
 128
 129         if (skb->len < sizeof(*rp)) {
 130                 bt_dev_err(hdev, "MSFT supported features length mismatch");
 131                 goto failed;
 132         }
 133
 134         rp = (struct msft_rp_read_supported_features *)skb->data;
 135
 136         if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
 137                 goto failed;
 138
 139         if (rp->evt_prefix_len > 0) {
 140                 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
 141                                            GFP_KERNEL);
 142                 if (!msft->evt_prefix)
 143                         goto failed;
 144         }
 145
 146         msft->evt_prefix_len = rp->evt_prefix_len;
 147         msft->features = __le64_to_cpu(rp->features);
 148
 149         if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
 150                 hdev->msft_curve_validity = true;
 151
 152         kfree_skb(skb);
 153         return true;
 154
 155 failed:
 156         kfree_skb(skb);
 157         return false;
 158 }
 159
 160 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
 161  * is_mgmt = false matches the handle used by the msft controller.
 162  * This function requires the caller holds hdev->lock
 163  */
 164 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
 165                                 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
 166 {
 167         struct msft_monitor_advertisement_handle_data *entry;
 168         struct msft_data *msft = hdev->msft_data;
 169
 170         list_for_each_entry(entry, &msft->handle_map, list) {
 171                 if (is_mgmt && entry->mgmt_handle == handle)
 172                         return entry;
 173                 if (!is_mgmt && entry->msft_handle == handle)
 174                         return entry;
 175         }
 176
 177         return NULL;
 178 }
 179
 180 /* This function requires the caller holds hdev->lock */
 181 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
 182                                    bdaddr_t *bdaddr, __u8 addr_type,
 183                                    bool notify)
 184 {
 185         struct monitored_device *dev, *tmp;
 186         int count = 0;
 187
 188         list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
 189                 /* mgmt_handle == 0 indicates remove all devices, whereas,
 190                  * bdaddr == NULL indicates remove all devices matching the
 191                  * mgmt_handle.
 192                  */
 193                 if ((!mgmt_handle || dev->handle == mgmt_handle) &&
 194                     (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
 195                                  addr_type == dev->addr_type))) {
 196                         if (notify && dev->notified) {
 197                                 mgmt_adv_monitor_device_lost(hdev, dev->handle,
 198                                                              &dev->bdaddr,
 199                                                              dev->addr_type);
 200                         }
 201
 202                         list_del(&dev->list);
 203                         kfree(dev);
 204                         count++;
 205                 }
 206         }
 207
 208         return count;
 209 }
 210
 211 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
 212                                             struct adv_monitor *monitor,
 213                                             struct sk_buff *skb)
 214 {
 215         struct msft_rp_le_monitor_advertisement *rp;
 216         struct msft_monitor_advertisement_handle_data *handle_data;
 217         struct msft_data *msft = hdev->msft_data;
 218         int status = 0;
 219
 220         hci_dev_lock(hdev);
 221
 222         rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
 223         if (skb->len < sizeof(*rp)) {
 224                 status = HCI_ERROR_UNSPECIFIED;
 225                 goto unlock;
 226         }
 227
 228         status = rp->status;
 229         if (status)
 230                 goto unlock;
 231
 232         handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
 233         if (!handle_data) {
 234                 status = HCI_ERROR_UNSPECIFIED;
 235                 goto unlock;
 236         }
 237
 238         handle_data->mgmt_handle = monitor->handle;
 239         handle_data->msft_handle = rp->handle;
 240         INIT_LIST_HEAD(&handle_data->list);
 241         list_add(&handle_data->list, &msft->handle_map);
 242
 243         monitor->state = ADV_MONITOR_STATE_OFFLOADED;
 244
 245 unlock:
 246         if (status)
 247                 hci_free_adv_monitor(hdev, monitor);
 248
 249         hci_dev_unlock(hdev);
 250
 251         return status;
 252 }
 253
 254 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 255                                                    u16 opcode,
 256                                                    struct adv_monitor *monitor,
 257                                                    struct sk_buff *skb)
 258 {
 259         struct msft_rp_le_cancel_monitor_advertisement *rp;
 260         struct msft_monitor_advertisement_handle_data *handle_data;
 261         struct msft_data *msft = hdev->msft_data;
 262         int status = 0;
 263
 264         rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
 265         if (skb->len < sizeof(*rp)) {
 266                 status = HCI_ERROR_UNSPECIFIED;
 267                 goto done;
 268         }
 269
 270         status = rp->status;
 271         if (status)
 272                 goto done;
 273
 274         hci_dev_lock(hdev);
 275
 276         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 277
 278         if (handle_data) {
 279                 if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 280                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 281
 282                 /* Do not free the monitor if it is being removed due to
 283                  * suspend. It will be re-monitored on resume.
 284                  */
 285                 if (!msft->suspending) {
 286                         hci_free_adv_monitor(hdev, monitor);
 287
 288                         /* Clear any monitored devices by this Adv Monitor */
 289                         msft_monitor_device_del(hdev, handle_data->mgmt_handle,
 290                                                 NULL, 0, false);
 291                 }
 292
 293                 list_del(&handle_data->list);
 294                 kfree(handle_data);
 295         }
 296
 297         hci_dev_unlock(hdev);
 298
 299 done:
 300         return status;
 301 }
 302
 303 /* This function requires the caller holds hci_req_sync_lock */
 304 static int msft_remove_monitor_sync(struct hci_dev *hdev,
 305                                     struct adv_monitor *monitor)
 306 {
 307         struct msft_cp_le_cancel_monitor_advertisement cp;
 308         struct msft_monitor_advertisement_handle_data *handle_data;
 309         struct sk_buff *skb;
 310
 311         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 312
 313         /* If no matched handle, just remove without telling controller */
 314         if (!handle_data)
 315                 return -ENOENT;
 316
 317         cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
 318         cp.handle = handle_data->msft_handle;
 319
 320         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 321                              HCI_CMD_TIMEOUT);
 322         if (IS_ERR(skb))
 323                 return PTR_ERR(skb);
 324
 325         return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
 326                                                        monitor, skb);
 327 }
 328
 329 /* This function requires the caller holds hci_req_sync_lock */
 330 int msft_suspend_sync(struct hci_dev *hdev)
 331 {
 332         struct msft_data *msft = hdev->msft_data;
 333         struct adv_monitor *monitor;
 334         int handle = 0;
 335
 336         if (!msft || !msft_monitor_supported(hdev))
 337                 return 0;
 338
 339         msft->suspending = true;
 340
 341         while (1) {
 342                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 343                 if (!monitor)
 344                         break;
 345
 346                 msft_remove_monitor_sync(hdev, monitor);
 347
 348                 handle++;
 349         }
 350
 351         /* All monitors have been removed */
 352         msft->suspending = false;
 353
 354         return 0;
 355 }
 356
 357 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
 358 {
 359         struct adv_rssi_thresholds *r = &monitor->rssi;
 360
 361         if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 362             r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
 363             r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 364             r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
 365                 return false;
 366
 367         /* High_threshold_timeout is not supported,
 368          * once high_threshold is reached, events are immediately reported.
 369          */
 370         if (r->high_threshold_timeout != 0)
 371                 return false;
 372
 373         if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
 374                 return false;
 375
 376         /* Sampling period from 0x00 to 0xFF are all allowed */
 377         return true;
 378 }
 379
 380 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
 381 {
 382         return msft_monitor_rssi_valid(monitor);
 383         /* No additional check needed for pattern-based monitor */
 384 }
 385
 386 static int msft_add_monitor_sync(struct hci_dev *hdev,
 387                                  struct adv_monitor *monitor)
 388 {
 389         struct msft_cp_le_monitor_advertisement *cp;
 390         struct msft_le_monitor_advertisement_pattern_data *pattern_data;
 391         struct msft_le_monitor_advertisement_pattern *pattern;
 392         struct adv_pattern *entry;
 393         size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
 394         ptrdiff_t offset = 0;
 395         u8 pattern_count = 0;
 396         struct sk_buff *skb;
 397
 398         if (!msft_monitor_pattern_valid(monitor))
 399                 return -EINVAL;
 400
 401         list_for_each_entry(entry, &monitor->patterns, list) {
 402                 pattern_count++;
 403                 total_size += sizeof(*pattern) + entry->length;
 404         }
 405
 406         cp = kmalloc(total_size, GFP_KERNEL);
 407         if (!cp)
 408                 return -ENOMEM;
 409
 410         cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
 411         cp->rssi_high = monitor->rssi.high_threshold;
 412         cp->rssi_low = monitor->rssi.low_threshold;
 413         cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
 414         cp->rssi_sampling_period = monitor->rssi.sampling_period;
 415
 416         cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
 417
 418         pattern_data = (void *)cp->data;
 419         pattern_data->count = pattern_count;
 420
 421         list_for_each_entry(entry, &monitor->patterns, list) {
 422                 pattern = (void *)(pattern_data->data + offset);
 423                 /* the length also includes data_type and offset */
 424                 pattern->length = entry->length + 2;
 425                 pattern->data_type = entry->ad_type;
 426                 pattern->start_byte = entry->offset;
 427                 memcpy(pattern->pattern, entry->value, entry->length);
 428                 offset += sizeof(*pattern) + entry->length;
 429         }
 430
 431         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
 432                              HCI_CMD_TIMEOUT);
 433         kfree(cp);
 434
 435         if (IS_ERR(skb))
 436                 return PTR_ERR(skb);
 437
 438         return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
 439                                                 monitor, skb);
 440 }
 441
 442 /* This function requires the caller holds hci_req_sync_lock */
 443 static void reregister_monitor(struct hci_dev *hdev)
 444 {
 445         struct adv_monitor *monitor;
 446         struct msft_data *msft = hdev->msft_data;
 447         int handle = 0;
 448
 449         if (!msft)
 450                 return;
 451
 452         msft->resuming = true;
 453
 454         while (1) {
 455                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 456                 if (!monitor)
 457                         break;
 458
 459                 msft_add_monitor_sync(hdev, monitor);
 460
 461                 handle++;
 462         }
 463
 464         /* All monitors have been reregistered */
 465         msft->resuming = false;
 466 }
 467
 468 /* This function requires the caller holds hci_req_sync_lock */
 469 int msft_resume_sync(struct hci_dev *hdev)
 470 {
 471         struct msft_data *msft = hdev->msft_data;
 472
 473         if (!msft || !msft_monitor_supported(hdev))
 474                 return 0;
 475
 476         hci_dev_lock(hdev);
 477
 478         /* Clear already tracked devices on resume. Once the monitors are
 479          * reregistered, devices in range will be found again after resume.
 480          */
 481         hdev->advmon_pend_notify = false;
 482         msft_monitor_device_del(hdev, 0, NULL, 0, true);
 483
 484         hci_dev_unlock(hdev);
 485
 486         reregister_monitor(hdev);
 487
 488         return 0;
 489 }
 490
 491 /* This function requires the caller holds hci_req_sync_lock */
 492 void msft_do_open(struct hci_dev *hdev)
 493 {
 494         struct msft_data *msft = hdev->msft_data;
 495
 496         if (hdev->msft_opcode == HCI_OP_NOP)
 497                 return;
 498
 499         if (!msft) {
 500                 bt_dev_err(hdev, "MSFT extension not registered");
 501                 return;
 502         }
 503
 504         bt_dev_dbg(hdev, "Initialize MSFT extension");
 505
 506         /* Reset existing MSFT data before re-reading */
 507         kfree(msft->evt_prefix);
 508         msft->evt_prefix = NULL;
 509         msft->evt_prefix_len = 0;
 510         msft->features = 0;
 511
 512         if (!read_supported_features(hdev, msft)) {
 513                 hdev->msft_data = NULL;
 514                 kfree(msft);
 515                 return;
 516         }
 517
 518         if (msft_monitor_supported(hdev)) {
 519                 msft->resuming = true;
 520                 msft_set_filter_enable(hdev, true);
 521                 /* Monitors get removed on power off, so we need to explicitly
 522                  * tell the controller to re-monitor.
 523                  */
 524                 reregister_monitor(hdev);
 525         }
 526 }
 527
 528 void msft_do_close(struct hci_dev *hdev)
 529 {
 530         struct msft_data *msft = hdev->msft_data;
 531         struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
 532         struct adv_monitor *monitor;
 533
 534         if (!msft)
 535                 return;
 536
 537         bt_dev_dbg(hdev, "Cleanup of MSFT extension");
 538
 539         /* The controller will silently remove all monitors on power off.
 540          * Therefore, remove handle_data mapping and reset monitor state.
 541          */
 542         list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
 543                 monitor = idr_find(&hdev->adv_monitors_idr,
 544                                    handle_data->mgmt_handle);
 545
 546                 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 547                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 548
 549                 list_del(&handle_data->list);
 550                 kfree(handle_data);
 551         }
 552
 553         hci_dev_lock(hdev);
 554
 555         /* Clear any devices that are being monitored and notify device lost */
 556         hdev->advmon_pend_notify = false;
 557         msft_monitor_device_del(hdev, 0, NULL, 0, true);
 558
 559         hci_dev_unlock(hdev);
 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 /* This function requires the caller holds hdev->lock */
 594 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
 595                               __u8 addr_type, __u16 mgmt_handle)
 596 {
 597         struct monitored_device *dev;
 598
 599         dev = kmalloc(sizeof(*dev), GFP_KERNEL);
 600         if (!dev) {
 601                 bt_dev_err(hdev, "MSFT vendor event %u: no memory",
 602                            MSFT_EV_LE_MONITOR_DEVICE);
 603                 return;
 604         }
 605
 606         bacpy(&dev->bdaddr, bdaddr);
 607         dev->addr_type = addr_type;
 608         dev->handle = mgmt_handle;
 609         dev->notified = false;
 610
 611         INIT_LIST_HEAD(&dev->list);
 612         list_add(&dev->list, &hdev->monitored_devices);
 613         hdev->advmon_pend_notify = true;
 614 }
 615
 616 /* This function requires the caller holds hdev->lock */
 617 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
 618                              __u8 addr_type, __u16 mgmt_handle)
 619 {
 620         if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
 621                                      true)) {
 622                 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
 623                            MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
 624         }
 625 }
 626
 627 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
 628                            u8 ev, size_t len)
 629 {
 630         void *data;
 631
 632         data = skb_pull_data(skb, len);
 633         if (!data)
 634                 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
 635
 636         return data;
 637 }
 638
 639 /* This function requires the caller holds hdev->lock */
 640 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
 641 {
 642         struct msft_ev_le_monitor_device *ev;
 643         struct msft_monitor_advertisement_handle_data *handle_data;
 644         u8 addr_type;
 645
 646         ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
 647         if (!ev)
 648                 return;
 649
 650         bt_dev_dbg(hdev,
 651                    "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
 652                    MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
 653                    ev->monitor_state, &ev->bdaddr);
 654
 655         handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
 656         if (!handle_data)
 657                 return;
 658
 659         switch (ev->addr_type) {
 660         case ADDR_LE_DEV_PUBLIC:
 661                 addr_type = BDADDR_LE_PUBLIC;
 662                 break;
 663
 664         case ADDR_LE_DEV_RANDOM:
 665                 addr_type = BDADDR_LE_RANDOM;
 666                 break;
 667
 668         default:
 669                 bt_dev_err(hdev,
 670                            "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
 671                            MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
 672                 return;
 673         }
 674
 675         if (ev->monitor_state)
 676                 msft_device_found(hdev, &ev->bdaddr, addr_type,
 677                                   handle_data->mgmt_handle);
 678         else
 679                 msft_device_lost(hdev, &ev->bdaddr, addr_type,
 680                                  handle_data->mgmt_handle);
 681 }
 682
 683 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
 684 {
 685         struct msft_data *msft = hdev->msft_data;
 686         u8 *evt_prefix;
 687         u8 *evt;
 688
 689         if (!msft)
 690                 return;
 691
 692         /* When the extension has defined an event prefix, check that it
 693          * matches, and otherwise just return.
 694          */
 695         if (msft->evt_prefix_len > 0) {
 696                 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
 697                 if (!evt_prefix)
 698                         return;
 699
 700                 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
 701                         return;
 702         }
 703
 704         /* Every event starts at least with an event code and the rest of
 705          * the data is variable and depends on the event code.
 706          */
 707         if (skb->len < 1)
 708                 return;
 709
 710         evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
 711         if (!evt)
 712                 return;
 713
 714         hci_dev_lock(hdev);
 715
 716         switch (*evt) {
 717         case MSFT_EV_LE_MONITOR_DEVICE:
 718                 msft_monitor_device_evt(hdev, skb);
 719                 break;
 720
 721         default:
 722                 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
 723                 break;
 724         }
 725
 726         hci_dev_unlock(hdev);
 727 }
 728
 729 __u64 msft_get_features(struct hci_dev *hdev)
 730 {
 731         struct msft_data *msft = hdev->msft_data;
 732
 733         return msft ? msft->features : 0;
 734 }
 735
 736 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
 737                                                        u8 status, u16 opcode,
 738                                                        struct sk_buff *skb)
 739 {
 740         struct msft_cp_le_set_advertisement_filter_enable *cp;
 741         struct msft_rp_le_set_advertisement_filter_enable *rp;
 742         struct msft_data *msft = hdev->msft_data;
 743
 744         rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
 745         if (skb->len < sizeof(*rp))
 746                 return;
 747
 748         /* Error 0x0C would be returned if the filter enabled status is
 749          * already set to whatever we were trying to set.
 750          * Although the default state should be disabled, some controller set
 751          * the initial value to enabled. Because there is no way to know the
 752          * actual initial value before sending this command, here we also treat
 753          * error 0x0C as success.
 754          */
 755         if (status != 0x00 && status != 0x0C)
 756                 return;
 757
 758         hci_dev_lock(hdev);
 759
 760         cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
 761         msft->filter_enabled = cp->enable;
 762
 763         if (status == 0x0C)
 764                 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
 765                             cp->enable ? "on" : "off");
 766
 767         hci_dev_unlock(hdev);
 768 }
 769
 770 /* This function requires the caller holds hci_req_sync_lock */
 771 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
 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_add_monitor_sync(hdev, monitor);
 782 }
 783
 784 /* This function requires the caller holds hci_req_sync_lock */
 785 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
 786 {
 787         struct msft_data *msft = hdev->msft_data;
 788
 789         if (!msft)
 790                 return -EOPNOTSUPP;
 791
 792         if (msft->resuming || msft->suspending)
 793                 return -EBUSY;
 794
 795         return msft_remove_monitor_sync(hdev, monitor);
 796 }
 797
 798 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
 799 {
 800         struct hci_dev *hdev = req->hdev;
 801         struct msft_cp_le_set_advertisement_filter_enable cp;
 802
 803         cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
 804         cp.enable = enable;
 805
 806         hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
 807 }
 808
 809 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
 810 {
 811         struct hci_request req;
 812         struct msft_data *msft = hdev->msft_data;
 813         int err;
 814
 815         if (!msft)
 816                 return -EOPNOTSUPP;
 817
 818         hci_req_init(&req, hdev);
 819         msft_req_add_set_filter_enable(&req, enable);
 820         err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
 821
 822         return err;
 823 }
 824
 825 bool msft_curve_validity(struct hci_dev *hdev)
 826 {
 827         return hdev->msft_curve_validity;
 828 }