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