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