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 "mgmt_util.h" 11 #include "msft.h" 12 13 #define MSFT_RSSI_THRESHOLD_VALUE_MIN -127 14 #define MSFT_RSSI_THRESHOLD_VALUE_MAX 20 15 #define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C 16 17 #define MSFT_OP_READ_SUPPORTED_FEATURES 0x00 18 struct msft_cp_read_supported_features { 19 __u8 sub_opcode; 20 } __packed; 21 22 struct msft_rp_read_supported_features { 23 __u8 status; 24 __u8 sub_opcode; 25 __le64 features; 26 __u8 evt_prefix_len; 27 __u8 evt_prefix[]; 28 } __packed; 29 30 #define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03 31 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01 32 struct msft_le_monitor_advertisement_pattern { 33 __u8 length; 34 __u8 data_type; 35 __u8 start_byte; 36 __u8 pattern[]; 37 }; 38 39 struct msft_le_monitor_advertisement_pattern_data { 40 __u8 count; 41 __u8 data[]; 42 }; 43 44 struct msft_cp_le_monitor_advertisement { 45 __u8 sub_opcode; 46 __s8 rssi_high; 47 __s8 rssi_low; 48 __u8 rssi_low_interval; 49 __u8 rssi_sampling_period; 50 __u8 cond_type; 51 __u8 data[]; 52 } __packed; 53 54 struct msft_rp_le_monitor_advertisement { 55 __u8 status; 56 __u8 sub_opcode; 57 __u8 handle; 58 } __packed; 59 60 #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04 61 struct msft_cp_le_cancel_monitor_advertisement { 62 __u8 sub_opcode; 63 __u8 handle; 64 } __packed; 65 66 struct msft_rp_le_cancel_monitor_advertisement { 67 __u8 status; 68 __u8 sub_opcode; 69 } __packed; 70 71 #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05 72 struct msft_cp_le_set_advertisement_filter_enable { 73 __u8 sub_opcode; 74 __u8 enable; 75 } __packed; 76 77 struct msft_rp_le_set_advertisement_filter_enable { 78 __u8 status; 79 __u8 sub_opcode; 80 } __packed; 81 82 #define MSFT_EV_LE_MONITOR_DEVICE 0x02 83 struct msft_ev_le_monitor_device { 84 __u8 addr_type; 85 bdaddr_t bdaddr; 86 __u8 monitor_handle; 87 __u8 monitor_state; 88 } __packed; 89 90 struct msft_monitor_advertisement_handle_data { 91 __u8 msft_handle; 92 __u16 mgmt_handle; 93 __s8 rssi_high; 94 __s8 rssi_low; 95 __u8 rssi_low_interval; 96 __u8 rssi_sampling_period; 97 __u8 cond_type; 98 struct list_head list; 99 }; 100 101 enum monitor_addr_filter_state { 102 AF_STATE_IDLE, 103 AF_STATE_ADDING, 104 AF_STATE_ADDED, 105 AF_STATE_REMOVING, 106 }; 107 108 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR 0x04 109 struct msft_monitor_addr_filter_data { 110 __u8 msft_handle; 111 __u8 pattern_handle; /* address filters pertain to */ 112 __u16 mgmt_handle; 113 int state; 114 __s8 rssi_high; 115 __s8 rssi_low; 116 __u8 rssi_low_interval; 117 __u8 rssi_sampling_period; 118 __u8 addr_type; 119 bdaddr_t bdaddr; 120 struct list_head list; 121 }; 122 123 struct msft_data { 124 __u64 features; 125 __u8 evt_prefix_len; 126 __u8 *evt_prefix; 127 struct list_head handle_map; 128 struct list_head address_filters; 129 __u8 resuming; 130 __u8 suspending; 131 __u8 filter_enabled; 132 /* To synchronize add/remove address filter and monitor device event.*/ 133 struct mutex filter_lock; 134 }; 135 136 bool msft_monitor_supported(struct hci_dev *hdev) 137 { 138 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR); 139 } 140 141 static bool read_supported_features(struct hci_dev *hdev, 142 struct msft_data *msft) 143 { 144 struct msft_cp_read_supported_features cp; 145 struct msft_rp_read_supported_features *rp; 146 struct sk_buff *skb; 147 148 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES; 149 150 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 151 HCI_CMD_TIMEOUT); 152 if (IS_ERR(skb)) { 153 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)", 154 PTR_ERR(skb)); 155 return false; 156 } 157 158 if (skb->len < sizeof(*rp)) { 159 bt_dev_err(hdev, "MSFT supported features length mismatch"); 160 goto failed; 161 } 162 163 rp = (struct msft_rp_read_supported_features *)skb->data; 164 165 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES) 166 goto failed; 167 168 if (skb->len < sizeof(*rp) + rp->evt_prefix_len) { 169 bt_dev_err(hdev, "MSFT event prefix length mismatch"); 170 goto failed; 171 } 172 173 if (rp->evt_prefix_len > 0) { 174 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len, 175 GFP_KERNEL); 176 if (!msft->evt_prefix) 177 goto failed; 178 } 179 180 msft->evt_prefix_len = rp->evt_prefix_len; 181 msft->features = __le64_to_cpu(rp->features); 182 183 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY) 184 hdev->msft_curve_validity = true; 185 186 kfree_skb(skb); 187 return true; 188 189 failed: 190 kfree_skb(skb); 191 return false; 192 } 193 194 /* is_mgmt = true matches the handle exposed to userspace via mgmt. 195 * is_mgmt = false matches the handle used by the msft controller. 196 * This function requires the caller holds hdev->lock 197 */ 198 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data 199 (struct hci_dev *hdev, u16 handle, bool is_mgmt) 200 { 201 struct msft_monitor_advertisement_handle_data *entry; 202 struct msft_data *msft = hdev->msft_data; 203 204 list_for_each_entry(entry, &msft->handle_map, list) { 205 if (is_mgmt && entry->mgmt_handle == handle) 206 return entry; 207 if (!is_mgmt && entry->msft_handle == handle) 208 return entry; 209 } 210 211 return NULL; 212 } 213 214 /* This function requires the caller holds msft->filter_lock */ 215 static struct msft_monitor_addr_filter_data *msft_find_address_data 216 (struct hci_dev *hdev, u8 addr_type, bdaddr_t *addr, 217 u8 pattern_handle) 218 { 219 struct msft_monitor_addr_filter_data *entry; 220 struct msft_data *msft = hdev->msft_data; 221 222 list_for_each_entry(entry, &msft->address_filters, list) { 223 if (entry->pattern_handle == pattern_handle && 224 addr_type == entry->addr_type && 225 !bacmp(addr, &entry->bdaddr)) 226 return entry; 227 } 228 229 return NULL; 230 } 231 232 /* This function requires the caller holds hdev->lock */ 233 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle, 234 bdaddr_t *bdaddr, __u8 addr_type, 235 bool notify) 236 { 237 struct monitored_device *dev, *tmp; 238 int count = 0; 239 240 list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) { 241 /* mgmt_handle == 0 indicates remove all devices, whereas, 242 * bdaddr == NULL indicates remove all devices matching the 243 * mgmt_handle. 244 */ 245 if ((!mgmt_handle || dev->handle == mgmt_handle) && 246 (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) && 247 addr_type == dev->addr_type))) { 248 if (notify && dev->notified) { 249 mgmt_adv_monitor_device_lost(hdev, dev->handle, 250 &dev->bdaddr, 251 dev->addr_type); 252 } 253 254 list_del(&dev->list); 255 kfree(dev); 256 count++; 257 } 258 } 259 260 return count; 261 } 262 263 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode, 264 struct adv_monitor *monitor, 265 struct sk_buff *skb) 266 { 267 struct msft_rp_le_monitor_advertisement *rp; 268 struct msft_monitor_advertisement_handle_data *handle_data; 269 struct msft_data *msft = hdev->msft_data; 270 int status = 0; 271 272 hci_dev_lock(hdev); 273 274 rp = (struct msft_rp_le_monitor_advertisement *)skb->data; 275 if (skb->len < sizeof(*rp)) { 276 status = HCI_ERROR_UNSPECIFIED; 277 goto unlock; 278 } 279 280 status = rp->status; 281 if (status) 282 goto unlock; 283 284 handle_data = kmalloc_obj(*handle_data); 285 if (!handle_data) { 286 status = HCI_ERROR_UNSPECIFIED; 287 goto unlock; 288 } 289 290 handle_data->mgmt_handle = monitor->handle; 291 handle_data->msft_handle = rp->handle; 292 handle_data->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 293 INIT_LIST_HEAD(&handle_data->list); 294 list_add(&handle_data->list, &msft->handle_map); 295 296 monitor->state = ADV_MONITOR_STATE_OFFLOADED; 297 298 unlock: 299 if (status && msft->resuming) 300 hci_free_adv_monitor(hdev, monitor); 301 302 hci_dev_unlock(hdev); 303 304 return status; 305 } 306 307 /* This function requires the caller holds hci_req_sync_lock */ 308 static void msft_remove_addr_filters_sync(struct hci_dev *hdev, u8 handle) 309 { 310 struct msft_monitor_addr_filter_data *address_filter, *n; 311 struct msft_cp_le_cancel_monitor_advertisement cp; 312 struct msft_data *msft = hdev->msft_data; 313 struct list_head head; 314 struct sk_buff *skb; 315 316 INIT_LIST_HEAD(&head); 317 318 /* Cancel all corresponding address monitors */ 319 mutex_lock(&msft->filter_lock); 320 321 list_for_each_entry_safe(address_filter, n, &msft->address_filters, 322 list) { 323 if (address_filter->pattern_handle != handle) 324 continue; 325 326 list_del(&address_filter->list); 327 328 /* Keep the address filter and let 329 * msft_add_address_filter_sync() remove and free the address 330 * filter. 331 */ 332 if (address_filter->state == AF_STATE_ADDING) { 333 address_filter->state = AF_STATE_REMOVING; 334 continue; 335 } 336 337 /* Keep the address filter and let 338 * msft_cancel_address_filter_sync() remove and free the address 339 * filter 340 */ 341 if (address_filter->state == AF_STATE_REMOVING) 342 continue; 343 344 list_add_tail(&address_filter->list, &head); 345 } 346 347 mutex_unlock(&msft->filter_lock); 348 349 list_for_each_entry_safe(address_filter, n, &head, list) { 350 list_del(&address_filter->list); 351 352 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 353 cp.handle = address_filter->msft_handle; 354 355 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 356 HCI_CMD_TIMEOUT); 357 if (IS_ERR(skb)) { 358 kfree(address_filter); 359 continue; 360 } 361 362 kfree_skb(skb); 363 364 bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter", 365 &address_filter->bdaddr); 366 367 kfree(address_filter); 368 } 369 } 370 371 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, 372 u16 opcode, 373 struct adv_monitor *monitor, 374 struct sk_buff *skb) 375 { 376 struct msft_rp_le_cancel_monitor_advertisement *rp; 377 struct msft_monitor_advertisement_handle_data *handle_data; 378 struct msft_data *msft = hdev->msft_data; 379 int status = 0; 380 u8 msft_handle; 381 382 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data; 383 if (skb->len < sizeof(*rp)) { 384 status = HCI_ERROR_UNSPECIFIED; 385 goto done; 386 } 387 388 status = rp->status; 389 if (status) 390 goto done; 391 392 hci_dev_lock(hdev); 393 394 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 395 396 if (handle_data) { 397 if (monitor->state == ADV_MONITOR_STATE_OFFLOADED) 398 monitor->state = ADV_MONITOR_STATE_REGISTERED; 399 400 /* Do not free the monitor if it is being removed due to 401 * suspend. It will be re-monitored on resume. 402 */ 403 if (!msft->suspending) { 404 hci_free_adv_monitor(hdev, monitor); 405 406 /* Clear any monitored devices by this Adv Monitor */ 407 msft_monitor_device_del(hdev, handle_data->mgmt_handle, 408 NULL, 0, false); 409 } 410 411 msft_handle = handle_data->msft_handle; 412 413 list_del(&handle_data->list); 414 kfree(handle_data); 415 416 hci_dev_unlock(hdev); 417 418 msft_remove_addr_filters_sync(hdev, msft_handle); 419 } else { 420 hci_dev_unlock(hdev); 421 } 422 423 done: 424 return status; 425 } 426 427 /* This function requires the caller holds hci_req_sync_lock */ 428 static int msft_remove_monitor_sync(struct hci_dev *hdev, 429 struct adv_monitor *monitor) 430 { 431 struct msft_cp_le_cancel_monitor_advertisement cp; 432 struct msft_monitor_advertisement_handle_data *handle_data; 433 struct sk_buff *skb; 434 435 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 436 437 /* If no matched handle, just remove without telling controller */ 438 if (!handle_data) 439 return -ENOENT; 440 441 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 442 cp.handle = handle_data->msft_handle; 443 444 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 445 HCI_CMD_TIMEOUT); 446 if (IS_ERR(skb)) 447 return PTR_ERR(skb); 448 449 return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode, 450 monitor, skb); 451 } 452 453 /* This function requires the caller holds hci_req_sync_lock */ 454 int msft_suspend_sync(struct hci_dev *hdev) 455 { 456 struct msft_data *msft = hdev->msft_data; 457 struct adv_monitor *monitor; 458 int handle = 0; 459 460 if (!msft || !msft_monitor_supported(hdev)) 461 return 0; 462 463 msft->suspending = true; 464 465 while (1) { 466 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 467 if (!monitor) 468 break; 469 470 msft_remove_monitor_sync(hdev, monitor); 471 472 handle++; 473 } 474 475 /* All monitors have been removed */ 476 msft->suspending = false; 477 478 return 0; 479 } 480 481 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor) 482 { 483 struct adv_rssi_thresholds *r = &monitor->rssi; 484 485 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 486 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX || 487 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 488 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX) 489 return false; 490 491 /* High_threshold_timeout is not supported, 492 * once high_threshold is reached, events are immediately reported. 493 */ 494 if (r->high_threshold_timeout != 0) 495 return false; 496 497 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX) 498 return false; 499 500 /* Sampling period from 0x00 to 0xFF are all allowed */ 501 return true; 502 } 503 504 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor) 505 { 506 return msft_monitor_rssi_valid(monitor); 507 /* No additional check needed for pattern-based monitor */ 508 } 509 510 static int msft_add_monitor_sync(struct hci_dev *hdev, 511 struct adv_monitor *monitor) 512 { 513 struct msft_cp_le_monitor_advertisement *cp; 514 struct msft_le_monitor_advertisement_pattern_data *pattern_data; 515 struct msft_monitor_advertisement_handle_data *handle_data; 516 struct msft_le_monitor_advertisement_pattern *pattern; 517 struct adv_pattern *entry; 518 size_t total_size = sizeof(*cp) + sizeof(*pattern_data); 519 ptrdiff_t offset = 0; 520 u8 pattern_count = 0; 521 struct sk_buff *skb; 522 int err; 523 524 if (!msft_monitor_pattern_valid(monitor)) 525 return -EINVAL; 526 527 list_for_each_entry(entry, &monitor->patterns, list) { 528 pattern_count++; 529 total_size += sizeof(*pattern) + entry->length; 530 } 531 532 cp = kmalloc(total_size, GFP_KERNEL); 533 if (!cp) 534 return -ENOMEM; 535 536 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 537 cp->rssi_high = monitor->rssi.high_threshold; 538 cp->rssi_low = monitor->rssi.low_threshold; 539 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; 540 cp->rssi_sampling_period = monitor->rssi.sampling_period; 541 542 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 543 544 pattern_data = (void *)cp->data; 545 pattern_data->count = pattern_count; 546 547 list_for_each_entry(entry, &monitor->patterns, list) { 548 pattern = (void *)(pattern_data->data + offset); 549 /* the length also includes data_type and offset */ 550 pattern->length = entry->length + 2; 551 pattern->data_type = entry->ad_type; 552 pattern->start_byte = entry->offset; 553 memcpy(pattern->pattern, entry->value, entry->length); 554 offset += sizeof(*pattern) + entry->length; 555 } 556 557 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp, 558 HCI_CMD_TIMEOUT); 559 560 if (IS_ERR(skb)) { 561 err = PTR_ERR(skb); 562 goto out_free; 563 } 564 565 err = msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode, 566 monitor, skb); 567 if (err) 568 goto out_free; 569 570 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 571 if (!handle_data) { 572 err = -ENODATA; 573 goto out_free; 574 } 575 576 handle_data->rssi_high = cp->rssi_high; 577 handle_data->rssi_low = cp->rssi_low; 578 handle_data->rssi_low_interval = cp->rssi_low_interval; 579 handle_data->rssi_sampling_period = cp->rssi_sampling_period; 580 581 out_free: 582 kfree(cp); 583 return err; 584 } 585 586 /* This function requires the caller holds hci_req_sync_lock */ 587 static void reregister_monitor(struct hci_dev *hdev) 588 { 589 struct adv_monitor *monitor; 590 struct msft_data *msft = hdev->msft_data; 591 int handle = 0; 592 593 if (!msft) 594 return; 595 596 msft->resuming = true; 597 598 while (1) { 599 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 600 if (!monitor) 601 break; 602 603 msft_add_monitor_sync(hdev, monitor); 604 605 handle++; 606 } 607 608 /* All monitors have been reregistered */ 609 msft->resuming = false; 610 } 611 612 /* This function requires the caller holds hci_req_sync_lock */ 613 int msft_resume_sync(struct hci_dev *hdev) 614 { 615 struct msft_data *msft = hdev->msft_data; 616 617 if (!msft || !msft_monitor_supported(hdev)) 618 return 0; 619 620 hci_dev_lock(hdev); 621 622 /* Clear already tracked devices on resume. Once the monitors are 623 * reregistered, devices in range will be found again after resume. 624 */ 625 hdev->advmon_pend_notify = false; 626 msft_monitor_device_del(hdev, 0, NULL, 0, true); 627 628 hci_dev_unlock(hdev); 629 630 reregister_monitor(hdev); 631 632 return 0; 633 } 634 635 /* This function requires the caller holds hci_req_sync_lock */ 636 void msft_do_open(struct hci_dev *hdev) 637 { 638 struct msft_data *msft = hdev->msft_data; 639 640 if (hdev->msft_opcode == HCI_OP_NOP) 641 return; 642 643 if (!msft) { 644 bt_dev_err(hdev, "MSFT extension not registered"); 645 return; 646 } 647 648 bt_dev_dbg(hdev, "Initialize MSFT extension"); 649 650 /* Reset existing MSFT data before re-reading */ 651 kfree(msft->evt_prefix); 652 msft->evt_prefix = NULL; 653 msft->evt_prefix_len = 0; 654 msft->features = 0; 655 656 if (!read_supported_features(hdev, msft)) { 657 hci_dev_lock(hdev); 658 hdev->msft_data = NULL; 659 kfree(msft); 660 hci_dev_unlock(hdev); 661 return; 662 } 663 664 if (msft_monitor_supported(hdev)) { 665 msft->resuming = true; 666 msft_set_filter_enable(hdev, true); 667 /* Monitors get removed on power off, so we need to explicitly 668 * tell the controller to re-monitor. 669 */ 670 reregister_monitor(hdev); 671 } 672 } 673 674 void msft_do_close(struct hci_dev *hdev) 675 { 676 struct msft_data *msft = hdev->msft_data; 677 struct msft_monitor_advertisement_handle_data *handle_data, *tmp; 678 struct msft_monitor_addr_filter_data *address_filter, *n; 679 struct adv_monitor *monitor; 680 681 if (!msft) 682 return; 683 684 bt_dev_dbg(hdev, "Cleanup of MSFT extension"); 685 686 hci_dev_lock(hdev); 687 688 /* The controller will silently remove all monitors on power off. 689 * Therefore, remove handle_data mapping and reset monitor state. 690 */ 691 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) { 692 monitor = idr_find(&hdev->adv_monitors_idr, 693 handle_data->mgmt_handle); 694 695 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED) 696 monitor->state = ADV_MONITOR_STATE_REGISTERED; 697 698 list_del(&handle_data->list); 699 kfree(handle_data); 700 } 701 702 hci_dev_unlock(hdev); 703 704 mutex_lock(&msft->filter_lock); 705 list_for_each_entry_safe(address_filter, n, &msft->address_filters, 706 list) { 707 list_del(&address_filter->list); 708 kfree(address_filter); 709 } 710 mutex_unlock(&msft->filter_lock); 711 712 hci_dev_lock(hdev); 713 714 /* Clear any devices that are being monitored and notify device lost */ 715 hdev->advmon_pend_notify = false; 716 msft_monitor_device_del(hdev, 0, NULL, 0, true); 717 718 hci_dev_unlock(hdev); 719 } 720 721 static int msft_cancel_address_filter_sync(struct hci_dev *hdev, void *data) 722 { 723 struct msft_monitor_addr_filter_data *address_filter = data; 724 struct msft_cp_le_cancel_monitor_advertisement cp; 725 struct msft_data *msft = hdev->msft_data; 726 struct sk_buff *skb; 727 int err = 0; 728 729 if (!msft) { 730 bt_dev_err(hdev, "MSFT: msft data is freed"); 731 return -EINVAL; 732 } 733 734 /* The address filter has been removed by hci dev close */ 735 if (!test_bit(HCI_UP, &hdev->flags)) 736 return 0; 737 738 mutex_lock(&msft->filter_lock); 739 list_del(&address_filter->list); 740 mutex_unlock(&msft->filter_lock); 741 742 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 743 cp.handle = address_filter->msft_handle; 744 745 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 746 HCI_CMD_TIMEOUT); 747 if (IS_ERR(skb)) { 748 bt_dev_err(hdev, "MSFT: Failed to cancel address (%pMR) filter", 749 &address_filter->bdaddr); 750 err = PTR_ERR(skb); 751 goto done; 752 } 753 kfree_skb(skb); 754 755 bt_dev_dbg(hdev, "MSFT: Canceled device %pMR address filter", 756 &address_filter->bdaddr); 757 758 done: 759 kfree(address_filter); 760 761 return err; 762 } 763 764 void msft_register(struct hci_dev *hdev) 765 { 766 struct msft_data *msft = NULL; 767 768 bt_dev_dbg(hdev, "Register MSFT extension"); 769 770 msft = kzalloc_obj(*msft); 771 if (!msft) { 772 bt_dev_err(hdev, "Failed to register MSFT extension"); 773 return; 774 } 775 776 INIT_LIST_HEAD(&msft->handle_map); 777 INIT_LIST_HEAD(&msft->address_filters); 778 mutex_init(&msft->filter_lock); 779 hdev->msft_data = msft; 780 } 781 782 void msft_release(struct hci_dev *hdev) 783 { 784 struct msft_data *msft = hdev->msft_data; 785 786 if (!msft) 787 return; 788 789 bt_dev_dbg(hdev, "Unregister MSFT extension"); 790 791 hdev->msft_data = NULL; 792 793 kfree(msft->evt_prefix); 794 mutex_destroy(&msft->filter_lock); 795 kfree(msft); 796 } 797 798 /* This function requires the caller holds hdev->lock */ 799 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, 800 __u8 addr_type, __u16 mgmt_handle) 801 { 802 struct monitored_device *dev; 803 804 dev = kmalloc_obj(*dev); 805 if (!dev) { 806 bt_dev_err(hdev, "MSFT vendor event %u: no memory", 807 MSFT_EV_LE_MONITOR_DEVICE); 808 return; 809 } 810 811 bacpy(&dev->bdaddr, bdaddr); 812 dev->addr_type = addr_type; 813 dev->handle = mgmt_handle; 814 dev->notified = false; 815 816 INIT_LIST_HEAD(&dev->list); 817 list_add(&dev->list, &hdev->monitored_devices); 818 hdev->advmon_pend_notify = true; 819 } 820 821 /* This function requires the caller holds hdev->lock */ 822 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr, 823 __u8 addr_type, __u16 mgmt_handle) 824 { 825 if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type, 826 true)) { 827 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list", 828 MSFT_EV_LE_MONITOR_DEVICE, bdaddr); 829 } 830 } 831 832 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, 833 u8 ev, size_t len) 834 { 835 void *data; 836 837 data = skb_pull_data(skb, len); 838 if (!data) 839 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev); 840 841 return data; 842 } 843 844 static int msft_add_address_filter_sync(struct hci_dev *hdev, void *data) 845 { 846 struct msft_monitor_addr_filter_data *address_filter = data; 847 struct msft_rp_le_monitor_advertisement *rp; 848 struct msft_cp_le_monitor_advertisement *cp; 849 struct msft_data *msft = hdev->msft_data; 850 struct sk_buff *skb = NULL; 851 bool remove = false; 852 size_t size; 853 854 if (!msft) { 855 bt_dev_err(hdev, "MSFT: msft data is freed"); 856 return -EINVAL; 857 } 858 859 /* The address filter has been removed by hci dev close */ 860 if (!test_bit(HCI_UP, &hdev->flags)) 861 return -ENODEV; 862 863 /* We are safe to use the address filter from now on. 864 * msft_monitor_device_evt() wouldn't delete this filter because it's 865 * not been added by now. 866 * And all other functions that requiring hci_req_sync_lock wouldn't 867 * touch this filter before this func completes because it's protected 868 * by hci_req_sync_lock. 869 */ 870 871 if (address_filter->state == AF_STATE_REMOVING) { 872 mutex_lock(&msft->filter_lock); 873 list_del(&address_filter->list); 874 mutex_unlock(&msft->filter_lock); 875 kfree(address_filter); 876 return 0; 877 } 878 879 size = sizeof(*cp) + 880 sizeof(address_filter->addr_type) + 881 sizeof(address_filter->bdaddr); 882 cp = kzalloc(size, GFP_KERNEL); 883 if (!cp) { 884 bt_dev_err(hdev, "MSFT: Alloc cmd param err"); 885 remove = true; 886 goto done; 887 } 888 889 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 890 cp->rssi_high = address_filter->rssi_high; 891 cp->rssi_low = address_filter->rssi_low; 892 cp->rssi_low_interval = address_filter->rssi_low_interval; 893 cp->rssi_sampling_period = address_filter->rssi_sampling_period; 894 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_ADDR; 895 cp->data[0] = address_filter->addr_type; 896 memcpy(&cp->data[1], &address_filter->bdaddr, 897 sizeof(address_filter->bdaddr)); 898 899 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, size, cp, 900 HCI_CMD_TIMEOUT); 901 kfree(cp); 902 903 if (IS_ERR(skb)) { 904 bt_dev_err(hdev, "Failed to enable address %pMR filter", 905 &address_filter->bdaddr); 906 skb = NULL; 907 remove = true; 908 goto done; 909 } 910 911 rp = skb_pull_data(skb, sizeof(*rp)); 912 if (!rp || rp->sub_opcode != MSFT_OP_LE_MONITOR_ADVERTISEMENT || 913 rp->status) 914 remove = true; 915 916 done: 917 mutex_lock(&msft->filter_lock); 918 919 if (remove) { 920 bt_dev_warn(hdev, "MSFT: Remove address (%pMR) filter", 921 &address_filter->bdaddr); 922 list_del(&address_filter->list); 923 kfree(address_filter); 924 } else { 925 address_filter->state = AF_STATE_ADDED; 926 address_filter->msft_handle = rp->handle; 927 bt_dev_dbg(hdev, "MSFT: Address %pMR filter enabled", 928 &address_filter->bdaddr); 929 } 930 mutex_unlock(&msft->filter_lock); 931 932 kfree_skb(skb); 933 934 return 0; 935 } 936 937 /* This function requires the caller holds msft->filter_lock */ 938 static struct msft_monitor_addr_filter_data *msft_add_address_filter 939 (struct hci_dev *hdev, u8 addr_type, bdaddr_t *bdaddr, 940 struct msft_monitor_advertisement_handle_data *handle_data) 941 { 942 struct msft_monitor_addr_filter_data *address_filter = NULL; 943 struct msft_data *msft = hdev->msft_data; 944 int err; 945 946 address_filter = kzalloc_obj(*address_filter); 947 if (!address_filter) 948 return NULL; 949 950 address_filter->state = AF_STATE_ADDING; 951 address_filter->msft_handle = 0xff; 952 address_filter->pattern_handle = handle_data->msft_handle; 953 address_filter->mgmt_handle = handle_data->mgmt_handle; 954 address_filter->rssi_high = handle_data->rssi_high; 955 address_filter->rssi_low = handle_data->rssi_low; 956 address_filter->rssi_low_interval = handle_data->rssi_low_interval; 957 address_filter->rssi_sampling_period = handle_data->rssi_sampling_period; 958 address_filter->addr_type = addr_type; 959 bacpy(&address_filter->bdaddr, bdaddr); 960 961 /* With the above AF_STATE_ADDING, duplicated address filter can be 962 * avoided when receiving monitor device event (found/lost) frequently 963 * for the same device. 964 */ 965 list_add_tail(&address_filter->list, &msft->address_filters); 966 967 err = hci_cmd_sync_queue(hdev, msft_add_address_filter_sync, 968 address_filter, NULL); 969 if (err < 0) { 970 bt_dev_err(hdev, "MSFT: Add address %pMR filter err", bdaddr); 971 list_del(&address_filter->list); 972 kfree(address_filter); 973 return NULL; 974 } 975 976 bt_dev_dbg(hdev, "MSFT: Add device %pMR address filter", 977 &address_filter->bdaddr); 978 979 return address_filter; 980 } 981 982 /* This function requires the caller holds hdev->lock */ 983 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb) 984 { 985 struct msft_monitor_addr_filter_data *n, *address_filter = NULL; 986 struct msft_ev_le_monitor_device *ev; 987 struct msft_monitor_advertisement_handle_data *handle_data; 988 struct msft_data *msft = hdev->msft_data; 989 u16 mgmt_handle = 0xffff; 990 u8 addr_type; 991 992 ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev)); 993 if (!ev) 994 return; 995 996 bt_dev_dbg(hdev, 997 "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR", 998 MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle, 999 ev->monitor_state, &ev->bdaddr); 1000 1001 handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false); 1002 1003 if (!hci_test_quirk(hdev, HCI_QUIRK_USE_MSFT_EXT_ADDRESS_FILTER)) { 1004 if (!handle_data) 1005 return; 1006 mgmt_handle = handle_data->mgmt_handle; 1007 goto report_state; 1008 } 1009 1010 if (handle_data) { 1011 /* Don't report any device found/lost event from pattern 1012 * monitors. Pattern monitor always has its address filters for 1013 * tracking devices. 1014 */ 1015 1016 address_filter = msft_find_address_data(hdev, ev->addr_type, 1017 &ev->bdaddr, 1018 handle_data->msft_handle); 1019 if (address_filter) 1020 return; 1021 1022 if (ev->monitor_state && handle_data->cond_type == 1023 MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN) 1024 msft_add_address_filter(hdev, ev->addr_type, 1025 &ev->bdaddr, handle_data); 1026 1027 return; 1028 } 1029 1030 /* This device event is not from pattern monitor. 1031 * Report it if there is a corresponding address_filter for it. 1032 */ 1033 list_for_each_entry(n, &msft->address_filters, list) { 1034 if (n->state == AF_STATE_ADDED && 1035 n->msft_handle == ev->monitor_handle) { 1036 mgmt_handle = n->mgmt_handle; 1037 address_filter = n; 1038 break; 1039 } 1040 } 1041 1042 if (!address_filter) { 1043 bt_dev_warn(hdev, "MSFT: Unexpected device event %pMR, %u, %u", 1044 &ev->bdaddr, ev->monitor_handle, ev->monitor_state); 1045 return; 1046 } 1047 1048 report_state: 1049 switch (ev->addr_type) { 1050 case ADDR_LE_DEV_PUBLIC: 1051 addr_type = BDADDR_LE_PUBLIC; 1052 break; 1053 1054 case ADDR_LE_DEV_RANDOM: 1055 addr_type = BDADDR_LE_RANDOM; 1056 break; 1057 1058 default: 1059 bt_dev_err(hdev, 1060 "MSFT vendor event 0x%02x: unknown addr type 0x%02x", 1061 MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type); 1062 return; 1063 } 1064 1065 if (ev->monitor_state) { 1066 msft_device_found(hdev, &ev->bdaddr, addr_type, mgmt_handle); 1067 } else { 1068 if (address_filter && address_filter->state == AF_STATE_ADDED) { 1069 address_filter->state = AF_STATE_REMOVING; 1070 hci_cmd_sync_queue(hdev, 1071 msft_cancel_address_filter_sync, 1072 address_filter, 1073 NULL); 1074 } 1075 msft_device_lost(hdev, &ev->bdaddr, addr_type, mgmt_handle); 1076 } 1077 } 1078 1079 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) 1080 { 1081 struct msft_data *msft = hdev->msft_data; 1082 u8 *evt_prefix; 1083 u8 *evt; 1084 1085 if (!msft) 1086 return; 1087 1088 /* When the extension has defined an event prefix, check that it 1089 * matches, and otherwise just return. 1090 */ 1091 if (msft->evt_prefix_len > 0) { 1092 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len); 1093 if (!evt_prefix) 1094 return; 1095 1096 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len)) 1097 return; 1098 } 1099 1100 /* Every event starts at least with an event code and the rest of 1101 * the data is variable and depends on the event code. 1102 */ 1103 if (skb->len < 1) 1104 return; 1105 1106 evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt)); 1107 if (!evt) 1108 return; 1109 1110 hci_dev_lock(hdev); 1111 1112 switch (*evt) { 1113 case MSFT_EV_LE_MONITOR_DEVICE: 1114 mutex_lock(&msft->filter_lock); 1115 msft_monitor_device_evt(hdev, skb); 1116 mutex_unlock(&msft->filter_lock); 1117 break; 1118 1119 default: 1120 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt); 1121 break; 1122 } 1123 1124 hci_dev_unlock(hdev); 1125 } 1126 1127 __u64 msft_get_features(struct hci_dev *hdev) 1128 { 1129 struct msft_data *msft = hdev->msft_data; 1130 1131 return msft ? msft->features : 0; 1132 } 1133 1134 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, 1135 void *user_data, 1136 u8 status) 1137 { 1138 struct msft_cp_le_set_advertisement_filter_enable *cp = user_data; 1139 struct msft_data *msft = hdev->msft_data; 1140 1141 /* Error 0x0C would be returned if the filter enabled status is 1142 * already set to whatever we were trying to set. 1143 * Although the default state should be disabled, some controller set 1144 * the initial value to enabled. Because there is no way to know the 1145 * actual initial value before sending this command, here we also treat 1146 * error 0x0C as success. 1147 */ 1148 if (status != 0x00 && status != 0x0C) 1149 return; 1150 1151 hci_dev_lock(hdev); 1152 1153 msft->filter_enabled = cp->enable; 1154 1155 if (status == 0x0C) 1156 bt_dev_warn(hdev, "MSFT filter_enable is already %s", 1157 cp->enable ? "on" : "off"); 1158 1159 hci_dev_unlock(hdev); 1160 } 1161 1162 /* This function requires the caller holds hci_req_sync_lock */ 1163 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) 1164 { 1165 struct msft_data *msft = hdev->msft_data; 1166 1167 if (!msft) 1168 return -EOPNOTSUPP; 1169 1170 if (msft->resuming || msft->suspending) 1171 return -EBUSY; 1172 1173 return msft_add_monitor_sync(hdev, monitor); 1174 } 1175 1176 /* This function requires the caller holds hci_req_sync_lock */ 1177 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor) 1178 { 1179 struct msft_data *msft = hdev->msft_data; 1180 1181 if (!msft) 1182 return -EOPNOTSUPP; 1183 1184 if (msft->resuming || msft->suspending) 1185 return -EBUSY; 1186 1187 return msft_remove_monitor_sync(hdev, monitor); 1188 } 1189 1190 int msft_set_filter_enable(struct hci_dev *hdev, bool enable) 1191 { 1192 struct msft_cp_le_set_advertisement_filter_enable cp; 1193 struct msft_data *msft = hdev->msft_data; 1194 int err; 1195 1196 if (!msft) 1197 return -EOPNOTSUPP; 1198 1199 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE; 1200 cp.enable = enable; 1201 err = __hci_cmd_sync_status(hdev, hdev->msft_opcode, sizeof(cp), &cp, 1202 HCI_CMD_TIMEOUT); 1203 1204 msft_le_set_advertisement_filter_enable_cb(hdev, &cp, err); 1205 1206 return 0; 1207 } 1208 1209 bool msft_curve_validity(struct hci_dev *hdev) 1210 { 1211 return hdev->msft_curve_validity; 1212 } 1213