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