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[0];
  38 };
  39
  40 struct msft_le_monitor_advertisement_pattern_data {
  41         __u8 count;
  42         __u8 data[0];
  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[0];
  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 struct msft_monitor_advertisement_handle_data {
  84         __u8  msft_handle;
  85         __u16 mgmt_handle;
  86         struct list_head list;
  87 };
  88
  89 struct msft_data {
  90         __u64 features;
  91         __u8  evt_prefix_len;
  92         __u8  *evt_prefix;
  93         struct list_head handle_map;
  94         __u16 pending_add_handle;
  95         __u16 pending_remove_handle;
  96         __u8 reregistering;
  97         __u8 filter_enabled;
  98 };
  99
 100 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
 101                                       struct adv_monitor *monitor);
 102
 103 bool msft_monitor_supported(struct hci_dev *hdev)
 104 {
 105         return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
 106 }
 107
 108 static bool read_supported_features(struct hci_dev *hdev,
 109                                     struct msft_data *msft)
 110 {
 111         struct msft_cp_read_supported_features cp;
 112         struct msft_rp_read_supported_features *rp;
 113         struct sk_buff *skb;
 114
 115         cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
 116
 117         skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
 118                              HCI_CMD_TIMEOUT);
 119         if (IS_ERR(skb)) {
 120                 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
 121                            PTR_ERR(skb));
 122                 return false;
 123         }
 124
 125         if (skb->len < sizeof(*rp)) {
 126                 bt_dev_err(hdev, "MSFT supported features length mismatch");
 127                 goto failed;
 128         }
 129
 130         rp = (struct msft_rp_read_supported_features *)skb->data;
 131
 132         if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
 133                 goto failed;
 134
 135         if (rp->evt_prefix_len > 0) {
 136                 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
 137                                            GFP_KERNEL);
 138                 if (!msft->evt_prefix)
 139                         goto failed;
 140         }
 141
 142         msft->evt_prefix_len = rp->evt_prefix_len;
 143         msft->features = __le64_to_cpu(rp->features);
 144
 145         kfree_skb(skb);
 146         return true;
 147
 148 failed:
 149         kfree_skb(skb);
 150         return false;
 151 }
 152
 153 /* This function requires the caller holds hdev->lock */
 154 static void reregister_monitor_on_restart(struct hci_dev *hdev, int handle)
 155 {
 156         struct adv_monitor *monitor;
 157         struct msft_data *msft = hdev->msft_data;
 158         int err;
 159
 160         while (1) {
 161                 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
 162                 if (!monitor) {
 163                         /* All monitors have been reregistered */
 164                         msft->reregistering = false;
 165                         hci_update_background_scan(hdev);
 166                         return;
 167                 }
 168
 169                 msft->pending_add_handle = (u16)handle;
 170                 err = __msft_add_monitor_pattern(hdev, monitor);
 171
 172                 /* If success, we return and wait for monitor added callback */
 173                 if (!err)
 174                         return;
 175
 176                 /* Otherwise remove the monitor and keep registering */
 177                 hci_free_adv_monitor(hdev, monitor);
 178                 handle++;
 179         }
 180 }
 181
 182 void msft_do_open(struct hci_dev *hdev)
 183 {
 184         struct msft_data *msft;
 185
 186         if (hdev->msft_opcode == HCI_OP_NOP)
 187                 return;
 188
 189         bt_dev_dbg(hdev, "Initialize MSFT extension");
 190
 191         msft = kzalloc(sizeof(*msft), GFP_KERNEL);
 192         if (!msft)
 193                 return;
 194
 195         if (!read_supported_features(hdev, msft)) {
 196                 kfree(msft);
 197                 return;
 198         }
 199
 200         INIT_LIST_HEAD(&msft->handle_map);
 201         hdev->msft_data = msft;
 202
 203         if (msft_monitor_supported(hdev)) {
 204                 msft->reregistering = true;
 205                 msft_set_filter_enable(hdev, true);
 206                 reregister_monitor_on_restart(hdev, 0);
 207         }
 208 }
 209
 210 void msft_do_close(struct hci_dev *hdev)
 211 {
 212         struct msft_data *msft = hdev->msft_data;
 213         struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
 214         struct adv_monitor *monitor;
 215
 216         if (!msft)
 217                 return;
 218
 219         bt_dev_dbg(hdev, "Cleanup of MSFT extension");
 220
 221         hdev->msft_data = NULL;
 222
 223         list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
 224                 monitor = idr_find(&hdev->adv_monitors_idr,
 225                                    handle_data->mgmt_handle);
 226
 227                 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
 228                         monitor->state = ADV_MONITOR_STATE_REGISTERED;
 229
 230                 list_del(&handle_data->list);
 231                 kfree(handle_data);
 232         }
 233
 234         kfree(msft->evt_prefix);
 235         kfree(msft);
 236 }
 237
 238 void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
 239 {
 240         struct msft_data *msft = hdev->msft_data;
 241         u8 event;
 242
 243         if (!msft)
 244                 return;
 245
 246         /* When the extension has defined an event prefix, check that it
 247          * matches, and otherwise just return.
 248          */
 249         if (msft->evt_prefix_len > 0) {
 250                 if (skb->len < msft->evt_prefix_len)
 251                         return;
 252
 253                 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
 254                         return;
 255
 256                 skb_pull(skb, msft->evt_prefix_len);
 257         }
 258
 259         /* Every event starts at least with an event code and the rest of
 260          * the data is variable and depends on the event code.
 261          */
 262         if (skb->len < 1)
 263                 return;
 264
 265         event = *skb->data;
 266         skb_pull(skb, 1);
 267
 268         bt_dev_dbg(hdev, "MSFT vendor event %u", event);
 269 }
 270
 271 __u64 msft_get_features(struct hci_dev *hdev)
 272 {
 273         struct msft_data *msft = hdev->msft_data;
 274
 275         return msft ? msft->features : 0;
 276 }
 277
 278 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
 279  * is_mgmt = false matches the handle used by the msft controller.
 280  * This function requires the caller holds hdev->lock
 281  */
 282 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
 283                                 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
 284 {
 285         struct msft_monitor_advertisement_handle_data *entry;
 286         struct msft_data *msft = hdev->msft_data;
 287
 288         list_for_each_entry(entry, &msft->handle_map, list) {
 289                 if (is_mgmt && entry->mgmt_handle == handle)
 290                         return entry;
 291                 if (!is_mgmt && entry->msft_handle == handle)
 292                         return entry;
 293         }
 294
 295         return NULL;
 296 }
 297
 298 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
 299                                              u8 status, u16 opcode,
 300                                              struct sk_buff *skb)
 301 {
 302         struct msft_rp_le_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
 307         hci_dev_lock(hdev);
 308
 309         monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
 310         if (!monitor) {
 311                 bt_dev_err(hdev, "msft add advmon: monitor %d is not found!",
 312                            msft->pending_add_handle);
 313                 status = HCI_ERROR_UNSPECIFIED;
 314                 goto unlock;
 315         }
 316
 317         if (status)
 318                 goto unlock;
 319
 320         rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
 321         if (skb->len < sizeof(*rp)) {
 322                 status = HCI_ERROR_UNSPECIFIED;
 323                 goto unlock;
 324         }
 325
 326         handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
 327         if (!handle_data) {
 328                 status = HCI_ERROR_UNSPECIFIED;
 329                 goto unlock;
 330         }
 331
 332         handle_data->mgmt_handle = monitor->handle;
 333         handle_data->msft_handle = rp->handle;
 334         INIT_LIST_HEAD(&handle_data->list);
 335         list_add(&handle_data->list, &msft->handle_map);
 336
 337         monitor->state = ADV_MONITOR_STATE_OFFLOADED;
 338
 339 unlock:
 340         if (status && monitor)
 341                 hci_free_adv_monitor(hdev, monitor);
 342
 343         /* If in restart/reregister sequence, keep registering. */
 344         if (msft->reregistering)
 345                 reregister_monitor_on_restart(hdev,
 346                                               msft->pending_add_handle + 1);
 347
 348         hci_dev_unlock(hdev);
 349
 350         if (!msft->reregistering)
 351                 hci_add_adv_patterns_monitor_complete(hdev, status);
 352 }
 353
 354 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
 355                                                     u8 status, u16 opcode,
 356                                                     struct sk_buff *skb)
 357 {
 358         struct msft_cp_le_cancel_monitor_advertisement *cp;
 359         struct msft_rp_le_cancel_monitor_advertisement *rp;
 360         struct adv_monitor *monitor;
 361         struct msft_monitor_advertisement_handle_data *handle_data;
 362         struct msft_data *msft = hdev->msft_data;
 363         int err;
 364         bool pending;
 365
 366         if (status)
 367                 goto done;
 368
 369         rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
 370         if (skb->len < sizeof(*rp)) {
 371                 status = HCI_ERROR_UNSPECIFIED;
 372                 goto done;
 373         }
 374
 375         hci_dev_lock(hdev);
 376
 377         cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
 378         handle_data = msft_find_handle_data(hdev, cp->handle, false);
 379
 380         if (handle_data) {
 381                 monitor = idr_find(&hdev->adv_monitors_idr,
 382                                    handle_data->mgmt_handle);
 383                 if (monitor)
 384                         hci_free_adv_monitor(hdev, monitor);
 385
 386                 list_del(&handle_data->list);
 387                 kfree(handle_data);
 388         }
 389
 390         /* If remove all monitors is required, we need to continue the process
 391          * here because the earlier it was paused when waiting for the
 392          * response from controller.
 393          */
 394         if (msft->pending_remove_handle == 0) {
 395                 pending = hci_remove_all_adv_monitor(hdev, &err);
 396                 if (pending) {
 397                         hci_dev_unlock(hdev);
 398                         return;
 399                 }
 400
 401                 if (err)
 402                         status = HCI_ERROR_UNSPECIFIED;
 403         }
 404
 405         hci_dev_unlock(hdev);
 406
 407 done:
 408         hci_remove_adv_monitor_complete(hdev, status);
 409 }
 410
 411 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
 412                                                        u8 status, u16 opcode,
 413                                                        struct sk_buff *skb)
 414 {
 415         struct msft_cp_le_set_advertisement_filter_enable *cp;
 416         struct msft_rp_le_set_advertisement_filter_enable *rp;
 417         struct msft_data *msft = hdev->msft_data;
 418
 419         rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
 420         if (skb->len < sizeof(*rp))
 421                 return;
 422
 423         /* Error 0x0C would be returned if the filter enabled status is
 424          * already set to whatever we were trying to set.
 425          * Although the default state should be disabled, some controller set
 426          * the initial value to enabled. Because there is no way to know the
 427          * actual initial value before sending this command, here we also treat
 428          * error 0x0C as success.
 429          */
 430         if (status != 0x00 && status != 0x0C)
 431                 return;
 432
 433         hci_dev_lock(hdev);
 434
 435         cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
 436         msft->filter_enabled = cp->enable;
 437
 438         if (status == 0x0C)
 439                 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
 440                             cp->enable ? "on" : "off");
 441
 442         hci_dev_unlock(hdev);
 443 }
 444
 445 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
 446 {
 447         struct adv_rssi_thresholds *r = &monitor->rssi;
 448
 449         if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 450             r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
 451             r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
 452             r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
 453                 return false;
 454
 455         /* High_threshold_timeout is not supported,
 456          * once high_threshold is reached, events are immediately reported.
 457          */
 458         if (r->high_threshold_timeout != 0)
 459                 return false;
 460
 461         if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
 462                 return false;
 463
 464         /* Sampling period from 0x00 to 0xFF are all allowed */
 465         return true;
 466 }
 467
 468 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
 469 {
 470         return msft_monitor_rssi_valid(monitor);
 471         /* No additional check needed for pattern-based monitor */
 472 }
 473
 474 /* This function requires the caller holds hdev->lock */
 475 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
 476                                       struct adv_monitor *monitor)
 477 {
 478         struct msft_cp_le_monitor_advertisement *cp;
 479         struct msft_le_monitor_advertisement_pattern_data *pattern_data;
 480         struct msft_le_monitor_advertisement_pattern *pattern;
 481         struct adv_pattern *entry;
 482         struct hci_request req;
 483         struct msft_data *msft = hdev->msft_data;
 484         size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
 485         ptrdiff_t offset = 0;
 486         u8 pattern_count = 0;
 487         int err = 0;
 488
 489         if (!msft_monitor_pattern_valid(monitor))
 490                 return -EINVAL;
 491
 492         list_for_each_entry(entry, &monitor->patterns, list) {
 493                 pattern_count++;
 494                 total_size += sizeof(*pattern) + entry->length;
 495         }
 496
 497         cp = kmalloc(total_size, GFP_KERNEL);
 498         if (!cp)
 499                 return -ENOMEM;
 500
 501         cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
 502         cp->rssi_high = monitor->rssi.high_threshold;
 503         cp->rssi_low = monitor->rssi.low_threshold;
 504         cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
 505         cp->rssi_sampling_period = monitor->rssi.sampling_period;
 506
 507         cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
 508
 509         pattern_data = (void *)cp->data;
 510         pattern_data->count = pattern_count;
 511
 512         list_for_each_entry(entry, &monitor->patterns, list) {
 513                 pattern = (void *)(pattern_data->data + offset);
 514                 /* the length also includes data_type and offset */
 515                 pattern->length = entry->length + 2;
 516                 pattern->data_type = entry->ad_type;
 517                 pattern->start_byte = entry->offset;
 518                 memcpy(pattern->pattern, entry->value, entry->length);
 519                 offset += sizeof(*pattern) + entry->length;
 520         }
 521
 522         hci_req_init(&req, hdev);
 523         hci_req_add(&req, hdev->msft_opcode, total_size, cp);
 524         err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
 525         kfree(cp);
 526
 527         if (!err)
 528                 msft->pending_add_handle = monitor->handle;
 529
 530         return err;
 531 }
 532
 533 /* This function requires the caller holds hdev->lock */
 534 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
 535 {
 536         struct msft_data *msft = hdev->msft_data;
 537
 538         if (!msft)
 539                 return -EOPNOTSUPP;
 540
 541         if (msft->reregistering)
 542                 return -EBUSY;
 543
 544         return __msft_add_monitor_pattern(hdev, monitor);
 545 }
 546
 547 /* This function requires the caller holds hdev->lock */
 548 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
 549                         u16 handle)
 550 {
 551         struct msft_cp_le_cancel_monitor_advertisement cp;
 552         struct msft_monitor_advertisement_handle_data *handle_data;
 553         struct hci_request req;
 554         struct msft_data *msft = hdev->msft_data;
 555         int err = 0;
 556
 557         if (!msft)
 558                 return -EOPNOTSUPP;
 559
 560         if (msft->reregistering)
 561                 return -EBUSY;
 562
 563         handle_data = msft_find_handle_data(hdev, monitor->handle, true);
 564
 565         /* If no matched handle, just remove without telling controller */
 566         if (!handle_data)
 567                 return -ENOENT;
 568
 569         cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
 570         cp.handle = handle_data->msft_handle;
 571
 572         hci_req_init(&req, hdev);
 573         hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
 574         err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
 575
 576         if (!err)
 577                 msft->pending_remove_handle = handle;
 578
 579         return err;
 580 }
 581
 582 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
 583 {
 584         struct hci_dev *hdev = req->hdev;
 585         struct msft_cp_le_set_advertisement_filter_enable cp;
 586
 587         cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
 588         cp.enable = enable;
 589
 590         hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
 591 }
 592
 593 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
 594 {
 595         struct hci_request req;
 596         struct msft_data *msft = hdev->msft_data;
 597         int err;
 598
 599         if (!msft)
 600                 return -EOPNOTSUPP;
 601
 602         hci_req_init(&req, hdev);
 603         msft_req_add_set_filter_enable(&req, enable);
 604         err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
 605
 606         return err;
 607 }