1 // SPDX-License-Identifier: GPL-2.0-only 2 /* 3 * Copyright (C) 2020 Google Corporation 4 */ 5 6 #include <net/bluetooth/bluetooth.h> 7 #include <net/bluetooth/hci_core.h> 8 #include <net/bluetooth/mgmt.h> 9 10 #include "hci_request.h" 11 #include "mgmt_util.h" 12 #include "msft.h" 13 14 #define MSFT_RSSI_THRESHOLD_VALUE_MIN -127 15 #define MSFT_RSSI_THRESHOLD_VALUE_MAX 20 16 #define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C 17 18 #define MSFT_OP_READ_SUPPORTED_FEATURES 0x00 19 struct msft_cp_read_supported_features { 20 __u8 sub_opcode; 21 } __packed; 22 23 struct msft_rp_read_supported_features { 24 __u8 status; 25 __u8 sub_opcode; 26 __le64 features; 27 __u8 evt_prefix_len; 28 __u8 evt_prefix[]; 29 } __packed; 30 31 #define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03 32 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01 33 struct msft_le_monitor_advertisement_pattern { 34 __u8 length; 35 __u8 data_type; 36 __u8 start_byte; 37 __u8 pattern[]; 38 }; 39 40 struct msft_le_monitor_advertisement_pattern_data { 41 __u8 count; 42 __u8 data[]; 43 }; 44 45 struct msft_cp_le_monitor_advertisement { 46 __u8 sub_opcode; 47 __s8 rssi_high; 48 __s8 rssi_low; 49 __u8 rssi_low_interval; 50 __u8 rssi_sampling_period; 51 __u8 cond_type; 52 __u8 data[]; 53 } __packed; 54 55 struct msft_rp_le_monitor_advertisement { 56 __u8 status; 57 __u8 sub_opcode; 58 __u8 handle; 59 } __packed; 60 61 #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04 62 struct msft_cp_le_cancel_monitor_advertisement { 63 __u8 sub_opcode; 64 __u8 handle; 65 } __packed; 66 67 struct msft_rp_le_cancel_monitor_advertisement { 68 __u8 status; 69 __u8 sub_opcode; 70 } __packed; 71 72 #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05 73 struct msft_cp_le_set_advertisement_filter_enable { 74 __u8 sub_opcode; 75 __u8 enable; 76 } __packed; 77 78 struct msft_rp_le_set_advertisement_filter_enable { 79 __u8 status; 80 __u8 sub_opcode; 81 } __packed; 82 83 #define MSFT_EV_LE_MONITOR_DEVICE 0x02 84 struct msft_ev_le_monitor_device { 85 __u8 addr_type; 86 bdaddr_t bdaddr; 87 __u8 monitor_handle; 88 __u8 monitor_state; 89 } __packed; 90 91 struct msft_monitor_advertisement_handle_data { 92 __u8 msft_handle; 93 __u16 mgmt_handle; 94 struct list_head list; 95 }; 96 97 struct msft_data { 98 __u64 features; 99 __u8 evt_prefix_len; 100 __u8 *evt_prefix; 101 struct list_head handle_map; 102 __u8 resuming; 103 __u8 suspending; 104 __u8 filter_enabled; 105 }; 106 107 bool msft_monitor_supported(struct hci_dev *hdev) 108 { 109 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR); 110 } 111 112 static bool read_supported_features(struct hci_dev *hdev, 113 struct msft_data *msft) 114 { 115 struct msft_cp_read_supported_features cp; 116 struct msft_rp_read_supported_features *rp; 117 struct sk_buff *skb; 118 119 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES; 120 121 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 122 HCI_CMD_TIMEOUT); 123 if (IS_ERR_OR_NULL(skb)) { 124 if (!skb) 125 skb = ERR_PTR(-EIO); 126 127 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)", 128 PTR_ERR(skb)); 129 return false; 130 } 131 132 if (skb->len < sizeof(*rp)) { 133 bt_dev_err(hdev, "MSFT supported features length mismatch"); 134 goto failed; 135 } 136 137 rp = (struct msft_rp_read_supported_features *)skb->data; 138 139 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES) 140 goto failed; 141 142 if (rp->evt_prefix_len > 0) { 143 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len, 144 GFP_KERNEL); 145 if (!msft->evt_prefix) 146 goto failed; 147 } 148 149 msft->evt_prefix_len = rp->evt_prefix_len; 150 msft->features = __le64_to_cpu(rp->features); 151 152 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY) 153 hdev->msft_curve_validity = true; 154 155 kfree_skb(skb); 156 return true; 157 158 failed: 159 kfree_skb(skb); 160 return false; 161 } 162 163 /* is_mgmt = true matches the handle exposed to userspace via mgmt. 164 * is_mgmt = false matches the handle used by the msft controller. 165 * This function requires the caller holds hdev->lock 166 */ 167 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data 168 (struct hci_dev *hdev, u16 handle, bool is_mgmt) 169 { 170 struct msft_monitor_advertisement_handle_data *entry; 171 struct msft_data *msft = hdev->msft_data; 172 173 list_for_each_entry(entry, &msft->handle_map, list) { 174 if (is_mgmt && entry->mgmt_handle == handle) 175 return entry; 176 if (!is_mgmt && entry->msft_handle == handle) 177 return entry; 178 } 179 180 return NULL; 181 } 182 183 /* This function requires the caller holds hdev->lock */ 184 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle, 185 bdaddr_t *bdaddr, __u8 addr_type, 186 bool notify) 187 { 188 struct monitored_device *dev, *tmp; 189 int count = 0; 190 191 list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) { 192 /* mgmt_handle == 0 indicates remove all devices, whereas, 193 * bdaddr == NULL indicates remove all devices matching the 194 * mgmt_handle. 195 */ 196 if ((!mgmt_handle || dev->handle == mgmt_handle) && 197 (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) && 198 addr_type == dev->addr_type))) { 199 if (notify && dev->notified) { 200 mgmt_adv_monitor_device_lost(hdev, dev->handle, 201 &dev->bdaddr, 202 dev->addr_type); 203 } 204 205 list_del(&dev->list); 206 kfree(dev); 207 count++; 208 } 209 } 210 211 return count; 212 } 213 214 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode, 215 struct adv_monitor *monitor, 216 struct sk_buff *skb) 217 { 218 struct msft_rp_le_monitor_advertisement *rp; 219 struct msft_monitor_advertisement_handle_data *handle_data; 220 struct msft_data *msft = hdev->msft_data; 221 int status = 0; 222 223 hci_dev_lock(hdev); 224 225 rp = (struct msft_rp_le_monitor_advertisement *)skb->data; 226 if (skb->len < sizeof(*rp)) { 227 status = HCI_ERROR_UNSPECIFIED; 228 goto unlock; 229 } 230 231 status = rp->status; 232 if (status) 233 goto unlock; 234 235 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL); 236 if (!handle_data) { 237 status = HCI_ERROR_UNSPECIFIED; 238 goto unlock; 239 } 240 241 handle_data->mgmt_handle = monitor->handle; 242 handle_data->msft_handle = rp->handle; 243 INIT_LIST_HEAD(&handle_data->list); 244 list_add(&handle_data->list, &msft->handle_map); 245 246 monitor->state = ADV_MONITOR_STATE_OFFLOADED; 247 248 unlock: 249 if (status) 250 hci_free_adv_monitor(hdev, monitor); 251 252 hci_dev_unlock(hdev); 253 254 return status; 255 } 256 257 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, 258 u16 opcode, 259 struct adv_monitor *monitor, 260 struct sk_buff *skb) 261 { 262 struct msft_rp_le_cancel_monitor_advertisement *rp; 263 struct msft_monitor_advertisement_handle_data *handle_data; 264 struct msft_data *msft = hdev->msft_data; 265 int status = 0; 266 267 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data; 268 if (skb->len < sizeof(*rp)) { 269 status = HCI_ERROR_UNSPECIFIED; 270 goto done; 271 } 272 273 status = rp->status; 274 if (status) 275 goto done; 276 277 hci_dev_lock(hdev); 278 279 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 280 281 if (handle_data) { 282 if (monitor->state == ADV_MONITOR_STATE_OFFLOADED) 283 monitor->state = ADV_MONITOR_STATE_REGISTERED; 284 285 /* Do not free the monitor if it is being removed due to 286 * suspend. It will be re-monitored on resume. 287 */ 288 if (!msft->suspending) { 289 hci_free_adv_monitor(hdev, monitor); 290 291 /* Clear any monitored devices by this Adv Monitor */ 292 msft_monitor_device_del(hdev, handle_data->mgmt_handle, 293 NULL, 0, false); 294 } 295 296 list_del(&handle_data->list); 297 kfree(handle_data); 298 } 299 300 hci_dev_unlock(hdev); 301 302 done: 303 return status; 304 } 305 306 /* This function requires the caller holds hci_req_sync_lock */ 307 static int msft_remove_monitor_sync(struct hci_dev *hdev, 308 struct adv_monitor *monitor) 309 { 310 struct msft_cp_le_cancel_monitor_advertisement cp; 311 struct msft_monitor_advertisement_handle_data *handle_data; 312 struct sk_buff *skb; 313 314 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 315 316 /* If no matched handle, just remove without telling controller */ 317 if (!handle_data) 318 return -ENOENT; 319 320 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 321 cp.handle = handle_data->msft_handle; 322 323 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 324 HCI_CMD_TIMEOUT); 325 if (IS_ERR_OR_NULL(skb)) { 326 if (!skb) 327 return -EIO; 328 return PTR_ERR(skb); 329 } 330 331 return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode, 332 monitor, skb); 333 } 334 335 /* This function requires the caller holds hci_req_sync_lock */ 336 int msft_suspend_sync(struct hci_dev *hdev) 337 { 338 struct msft_data *msft = hdev->msft_data; 339 struct adv_monitor *monitor; 340 int handle = 0; 341 342 if (!msft || !msft_monitor_supported(hdev)) 343 return 0; 344 345 msft->suspending = true; 346 347 while (1) { 348 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 349 if (!monitor) 350 break; 351 352 msft_remove_monitor_sync(hdev, monitor); 353 354 handle++; 355 } 356 357 /* All monitors have been removed */ 358 msft->suspending = false; 359 360 return 0; 361 } 362 363 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor) 364 { 365 struct adv_rssi_thresholds *r = &monitor->rssi; 366 367 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 368 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX || 369 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 370 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX) 371 return false; 372 373 /* High_threshold_timeout is not supported, 374 * once high_threshold is reached, events are immediately reported. 375 */ 376 if (r->high_threshold_timeout != 0) 377 return false; 378 379 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX) 380 return false; 381 382 /* Sampling period from 0x00 to 0xFF are all allowed */ 383 return true; 384 } 385 386 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor) 387 { 388 return msft_monitor_rssi_valid(monitor); 389 /* No additional check needed for pattern-based monitor */ 390 } 391 392 static int msft_add_monitor_sync(struct hci_dev *hdev, 393 struct adv_monitor *monitor) 394 { 395 struct msft_cp_le_monitor_advertisement *cp; 396 struct msft_le_monitor_advertisement_pattern_data *pattern_data; 397 struct msft_le_monitor_advertisement_pattern *pattern; 398 struct adv_pattern *entry; 399 size_t total_size = sizeof(*cp) + sizeof(*pattern_data); 400 ptrdiff_t offset = 0; 401 u8 pattern_count = 0; 402 struct sk_buff *skb; 403 404 if (!msft_monitor_pattern_valid(monitor)) 405 return -EINVAL; 406 407 list_for_each_entry(entry, &monitor->patterns, list) { 408 pattern_count++; 409 total_size += sizeof(*pattern) + entry->length; 410 } 411 412 cp = kmalloc(total_size, GFP_KERNEL); 413 if (!cp) 414 return -ENOMEM; 415 416 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 417 cp->rssi_high = monitor->rssi.high_threshold; 418 cp->rssi_low = monitor->rssi.low_threshold; 419 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; 420 cp->rssi_sampling_period = monitor->rssi.sampling_period; 421 422 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 423 424 pattern_data = (void *)cp->data; 425 pattern_data->count = pattern_count; 426 427 list_for_each_entry(entry, &monitor->patterns, list) { 428 pattern = (void *)(pattern_data->data + offset); 429 /* the length also includes data_type and offset */ 430 pattern->length = entry->length + 2; 431 pattern->data_type = entry->ad_type; 432 pattern->start_byte = entry->offset; 433 memcpy(pattern->pattern, entry->value, entry->length); 434 offset += sizeof(*pattern) + entry->length; 435 } 436 437 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp, 438 HCI_CMD_TIMEOUT); 439 kfree(cp); 440 441 if (IS_ERR_OR_NULL(skb)) { 442 if (!skb) 443 return -EIO; 444 return PTR_ERR(skb); 445 } 446 447 return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode, 448 monitor, skb); 449 } 450 451 /* This function requires the caller holds hci_req_sync_lock */ 452 static void reregister_monitor(struct hci_dev *hdev) 453 { 454 struct adv_monitor *monitor; 455 struct msft_data *msft = hdev->msft_data; 456 int handle = 0; 457 458 if (!msft) 459 return; 460 461 msft->resuming = true; 462 463 while (1) { 464 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 465 if (!monitor) 466 break; 467 468 msft_add_monitor_sync(hdev, monitor); 469 470 handle++; 471 } 472 473 /* All monitors have been reregistered */ 474 msft->resuming = false; 475 } 476 477 /* This function requires the caller holds hci_req_sync_lock */ 478 int msft_resume_sync(struct hci_dev *hdev) 479 { 480 struct msft_data *msft = hdev->msft_data; 481 482 if (!msft || !msft_monitor_supported(hdev)) 483 return 0; 484 485 hci_dev_lock(hdev); 486 487 /* Clear already tracked devices on resume. Once the monitors are 488 * reregistered, devices in range will be found again after resume. 489 */ 490 hdev->advmon_pend_notify = false; 491 msft_monitor_device_del(hdev, 0, NULL, 0, true); 492 493 hci_dev_unlock(hdev); 494 495 reregister_monitor(hdev); 496 497 return 0; 498 } 499 500 /* This function requires the caller holds hci_req_sync_lock */ 501 void msft_do_open(struct hci_dev *hdev) 502 { 503 struct msft_data *msft = hdev->msft_data; 504 505 if (hdev->msft_opcode == HCI_OP_NOP) 506 return; 507 508 if (!msft) { 509 bt_dev_err(hdev, "MSFT extension not registered"); 510 return; 511 } 512 513 bt_dev_dbg(hdev, "Initialize MSFT extension"); 514 515 /* Reset existing MSFT data before re-reading */ 516 kfree(msft->evt_prefix); 517 msft->evt_prefix = NULL; 518 msft->evt_prefix_len = 0; 519 msft->features = 0; 520 521 if (!read_supported_features(hdev, msft)) { 522 hdev->msft_data = NULL; 523 kfree(msft); 524 return; 525 } 526 527 if (msft_monitor_supported(hdev)) { 528 msft->resuming = true; 529 msft_set_filter_enable(hdev, true); 530 /* Monitors get removed on power off, so we need to explicitly 531 * tell the controller to re-monitor. 532 */ 533 reregister_monitor(hdev); 534 } 535 } 536 537 void msft_do_close(struct hci_dev *hdev) 538 { 539 struct msft_data *msft = hdev->msft_data; 540 struct msft_monitor_advertisement_handle_data *handle_data, *tmp; 541 struct adv_monitor *monitor; 542 543 if (!msft) 544 return; 545 546 bt_dev_dbg(hdev, "Cleanup of MSFT extension"); 547 548 /* The controller will silently remove all monitors on power off. 549 * Therefore, remove handle_data mapping and reset monitor state. 550 */ 551 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) { 552 monitor = idr_find(&hdev->adv_monitors_idr, 553 handle_data->mgmt_handle); 554 555 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED) 556 monitor->state = ADV_MONITOR_STATE_REGISTERED; 557 558 list_del(&handle_data->list); 559 kfree(handle_data); 560 } 561 562 hci_dev_lock(hdev); 563 564 /* Clear any devices that are being monitored and notify device lost */ 565 hdev->advmon_pend_notify = false; 566 msft_monitor_device_del(hdev, 0, NULL, 0, true); 567 568 hci_dev_unlock(hdev); 569 } 570 571 void msft_register(struct hci_dev *hdev) 572 { 573 struct msft_data *msft = NULL; 574 575 bt_dev_dbg(hdev, "Register MSFT extension"); 576 577 msft = kzalloc(sizeof(*msft), GFP_KERNEL); 578 if (!msft) { 579 bt_dev_err(hdev, "Failed to register MSFT extension"); 580 return; 581 } 582 583 INIT_LIST_HEAD(&msft->handle_map); 584 hdev->msft_data = msft; 585 } 586 587 void msft_unregister(struct hci_dev *hdev) 588 { 589 struct msft_data *msft = hdev->msft_data; 590 591 if (!msft) 592 return; 593 594 bt_dev_dbg(hdev, "Unregister MSFT extension"); 595 596 hdev->msft_data = NULL; 597 598 kfree(msft->evt_prefix); 599 kfree(msft); 600 } 601 602 /* This function requires the caller holds hdev->lock */ 603 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, 604 __u8 addr_type, __u16 mgmt_handle) 605 { 606 struct monitored_device *dev; 607 608 dev = kmalloc(sizeof(*dev), GFP_KERNEL); 609 if (!dev) { 610 bt_dev_err(hdev, "MSFT vendor event %u: no memory", 611 MSFT_EV_LE_MONITOR_DEVICE); 612 return; 613 } 614 615 bacpy(&dev->bdaddr, bdaddr); 616 dev->addr_type = addr_type; 617 dev->handle = mgmt_handle; 618 dev->notified = false; 619 620 INIT_LIST_HEAD(&dev->list); 621 list_add(&dev->list, &hdev->monitored_devices); 622 hdev->advmon_pend_notify = true; 623 } 624 625 /* This function requires the caller holds hdev->lock */ 626 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr, 627 __u8 addr_type, __u16 mgmt_handle) 628 { 629 if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type, 630 true)) { 631 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list", 632 MSFT_EV_LE_MONITOR_DEVICE, bdaddr); 633 } 634 } 635 636 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, 637 u8 ev, size_t len) 638 { 639 void *data; 640 641 data = skb_pull_data(skb, len); 642 if (!data) 643 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev); 644 645 return data; 646 } 647 648 /* This function requires the caller holds hdev->lock */ 649 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb) 650 { 651 struct msft_ev_le_monitor_device *ev; 652 struct msft_monitor_advertisement_handle_data *handle_data; 653 u8 addr_type; 654 655 ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev)); 656 if (!ev) 657 return; 658 659 bt_dev_dbg(hdev, 660 "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR", 661 MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle, 662 ev->monitor_state, &ev->bdaddr); 663 664 handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false); 665 if (!handle_data) 666 return; 667 668 switch (ev->addr_type) { 669 case ADDR_LE_DEV_PUBLIC: 670 addr_type = BDADDR_LE_PUBLIC; 671 break; 672 673 case ADDR_LE_DEV_RANDOM: 674 addr_type = BDADDR_LE_RANDOM; 675 break; 676 677 default: 678 bt_dev_err(hdev, 679 "MSFT vendor event 0x%02x: unknown addr type 0x%02x", 680 MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type); 681 return; 682 } 683 684 if (ev->monitor_state) 685 msft_device_found(hdev, &ev->bdaddr, addr_type, 686 handle_data->mgmt_handle); 687 else 688 msft_device_lost(hdev, &ev->bdaddr, addr_type, 689 handle_data->mgmt_handle); 690 } 691 692 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) 693 { 694 struct msft_data *msft = hdev->msft_data; 695 u8 *evt_prefix; 696 u8 *evt; 697 698 if (!msft) 699 return; 700 701 /* When the extension has defined an event prefix, check that it 702 * matches, and otherwise just return. 703 */ 704 if (msft->evt_prefix_len > 0) { 705 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len); 706 if (!evt_prefix) 707 return; 708 709 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len)) 710 return; 711 } 712 713 /* Every event starts at least with an event code and the rest of 714 * the data is variable and depends on the event code. 715 */ 716 if (skb->len < 1) 717 return; 718 719 evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt)); 720 if (!evt) 721 return; 722 723 hci_dev_lock(hdev); 724 725 switch (*evt) { 726 case MSFT_EV_LE_MONITOR_DEVICE: 727 msft_monitor_device_evt(hdev, skb); 728 break; 729 730 default: 731 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt); 732 break; 733 } 734 735 hci_dev_unlock(hdev); 736 } 737 738 __u64 msft_get_features(struct hci_dev *hdev) 739 { 740 struct msft_data *msft = hdev->msft_data; 741 742 return msft ? msft->features : 0; 743 } 744 745 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, 746 u8 status, u16 opcode, 747 struct sk_buff *skb) 748 { 749 struct msft_cp_le_set_advertisement_filter_enable *cp; 750 struct msft_rp_le_set_advertisement_filter_enable *rp; 751 struct msft_data *msft = hdev->msft_data; 752 753 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data; 754 if (skb->len < sizeof(*rp)) 755 return; 756 757 /* Error 0x0C would be returned if the filter enabled status is 758 * already set to whatever we were trying to set. 759 * Although the default state should be disabled, some controller set 760 * the initial value to enabled. Because there is no way to know the 761 * actual initial value before sending this command, here we also treat 762 * error 0x0C as success. 763 */ 764 if (status != 0x00 && status != 0x0C) 765 return; 766 767 hci_dev_lock(hdev); 768 769 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); 770 msft->filter_enabled = cp->enable; 771 772 if (status == 0x0C) 773 bt_dev_warn(hdev, "MSFT filter_enable is already %s", 774 cp->enable ? "on" : "off"); 775 776 hci_dev_unlock(hdev); 777 } 778 779 /* This function requires the caller holds hci_req_sync_lock */ 780 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) 781 { 782 struct msft_data *msft = hdev->msft_data; 783 784 if (!msft) 785 return -EOPNOTSUPP; 786 787 if (msft->resuming || msft->suspending) 788 return -EBUSY; 789 790 return msft_add_monitor_sync(hdev, monitor); 791 } 792 793 /* This function requires the caller holds hci_req_sync_lock */ 794 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor) 795 { 796 struct msft_data *msft = hdev->msft_data; 797 798 if (!msft) 799 return -EOPNOTSUPP; 800 801 if (msft->resuming || msft->suspending) 802 return -EBUSY; 803 804 return msft_remove_monitor_sync(hdev, monitor); 805 } 806 807 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable) 808 { 809 struct hci_dev *hdev = req->hdev; 810 struct msft_cp_le_set_advertisement_filter_enable cp; 811 812 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE; 813 cp.enable = enable; 814 815 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp); 816 } 817 818 int msft_set_filter_enable(struct hci_dev *hdev, bool enable) 819 { 820 struct hci_request req; 821 struct msft_data *msft = hdev->msft_data; 822 int err; 823 824 if (!msft) 825 return -EOPNOTSUPP; 826 827 hci_req_init(&req, hdev); 828 msft_req_add_set_filter_enable(&req, enable); 829 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb); 830 831 return err; 832 } 833 834 bool msft_curve_validity(struct hci_dev *hdev) 835 { 836 return hdev->msft_curve_validity; 837 } 838