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