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