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