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         __s8 rssi_high;
  95         __s8 rssi_low;
  96         __u8 rssi_low_interval;
  97         __u8 rssi_sampling_period;
  98         __u8 cond_type;
  99         struct list_head list;
 100 };
 101
 102 enum monitor_addr_filter_state {
 103         AF_STATE_IDLE,
 104         AF_STATE_ADDING,
 105         AF_STATE_ADDED,
 106         AF_STATE_REMOVING,
 107 };
 108
 109 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR    0x04
 110 struct msft_monitor_addr_filter_data {
 111         __u8     msft_handle;
 112         __u8     pattern_handle; /* address filters pertain to */
 113         __u16    mgmt_handle;
 114         int      state;
 115         __s8     rssi_high;
 116         __s8     rssi_low;
 117         __u8     rssi_low_interval;
 118         __u8     rssi_sampling_period;
 119         __u8     addr_type;
 120         bdaddr_t bdaddr;
 121         struct list_head list;
 122 };
 123
 124 struct msft_data {
 125         __u64 features;
 126         __u8  evt_prefix_len;
 127         __u8  *evt_prefix;
 128         struct list_head handle_map;
 129         struct list_head address_filters;
 130         __u8 resuming;
 131         __u8 suspending;
 132         __u8 filter_enabled;
 133         /* To synchronize add/remove address filter and monitor device event.*/
 134         struct mutex filter_lock;
 135 };
 136
 137 bool msft_monitor_supported(struct hci_dev *hdev)
 138 {
 139         return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
 140 }
 141
 142 static bool read_supported_features(struct hci_dev *hdev,
 143                                     struct msft_data *msft)
 144 {
 145         struct msft_cp_read_supported_features cp;
 146         struct msft_rp_read_supported_features *rp;
 147         struct sk_buff *skb;
 148
 149         cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
 150
 151         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 152                              HCI_CMD_TIMEOUT);
 153         if (IS_ERR(skb)) {
 154                 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
 155                            PTR_ERR(skb));
 156                 return false;
 157         }
 158
 159         if (skb->len < sizeof(*rp)) {
 160                 bt_dev_err(hdev, "MSFT supported features length mismatch");
 161                 goto failed;
 162         }
 163
 164         rp = (struct msft_rp_read_supported_features *)skb->data;
 165
 166         if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
 167                 goto failed;
 168
 169         if (rp->evt_prefix_len > 0) {
 170                 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
 171                                            GFP_KERNEL);
 172                 if (!msft->evt_prefix)
 173                         goto failed;
 174         }
 175
 176         msft->evt_prefix_len = rp->evt_prefix_len;
 177         msft->features = __le64_to_cpu(rp->features);
 178
 179         if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
 180                 hdev->msft_curve_validity = true;
 181
 182         kfree_skb(skb);
 183         return true;
 184
 185 failed:
 186         kfree_skb(skb);
 187         return false;
 188 }
 189
 190 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
 191  * is_mgmt = false matches the handle used by the msft controller.
 192  * This function requires the caller holds hdev->lock
 193  */
 194 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
 195                                 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
 196 {
 197         struct msft_monitor_advertisement_handle_data *entry;
 198         struct msft_data *msft = hdev->msft_data;
 199
 200         list_for_each_entry(entry, &msft->handle_map, list) {
 201                 if (is_mgmt && entry->mgmt_handle == handle)
 202                         return entry;
 203                 if (!is_mgmt && entry->msft_handle == handle)
 204                         return entry;
 205         }
 206
 207         return NULL;
 208 }
 209
 210 /* This function requires the caller holds msft->filter_lock */
 211 static struct msft_monitor_addr_filter_data *msft_find_address_data
 212                         (struct hci_dev *hdev, u8 addr_type, bdaddr_t *addr,
 213                          u8 pattern_handle)
 214 {
 215         struct msft_monitor_addr_filter_data *entry;
 216         struct msft_data *msft = hdev->msft_data;
 217
 218         list_for_each_entry(entry, &msft->address_filters, list) {
 219                 if (entry->pattern_handle == pattern_handle &&
 220                     addr_type == entry->addr_type &&
 221                     !bacmp(addr, &entry->bdaddr))
 222                         return entry;
 223         }
 224
 225         return NULL;
 226 }
 227
 228 /* This function requires the caller holds hdev->lock */
 229 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
 230                                    bdaddr_t *bdaddr, __u8 addr_type,
 231                                    bool notify)
 232 {
 233         struct monitored_device *dev, *tmp;
 234         int count = 0;
 235
 236         list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
 237                 /* mgmt_handle == 0 indicates remove all devices, whereas,
 238                  * bdaddr == NULL indicates remove all devices matching the
 239                  * mgmt_handle.
 240                  */
 241                 if ((!mgmt_handle || dev->handle == mgmt_handle) &&
 242                     (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
 243                                  addr_type == dev->addr_type))) {
 244                         if (notify && dev->notified) {
 245                                 mgmt_adv_monitor_device_lost(hdev, dev->handle,
 246                                                              &dev->bdaddr,
 247                                                              dev->addr_type);
 248                         }
 249
 250                         list_del(&dev->list);
 251                         kfree(dev);
 252                         count++;
 253                 }
 254         }
 255
 256         return count;
 257 }
 258
 259 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
 260                                             struct adv_monitor *monitor,
 261                                             struct sk_buff *skb)
 262 {
 263         struct msft_rp_le_monitor_advertisement *rp;
 264         struct msft_monitor_advertisement_handle_data *handle_data;
 265         struct msft_data *msft = hdev->msft_data;
 266         int status = 0;
 267
 268         hci_dev_lock(hdev);
 269
 270         rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
 271         if (skb->len < sizeof(*rp)) {
 272                 status = HCI_ERROR_UNSPECIFIED;
 273                 goto unlock;
 274         }
 275
 276         status = rp->status;
 277         if (status)
 278                 goto unlock;
 279
 280         handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
 281         if (!handle_data) {
 282                 status = HCI_ERROR_UNSPECIFIED;
 283                 goto unlock;
 284         }
 285
 286         handle_data->mgmt_handle = monitor->handle;
 287         handle_data->msft_handle = rp->handle;
 288         handle_data->cond_type   = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
 289         INIT_LIST_HEAD(&handle_data->list);
 290         list_add(&handle_data->list, &msft->handle_map);
 291
 292         monitor->state = ADV_MONITOR_STATE_OFFLOADED;
 293
 294 unlock:
 295         if (status)
 296                 hci_free_adv_monitor(hdev, monitor);
 297
 298         hci_dev_unlock(hdev);
 299
 300         return status;
 301 }
 302
 303 /* This function requires the caller holds hci_req_sync_lock */
 304 static void msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle)
 305 {
 306         struct msft_monitor_addr_filter_data *address_filter, *n;
 307         struct msft_cp_le_cancel_monitor_advertisement cp;
 308         struct msft_data *msft = hdev->msft_data;
 309         struct list_head head;
 310         struct sk_buff *skb;
 311
 312         INIT_LIST_HEAD(&head);
 313
 314         /* Cancel all corresponding address monitors */
 315         mutex_lock(&msft->filter_lock);
 316
 317         list_for_each_entry_safe(address_filter, n, &msft->address_filters,
 318                                  list) {
 319                 if (address_filter->pattern_handle != handle)
 320                         continue;
 321
 322                 list_del(&address_filter->list);
 323
 324                 /* Keep the address filter and let
 325                  * msft_add_address_filter_sync() remove and free the address
 326                  * filter.
 327                  */
 328                 if (address_filter->state == AF_STATE_ADDING) {
 329                         address_filter->state = AF_STATE_REMOVING;
 330                         continue;
 331                 }
 332
 333                 /* Keep the address filter and let
 334                  * msft_cancel_address_filter_sync() remove and free the address
 335                  * filter
 336                  */
 337                 if (address_filter->state == AF_STATE_REMOVING)
 338                         continue;
 339
 340                 list_add_tail(&address_filter->list, &head);
 341         }
 342
 343         mutex_unlock(&msft->filter_lock);
 344
 345         list_for_each_entry_safe(address_filter, n, &head, list) {
 346                 list_del(&address_filter->list);
 347
 348                 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
 349                 cp.handle = address_filter->msft_handle;
 350
 351                 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 352                                      HCI_CMD_TIMEOUT);
 353                 if (IS_ERR(skb)) {
 354                         kfree(address_filter);
 355                         continue;
 356                 }
 357
 358                 kfree_skb(skb);
 359
 360                 bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",
 361                            &address_filter->bdaddr);
 362
 363                 kfree(address_filter);
 364         }
 365 }
 366
 367 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 368                                                    u16 opcode,
 369                                                    struct adv_monitor *monitor,
 370                                                    struct sk_buff *skb)
 371 {
 372         struct msft_rp_le_cancel_monitor_advertisement *rp;
 373         struct msft_monitor_advertisement_handle_data *handle_data;
 374         struct msft_data *msft = hdev->msft_data;
 375         int status = 0;
 376         u8 msft_handle;
 377
 378         rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
 379         if (skb->len < sizeof(*rp)) {
 380                 status = HCI_ERROR_UNSPECIFIED;
 381                 goto done;
 382         }
 383
 384         status = rp->status;
 385         if (status)
 386                 goto done;
 387
 388         hci_dev_lock(hdev);
 389
 390         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 391
 392         if (handle_data) {
 393                 if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 394                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 395
 396                 /* Do not free the monitor if it is being removed due to
 397                  * suspend. It will be re-monitored on resume.
 398                  */
 399                 if (!msft->suspending) {
 400                         hci_free_adv_monitor(hdev, monitor);
 401
 402                         /* Clear any monitored devices by this Adv Monitor */
 403                         msft_monitor_device_del(hdev, handle_data->mgmt_handle,
 404                                                 NULL, 0, false);
 405                 }
 406
 407                 msft_handle = handle_data->msft_handle;
 408
 409                 list_del(&handle_data->list);
 410                 kfree(handle_data);
 411
 412                 hci_dev_unlock(hdev);
 413
 414                 msft_remove_addr_filters_sync(hdev, msft_handle);
 415         } else {
 416                 hci_dev_unlock(hdev);
 417         }
 418
 419 done:
 420         return status;
 421 }
 422
 423 /* This function requires the caller holds hci_req_sync_lock */
 424 static int msft_remove_monitor_sync(struct hci_dev *hdev,
 425                                     struct adv_monitor *monitor)
 426 {
 427         struct msft_cp_le_cancel_monitor_advertisement cp;
 428         struct msft_monitor_advertisement_handle_data *handle_data;
 429         struct sk_buff *skb;
 430
 431         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 432
 433         /* If no matched handle, just remove without telling controller */
 434         if (!handle_data)
 435                 return -ENOENT;
 436
 437         cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
 438         cp.handle = handle_data->msft_handle;
 439
 440         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 441                              HCI_CMD_TIMEOUT);
 442         if (IS_ERR(skb))
 443                 return PTR_ERR(skb);
 444
 445         return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
 446                                                        monitor, skb);
 447 }
 448
 449 /* This function requires the caller holds hci_req_sync_lock */
 450 int msft_suspend_sync(struct hci_dev *hdev)
 451 {
 452         struct msft_data *msft = hdev->msft_data;
 453         struct adv_monitor *monitor;
 454         int handle = 0;
 455
 456         if (!msft || !msft_monitor_supported(hdev))
 457                 return 0;
 458
 459         msft->suspending = true;
 460
 461         while (1) {
 462                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 463                 if (!monitor)
 464                         break;
 465
 466                 msft_remove_monitor_sync(hdev, monitor);
 467
 468                 handle++;
 469         }
 470
 471         /* All monitors have been removed */
 472         msft->suspending = false;
 473
 474         return 0;
 475 }
 476
 477 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
 478 {
 479         struct adv_rssi_thresholds *r = &monitor->rssi;
 480
 481         if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 482             r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
 483             r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 484             r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
 485                 return false;
 486
 487         /* High_threshold_timeout is not supported,
 488          * once high_threshold is reached, events are immediately reported.
 489          */
 490         if (r->high_threshold_timeout != 0)
 491                 return false;
 492
 493         if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
 494                 return false;
 495
 496         /* Sampling period from 0x00 to 0xFF are all allowed */
 497         return true;
 498 }
 499
 500 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
 501 {
 502         return msft_monitor_rssi_valid(monitor);
 503         /* No additional check needed for pattern-based monitor */
 504 }
 505
 506 static int msft_add_monitor_sync(struct hci_dev *hdev,
 507                                  struct adv_monitor *monitor)
 508 {
 509         struct msft_cp_le_monitor_advertisement *cp;
 510         struct msft_le_monitor_advertisement_pattern_data *pattern_data;
 511         struct msft_monitor_advertisement_handle_data *handle_data;
 512         struct msft_le_monitor_advertisement_pattern *pattern;
 513         struct adv_pattern *entry;
 514         size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
 515         ptrdiff_t offset = 0;
 516         u8 pattern_count = 0;
 517         struct sk_buff *skb;
 518         int err;
 519
 520         if (!msft_monitor_pattern_valid(monitor))
 521                 return -EINVAL;
 522
 523         list_for_each_entry(entry, &monitor->patterns, list) {
 524                 pattern_count++;
 525                 total_size += sizeof(*pattern) + entry->length;
 526         }
 527
 528         cp = kmalloc(total_size, GFP_KERNEL);
 529         if (!cp)
 530                 return -ENOMEM;
 531
 532         cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
 533         cp->rssi_high = monitor->rssi.high_threshold;
 534         cp->rssi_low = monitor->rssi.low_threshold;
 535         cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
 536         cp->rssi_sampling_period = monitor->rssi.sampling_period;
 537
 538         cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
 539
 540         pattern_data = (void *)cp->data;
 541         pattern_data->count = pattern_count;
 542
 543         list_for_each_entry(entry, &monitor->patterns, list) {
 544                 pattern = (void *)(pattern_data->data + offset);
 545                 /* the length also includes data_type and offset */
 546                 pattern->length = entry->length + 2;
 547                 pattern->data_type = entry->ad_type;
 548                 pattern->start_byte = entry->offset;
 549                 memcpy(pattern->pattern, entry->value, entry->length);
 550                 offset += sizeof(*pattern) + entry->length;
 551         }
 552
 553         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
 554                              HCI_CMD_TIMEOUT);
 555
 556         if (IS_ERR(skb)) {
 557                 err = PTR_ERR(skb);
 558                 goto out_free;
 559         }
 560
 561         err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
 562                                                monitor, skb);
 563         if (err)
 564                 goto out_free;
 565
 566         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 567         if (!handle_data) {
 568                 err = -ENODATA;
 569                 goto out_free;
 570         }
 571
 572         handle_data->rssi_high  = cp->rssi_high;
 573         handle_data->rssi_low   = cp->rssi_low;
 574         handle_data->rssi_low_interval    = cp->rssi_low_interval;
 575         handle_data->rssi_sampling_period = cp->rssi_sampling_period;
 576
 577 out_free:
 578         kfree(cp);
 579         return err;
 580 }
 581
 582 /* This function requires the caller holds hci_req_sync_lock */
 583 static void reregister_monitor(struct hci_dev *hdev)
 584 {
 585         struct adv_monitor *monitor;
 586         struct msft_data *msft = hdev->msft_data;
 587         int handle = 0;
 588
 589         if (!msft)
 590                 return;
 591
 592         msft->resuming = true;
 593
 594         while (1) {
 595                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 596                 if (!monitor)
 597                         break;
 598
 599                 msft_add_monitor_sync(hdev, monitor);
 600
 601                 handle++;
 602         }
 603
 604         /* All monitors have been reregistered */
 605         msft->resuming = false;
 606 }
 607
 608 /* This function requires the caller holds hci_req_sync_lock */
 609 int msft_resume_sync(struct hci_dev *hdev)
 610 {
 611         struct msft_data *msft = hdev->msft_data;
 612
 613         if (!msft || !msft_monitor_supported(hdev))
 614                 return 0;
 615
 616         hci_dev_lock(hdev);
 617
 618         /* Clear already tracked devices on resume. Once the monitors are
 619          * reregistered, devices in range will be found again after resume.
 620          */
 621         hdev->advmon_pend_notify = false;
 622         msft_monitor_device_del(hdev, 0, NULL, 0, true);
 623
 624         hci_dev_unlock(hdev);
 625
 626         reregister_monitor(hdev);
 627
 628         return 0;
 629 }
 630
 631 /* This function requires the caller holds hci_req_sync_lock */
 632 void msft_do_open(struct hci_dev *hdev)
 633 {
 634         struct msft_data *msft = hdev->msft_data;
 635
 636         if (hdev->msft_opcode == HCI_OP_NOP)
 637                 return;
 638
 639         if (!msft) {
 640                 bt_dev_err(hdev, "MSFT extension not registered");
 641                 return;
 642         }
 643
 644         bt_dev_dbg(hdev, "Initialize MSFT extension");
 645
 646         /* Reset existing MSFT data before re-reading */
 647         kfree(msft->evt_prefix);
 648         msft->evt_prefix = NULL;
 649         msft->evt_prefix_len = 0;
 650         msft->features = 0;
 651
 652         if (!read_supported_features(hdev, msft)) {
 653                 hdev->msft_data = NULL;
 654                 kfree(msft);
 655                 return;
 656         }
 657
 658         if (msft_monitor_supported(hdev)) {
 659                 msft->resuming = true;
 660                 msft_set_filter_enable(hdev, true);
 661                 /* Monitors get removed on power off, so we need to explicitly
 662                  * tell the controller to re-monitor.
 663                  */
 664                 reregister_monitor(hdev);
 665         }
 666 }
 667
 668 void msft_do_close(struct hci_dev *hdev)
 669 {
 670         struct msft_data *msft = hdev->msft_data;
 671         struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
 672         struct msft_monitor_addr_filter_data *address_filter, *n;
 673         struct adv_monitor *monitor;
 674
 675         if (!msft)
 676                 return;
 677
 678         bt_dev_dbg(hdev, "Cleanup of MSFT extension");
 679
 680         /* The controller will silently remove all monitors on power off.
 681          * Therefore, remove handle_data mapping and reset monitor state.
 682          */
 683         list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
 684                 monitor = idr_find(&hdev->adv_monitors_idr,
 685                                    handle_data->mgmt_handle);
 686
 687                 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 688                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 689
 690                 list_del(&handle_data->list);
 691                 kfree(handle_data);
 692         }
 693
 694         mutex_lock(&msft->filter_lock);
 695         list_for_each_entry_safe(address_filter, n, &msft->address_filters,
 696                                  list) {
 697                 list_del(&address_filter->list);
 698                 kfree(address_filter);
 699         }
 700         mutex_unlock(&msft->filter_lock);
 701
 702         hci_dev_lock(hdev);
 703
 704         /* Clear any devices that are being monitored and notify device lost */
 705         hdev->advmon_pend_notify = false;
 706         msft_monitor_device_del(hdev, 0, NULL, 0, true);
 707
 708         hci_dev_unlock(hdev);
 709 }
 710
 711 static int msft_cancel_address_filter_sync(struct hci_dev *hdev, void *data)
 712 {
 713         struct msft_monitor_addr_filter_data *address_filter = data;
 714         struct msft_cp_le_cancel_monitor_advertisement cp;
 715         struct msft_data *msft = hdev->msft_data;
 716         struct sk_buff *skb;
 717         int err = 0;
 718
 719         if (!msft) {
 720                 bt_dev_err(hdev, "MSFT: msft data is freed");
 721                 return -EINVAL;
 722         }
 723
 724         /* The address filter has been removed by hci dev close */
 725         if (!test_bit(HCI_UP, &hdev->flags))
 726                 return 0;
 727
 728         mutex_lock(&msft->filter_lock);
 729         list_del(&address_filter->list);
 730         mutex_unlock(&msft->filter_lock);
 731
 732         cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
 733         cp.handle = address_filter->msft_handle;
 734
 735         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 736                              HCI_CMD_TIMEOUT);
 737         if (IS_ERR(skb)) {
 738                 bt_dev_err(hdev, "MSFT: Failed to cancel address (%pMR) filter",
 739                            &address_filter->bdaddr);
 740                 err = PTR_ERR(skb);
 741                 goto done;
 742         }
 743         kfree_skb(skb);
 744
 745         bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter",
 746                    &address_filter->bdaddr);
 747
 748 done:
 749         kfree(address_filter);
 750
 751         return err;
 752 }
 753
 754 void msft_register(struct hci_dev *hdev)
 755 {
 756         struct msft_data *msft = NULL;
 757
 758         bt_dev_dbg(hdev, "Register MSFT extension");
 759
 760         msft = kzalloc(sizeof(*msft), GFP_KERNEL);
 761         if (!msft) {
 762                 bt_dev_err(hdev, "Failed to register MSFT extension");
 763                 return;
 764         }
 765
 766         INIT_LIST_HEAD(&msft->handle_map);
 767         INIT_LIST_HEAD(&msft->address_filters);
 768         hdev->msft_data = msft;
 769         mutex_init(&msft->filter_lock);
 770 }
 771
 772 void msft_unregister(struct hci_dev *hdev)
 773 {
 774         struct msft_data *msft = hdev->msft_data;
 775
 776         if (!msft)
 777                 return;
 778
 779         bt_dev_dbg(hdev, "Unregister MSFT extension");
 780
 781         hdev->msft_data = NULL;
 782
 783         kfree(msft->evt_prefix);
 784         mutex_destroy(&msft->filter_lock);
 785         kfree(msft);
 786 }
 787
 788 /* This function requires the caller holds hdev->lock */
 789 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
 790                               __u8 addr_type, __u16 mgmt_handle)
 791 {
 792         struct monitored_device *dev;
 793
 794         dev = kmalloc(sizeof(*dev), GFP_KERNEL);
 795         if (!dev) {
 796                 bt_dev_err(hdev, "MSFT vendor event %u: no memory",
 797                            MSFT_EV_LE_MONITOR_DEVICE);
 798                 return;
 799         }
 800
 801         bacpy(&dev->bdaddr, bdaddr);
 802         dev->addr_type = addr_type;
 803         dev->handle = mgmt_handle;
 804         dev->notified = false;
 805
 806         INIT_LIST_HEAD(&dev->list);
 807         list_add(&dev->list, &hdev->monitored_devices);
 808         hdev->advmon_pend_notify = true;
 809 }
 810
 811 /* This function requires the caller holds hdev->lock */
 812 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
 813                              __u8 addr_type, __u16 mgmt_handle)
 814 {
 815         if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
 816                                      true)) {
 817                 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
 818                            MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
 819         }
 820 }
 821
 822 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
 823                            u8 ev, size_t len)
 824 {
 825         void *data;
 826
 827         data = skb_pull_data(skb, len);
 828         if (!data)
 829                 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
 830
 831         return data;
 832 }
 833
 834 static int msft_add_address_filter_sync(struct hci_dev *hdev, void *data)
 835 {
 836         struct msft_monitor_addr_filter_data *address_filter = data;
 837         struct msft_rp_le_monitor_advertisement *rp;
 838         struct msft_cp_le_monitor_advertisement *cp;
 839         struct msft_data *msft = hdev->msft_data;
 840         struct sk_buff *skb = NULL;
 841         bool remove = false;
 842         size_t size;
 843
 844         if (!msft) {
 845                 bt_dev_err(hdev, "MSFT: msft data is freed");
 846                 return -EINVAL;
 847         }
 848
 849         /* The address filter has been removed by hci dev close */
 850         if (!test_bit(HCI_UP, &hdev->flags))
 851                 return -ENODEV;
 852
 853         /* We are safe to use the address filter from now on.
 854          * msft_monitor_device_evt() wouldn't delete this filter because it's
 855          * not been added by now.
 856          * And all other functions that requiring hci_req_sync_lock wouldn't
 857          * touch this filter before this func completes because it's protected
 858          * by hci_req_sync_lock.
 859          */
 860
 861         if (address_filter->state == AF_STATE_REMOVING) {
 862                 mutex_lock(&msft->filter_lock);
 863                 list_del(&address_filter->list);
 864                 mutex_unlock(&msft->filter_lock);
 865                 kfree(address_filter);
 866                 return 0;
 867         }
 868
 869         size = sizeof(*cp) +
 870                sizeof(address_filter->addr_type) +
 871                sizeof(address_filter->bdaddr);
 872         cp = kzalloc(size, GFP_KERNEL);
 873         if (!cp) {
 874                 bt_dev_err(hdev, "MSFT: Alloc cmd param err");
 875                 remove = true;
 876                 goto done;
 877         }
 878         cp->sub_opcode           = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
 879         cp->rssi_high            = address_filter->rssi_high;
 880         cp->rssi_low             = address_filter->rssi_low;
 881         cp->rssi_low_interval    = address_filter->rssi_low_interval;
 882         cp->rssi_sampling_period = address_filter->rssi_sampling_period;
 883         cp->cond_type            = MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR;
 884         cp->data[0]              = address_filter->addr_type;
 885         memcpy(&cp->data[1], &address_filter->bdaddr,
 886                sizeof(address_filter->bdaddr));
 887
 888         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, size, cp,
 889                              HCI_CMD_TIMEOUT);
 890         if (IS_ERR(skb)) {
 891                 bt_dev_err(hdev, "Failed to enable address %pMR filter",
 892                            &address_filter->bdaddr);
 893                 skb = NULL;
 894                 remove = true;
 895                 goto done;
 896         }
 897
 898         rp = skb_pull_data(skb, sizeof(*rp));
 899         if (!rp || rp->sub_opcode != MSFT_OP_LE_MONITOR_ADVERTISEMENT ||
 900             rp->status)
 901                 remove = true;
 902
 903 done:
 904         mutex_lock(&msft->filter_lock);
 905
 906         if (remove) {
 907                 bt_dev_warn(hdev, "MSFT: Remove address (%pMR) filter",
 908                             &address_filter->bdaddr);
 909                 list_del(&address_filter->list);
 910                 kfree(address_filter);
 911         } else {
 912                 address_filter->state = AF_STATE_ADDED;
 913                 address_filter->msft_handle = rp->handle;
 914                 bt_dev_dbg(hdev, "MSFT: Address %pMR filter enabled",
 915                            &address_filter->bdaddr);
 916         }
 917         mutex_unlock(&msft->filter_lock);
 918
 919         kfree_skb(skb);
 920
 921         return 0;
 922 }
 923
 924 /* This function requires the caller holds msft->filter_lock */
 925 static struct msft_monitor_addr_filter_data *msft_add_address_filter
 926                 (struct hci_dev *hdev, u8 addr_type, bdaddr_t *bdaddr,
 927                  struct msft_monitor_advertisement_handle_data *handle_data)
 928 {
 929         struct msft_monitor_addr_filter_data *address_filter = NULL;
 930         struct msft_data *msft = hdev->msft_data;
 931         int err;
 932
 933         address_filter = kzalloc(sizeof(*address_filter), GFP_KERNEL);
 934         if (!address_filter)
 935                 return NULL;
 936
 937         address_filter->state             = AF_STATE_ADDING;
 938         address_filter->msft_handle       = 0xff;
 939         address_filter->pattern_handle    = handle_data->msft_handle;
 940         address_filter->mgmt_handle       = handle_data->mgmt_handle;
 941         address_filter->rssi_high         = handle_data->rssi_high;
 942         address_filter->rssi_low          = handle_data->rssi_low;
 943         address_filter->rssi_low_interval = handle_data->rssi_low_interval;
 944         address_filter->rssi_sampling_period = handle_data->rssi_sampling_period;
 945         address_filter->addr_type            = addr_type;
 946         bacpy(&address_filter->bdaddr, bdaddr);
 947
 948         /* With the above AF_STATE_ADDING, duplicated address filter can be
 949          * avoided when receiving monitor device event (found/lost) frequently
 950          * for the same device.
 951          */
 952         list_add_tail(&address_filter->list, &msft->address_filters);
 953
 954         err = hci_cmd_sync_queue(hdev, msft_add_address_filter_sync,
 955                                  address_filter, NULL);
 956         if (err < 0) {
 957                 bt_dev_err(hdev, "MSFT: Add address %pMR filter err", bdaddr);
 958                 list_del(&address_filter->list);
 959                 kfree(address_filter);
 960                 return NULL;
 961         }
 962
 963         bt_dev_dbg(hdev, "MSFT: Add device %pMR address filter",
 964                    &address_filter->bdaddr);
 965
 966         return address_filter;
 967 }
 968
 969 /* This function requires the caller holds hdev->lock */
 970 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
 971 {
 972         struct msft_monitor_addr_filter_data *n, *address_filter = NULL;
 973         struct msft_ev_le_monitor_device *ev;
 974         struct msft_monitor_advertisement_handle_data *handle_data;
 975         struct msft_data *msft = hdev->msft_data;
 976         u16 mgmt_handle = 0xffff;
 977         u8 addr_type;
 978
 979         ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
 980         if (!ev)
 981                 return;
 982
 983         bt_dev_dbg(hdev,
 984                    "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
 985                    MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
 986                    ev->monitor_state, &ev->bdaddr);
 987
 988         handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
 989
 990         if (!test_bit(HCI_QUIRK_USE_MSFT_EXT_ADDRESS_FILTER, &hdev->quirks)) {
 991                 if (!handle_data)
 992                         return;
 993                 mgmt_handle = handle_data->mgmt_handle;
 994                 goto report_state;
 995         }
 996
 997         if (handle_data) {
 998                 /* Don't report any device found/lost event from pattern
 999                  * monitors. Pattern monitor always has its address filters for
1000                  * tracking devices.
1001                  */
1002
1003                 address_filter = msft_find_address_data(hdev, ev->addr_type,
1004                                                         &ev->bdaddr,
1005                                                         handle_data->msft_handle);
1006                 if (address_filter)
1007                         return;
1008
1009                 if (ev->monitor_state && handle_data->cond_type ==
1010                                 MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN)
1011                         msft_add_address_filter(hdev, ev->addr_type,
1012                                                 &ev->bdaddr, handle_data);
1013
1014                 return;
1015         }
1016
1017         /* This device event is not from pattern monitor.
1018          * Report it if there is a corresponding address_filter for it.
1019          */
1020         list_for_each_entry(n, &msft->address_filters, list) {
1021                 if (n->state == AF_STATE_ADDED &&
1022                     n->msft_handle == ev->monitor_handle) {
1023                         mgmt_handle = n->mgmt_handle;
1024                         address_filter = n;
1025                         break;
1026                 }
1027         }
1028
1029         if (!address_filter) {
1030                 bt_dev_warn(hdev, "MSFT: Unexpected device event %pMR, %u, %u",
1031                             &ev->bdaddr, ev->monitor_handle, ev->monitor_state);
1032                 return;
1033         }
1034
1035 report_state:
1036         switch (ev->addr_type) {
1037         case ADDR_LE_DEV_PUBLIC:
1038                 addr_type = BDADDR_LE_PUBLIC;
1039                 break;
1040
1041         case ADDR_LE_DEV_RANDOM:
1042                 addr_type = BDADDR_LE_RANDOM;
1043                 break;
1044
1045         default:
1046                 bt_dev_err(hdev,
1047                            "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
1048                            MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
1049                 return;
1050         }
1051
1052         if (ev->monitor_state) {
1053                 msft_device_found(hdev, &ev->bdaddr, addr_type, mgmt_handle);
1054         } else {
1055                 if (address_filter && address_filter->state == AF_STATE_ADDED) {
1056                         address_filter->state = AF_STATE_REMOVING;
1057                         hci_cmd_sync_queue(hdev,
1058                                            msft_cancel_address_filter_sync,
1059                                            address_filter,
1060                                            NULL);
1061                 }
1062                 msft_device_lost(hdev, &ev->bdaddr, addr_type, mgmt_handle);
1063         }
1064 }
1065
1066 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
1067 {
1068         struct msft_data *msft = hdev->msft_data;
1069         u8 *evt_prefix;
1070         u8 *evt;
1071
1072         if (!msft)
1073                 return;
1074
1075         /* When the extension has defined an event prefix, check that it
1076          * matches, and otherwise just return.
1077          */
1078         if (msft->evt_prefix_len > 0) {
1079                 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
1080                 if (!evt_prefix)
1081                         return;
1082
1083                 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
1084                         return;
1085         }
1086
1087         /* Every event starts at least with an event code and the rest of
1088          * the data is variable and depends on the event code.
1089          */
1090         if (skb->len < 1)
1091                 return;
1092
1093         evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
1094         if (!evt)
1095                 return;
1096
1097         hci_dev_lock(hdev);
1098
1099         switch (*evt) {
1100         case MSFT_EV_LE_MONITOR_DEVICE:
1101                 mutex_lock(&msft->filter_lock);
1102                 msft_monitor_device_evt(hdev, skb);
1103                 mutex_unlock(&msft->filter_lock);
1104                 break;
1105
1106         default:
1107                 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
1108                 break;
1109         }
1110
1111         hci_dev_unlock(hdev);
1112 }
1113
1114 __u64 msft_get_features(struct hci_dev *hdev)
1115 {
1116         struct msft_data *msft = hdev->msft_data;
1117
1118         return msft ? msft->features : 0;
1119 }
1120
1121 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
1122                                                        void *user_data,
1123                                                        u8 status)
1124 {
1125         struct msft_cp_le_set_advertisement_filter_enable *cp = user_data;
1126         struct msft_data *msft = hdev->msft_data;
1127
1128         /* Error 0x0C would be returned if the filter enabled status is
1129          * already set to whatever we were trying to set.
1130          * Although the default state should be disabled, some controller set
1131          * the initial value to enabled. Because there is no way to know the
1132          * actual initial value before sending this command, here we also treat
1133          * error 0x0C as success.
1134          */
1135         if (status != 0x00 && status != 0x0C)
1136                 return;
1137
1138         hci_dev_lock(hdev);
1139
1140         msft->filter_enabled = cp->enable;
1141
1142         if (status == 0x0C)
1143                 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
1144                             cp->enable ? "on" : "off");
1145
1146         hci_dev_unlock(hdev);
1147 }
1148
1149 /* This function requires the caller holds hci_req_sync_lock */
1150 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
1151 {
1152         struct msft_data *msft = hdev->msft_data;
1153
1154         if (!msft)
1155                 return -EOPNOTSUPP;
1156
1157         if (msft->resuming || msft->suspending)
1158                 return -EBUSY;
1159
1160         return msft_add_monitor_sync(hdev, monitor);
1161 }
1162
1163 /* This function requires the caller holds hci_req_sync_lock */
1164 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
1165 {
1166         struct msft_data *msft = hdev->msft_data;
1167
1168         if (!msft)
1169                 return -EOPNOTSUPP;
1170
1171         if (msft->resuming || msft->suspending)
1172                 return -EBUSY;
1173
1174         return msft_remove_monitor_sync(hdev, monitor);
1175 }
1176
1177 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
1178 {
1179         struct msft_cp_le_set_advertisement_filter_enable cp;
1180         struct msft_data *msft = hdev->msft_data;
1181         int err;
1182
1183         if (!msft)
1184                 return -EOPNOTSUPP;
1185
1186         cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
1187         cp.enable = enable;
1188         err = __hci_cmd_sync_status(hdev, hdev->msft_opcode, sizeof(cp), &cp,
1189                                     HCI_CMD_TIMEOUT);
1190
1191         msft_le_set_advertisement_filter_enable_cb(hdev, &cp, err);
1192
1193         return 0;
1194 }
1195
1196 bool msft_curve_validity(struct hci_dev *hdev)
1197 {
1198         return hdev->msft_curve_validity;
1199 }