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