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