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