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 struct list_head list; 95 }; 96 97 struct msft_data { 98 __u64 features; 99 __u8 evt_prefix_len; 100 __u8 *evt_prefix; 101 struct list_head handle_map; 102 __u16 pending_add_handle; 103 __u16 pending_remove_handle; 104 __u8 resuming; 105 __u8 suspending; 106 __u8 filter_enabled; 107 }; 108 109 static int __msft_add_monitor_pattern(struct hci_dev *hdev, 110 struct adv_monitor *monitor); 111 static int __msft_remove_monitor(struct hci_dev *hdev, 112 struct adv_monitor *monitor, u16 handle); 113 114 bool msft_monitor_supported(struct hci_dev *hdev) 115 { 116 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR); 117 } 118 119 static bool read_supported_features(struct hci_dev *hdev, 120 struct msft_data *msft) 121 { 122 struct msft_cp_read_supported_features cp; 123 struct msft_rp_read_supported_features *rp; 124 struct sk_buff *skb; 125 126 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES; 127 128 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 129 HCI_CMD_TIMEOUT); 130 if (IS_ERR(skb)) { 131 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)", 132 PTR_ERR(skb)); 133 return false; 134 } 135 136 if (skb->len < sizeof(*rp)) { 137 bt_dev_err(hdev, "MSFT supported features length mismatch"); 138 goto failed; 139 } 140 141 rp = (struct msft_rp_read_supported_features *)skb->data; 142 143 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES) 144 goto failed; 145 146 if (rp->evt_prefix_len > 0) { 147 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len, 148 GFP_KERNEL); 149 if (!msft->evt_prefix) 150 goto failed; 151 } 152 153 msft->evt_prefix_len = rp->evt_prefix_len; 154 msft->features = __le64_to_cpu(rp->features); 155 156 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY) 157 hdev->msft_curve_validity = true; 158 159 kfree_skb(skb); 160 return true; 161 162 failed: 163 kfree_skb(skb); 164 return false; 165 } 166 167 static void reregister_monitor(struct hci_dev *hdev, int handle) 168 { 169 struct adv_monitor *monitor; 170 struct msft_data *msft = hdev->msft_data; 171 int err; 172 173 while (1) { 174 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 175 if (!monitor) { 176 /* All monitors have been resumed */ 177 msft->resuming = false; 178 hci_update_passive_scan(hdev); 179 return; 180 } 181 182 msft->pending_add_handle = (u16)handle; 183 err = __msft_add_monitor_pattern(hdev, monitor); 184 185 /* If success, we return and wait for monitor added callback */ 186 if (!err) 187 return; 188 189 /* Otherwise remove the monitor and keep registering */ 190 hci_free_adv_monitor(hdev, monitor); 191 handle++; 192 } 193 } 194 195 /* is_mgmt = true matches the handle exposed to userspace via mgmt. 196 * is_mgmt = false matches the handle used by the msft controller. 197 * This function requires the caller holds hdev->lock 198 */ 199 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data 200 (struct hci_dev *hdev, u16 handle, bool is_mgmt) 201 { 202 struct msft_monitor_advertisement_handle_data *entry; 203 struct msft_data *msft = hdev->msft_data; 204 205 list_for_each_entry(entry, &msft->handle_map, list) { 206 if (is_mgmt && entry->mgmt_handle == handle) 207 return entry; 208 if (!is_mgmt && entry->msft_handle == handle) 209 return entry; 210 } 211 212 return NULL; 213 } 214 215 /* This function requires the caller holds hdev->lock */ 216 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle, 217 bdaddr_t *bdaddr, __u8 addr_type, 218 bool notify) 219 { 220 struct monitored_device *dev, *tmp; 221 int count = 0; 222 223 list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) { 224 /* mgmt_handle == 0 indicates remove all devices, whereas, 225 * bdaddr == NULL indicates remove all devices matching the 226 * mgmt_handle. 227 */ 228 if ((!mgmt_handle || dev->handle == mgmt_handle) && 229 (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) && 230 addr_type == dev->addr_type))) { 231 if (notify && dev->notified) { 232 mgmt_adv_monitor_device_lost(hdev, dev->handle, 233 &dev->bdaddr, 234 dev->addr_type); 235 } 236 237 list_del(&dev->list); 238 kfree(dev); 239 count++; 240 } 241 } 242 243 return count; 244 } 245 246 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev, 247 u8 status, u16 opcode, 248 struct sk_buff *skb) 249 { 250 struct msft_rp_le_monitor_advertisement *rp; 251 struct adv_monitor *monitor; 252 struct msft_monitor_advertisement_handle_data *handle_data; 253 struct msft_data *msft = hdev->msft_data; 254 255 hci_dev_lock(hdev); 256 257 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle); 258 if (!monitor) { 259 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!", 260 msft->pending_add_handle); 261 status = HCI_ERROR_UNSPECIFIED; 262 goto unlock; 263 } 264 265 if (status) 266 goto unlock; 267 268 rp = (struct msft_rp_le_monitor_advertisement *)skb->data; 269 if (skb->len < sizeof(*rp)) { 270 status = HCI_ERROR_UNSPECIFIED; 271 goto unlock; 272 } 273 274 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL); 275 if (!handle_data) { 276 status = HCI_ERROR_UNSPECIFIED; 277 goto unlock; 278 } 279 280 handle_data->mgmt_handle = monitor->handle; 281 handle_data->msft_handle = rp->handle; 282 INIT_LIST_HEAD(&handle_data->list); 283 list_add(&handle_data->list, &msft->handle_map); 284 285 monitor->state = ADV_MONITOR_STATE_OFFLOADED; 286 287 unlock: 288 if (status && monitor) 289 hci_free_adv_monitor(hdev, monitor); 290 291 hci_dev_unlock(hdev); 292 293 if (!msft->resuming) 294 hci_add_adv_patterns_monitor_complete(hdev, status); 295 } 296 297 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, 298 u8 status, u16 opcode, 299 struct sk_buff *skb) 300 { 301 struct msft_cp_le_cancel_monitor_advertisement *cp; 302 struct msft_rp_le_cancel_monitor_advertisement *rp; 303 struct adv_monitor *monitor; 304 struct msft_monitor_advertisement_handle_data *handle_data; 305 struct msft_data *msft = hdev->msft_data; 306 int err; 307 bool pending; 308 309 if (status) 310 goto done; 311 312 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data; 313 if (skb->len < sizeof(*rp)) { 314 status = HCI_ERROR_UNSPECIFIED; 315 goto done; 316 } 317 318 hci_dev_lock(hdev); 319 320 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); 321 handle_data = msft_find_handle_data(hdev, cp->handle, false); 322 323 if (handle_data) { 324 monitor = idr_find(&hdev->adv_monitors_idr, 325 handle_data->mgmt_handle); 326 327 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED) 328 monitor->state = ADV_MONITOR_STATE_REGISTERED; 329 330 /* Do not free the monitor if it is being removed due to 331 * suspend. It will be re-monitored on resume. 332 */ 333 if (monitor && !msft->suspending) 334 hci_free_adv_monitor(hdev, monitor); 335 336 /* Clear any monitored devices by this Adv Monitor */ 337 msft_monitor_device_del(hdev, handle_data->mgmt_handle, NULL, 338 0, false); 339 340 list_del(&handle_data->list); 341 kfree(handle_data); 342 } 343 344 /* If remove all monitors is required, we need to continue the process 345 * here because the earlier it was paused when waiting for the 346 * response from controller. 347 */ 348 if (msft->pending_remove_handle == 0) { 349 pending = hci_remove_all_adv_monitor(hdev, &err); 350 if (pending) { 351 hci_dev_unlock(hdev); 352 return; 353 } 354 355 if (err) 356 status = HCI_ERROR_UNSPECIFIED; 357 } 358 359 hci_dev_unlock(hdev); 360 361 done: 362 if (!msft->suspending) 363 hci_remove_adv_monitor_complete(hdev, status); 364 } 365 366 static int msft_remove_monitor_sync(struct hci_dev *hdev, 367 struct adv_monitor *monitor) 368 { 369 struct msft_cp_le_cancel_monitor_advertisement cp; 370 struct msft_monitor_advertisement_handle_data *handle_data; 371 struct sk_buff *skb; 372 u8 status; 373 374 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 375 376 /* If no matched handle, just remove without telling controller */ 377 if (!handle_data) 378 return -ENOENT; 379 380 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 381 cp.handle = handle_data->msft_handle; 382 383 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 384 HCI_CMD_TIMEOUT); 385 if (IS_ERR(skb)) 386 return PTR_ERR(skb); 387 388 status = skb->data[0]; 389 skb_pull(skb, 1); 390 391 msft_le_cancel_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, 392 skb); 393 394 return status; 395 } 396 397 /* This function requires the caller holds hci_req_sync_lock */ 398 int msft_suspend_sync(struct hci_dev *hdev) 399 { 400 struct msft_data *msft = hdev->msft_data; 401 struct adv_monitor *monitor; 402 int handle = 0; 403 404 if (!msft || !msft_monitor_supported(hdev)) 405 return 0; 406 407 msft->suspending = true; 408 409 while (1) { 410 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 411 if (!monitor) 412 break; 413 414 msft_remove_monitor_sync(hdev, monitor); 415 416 handle++; 417 } 418 419 /* All monitors have been removed */ 420 msft->suspending = false; 421 422 return 0; 423 } 424 425 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor) 426 { 427 struct adv_rssi_thresholds *r = &monitor->rssi; 428 429 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 430 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX || 431 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 432 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX) 433 return false; 434 435 /* High_threshold_timeout is not supported, 436 * once high_threshold is reached, events are immediately reported. 437 */ 438 if (r->high_threshold_timeout != 0) 439 return false; 440 441 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX) 442 return false; 443 444 /* Sampling period from 0x00 to 0xFF are all allowed */ 445 return true; 446 } 447 448 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor) 449 { 450 return msft_monitor_rssi_valid(monitor); 451 /* No additional check needed for pattern-based monitor */ 452 } 453 454 static int msft_add_monitor_sync(struct hci_dev *hdev, 455 struct adv_monitor *monitor) 456 { 457 struct msft_cp_le_monitor_advertisement *cp; 458 struct msft_le_monitor_advertisement_pattern_data *pattern_data; 459 struct msft_le_monitor_advertisement_pattern *pattern; 460 struct adv_pattern *entry; 461 size_t total_size = sizeof(*cp) + sizeof(*pattern_data); 462 ptrdiff_t offset = 0; 463 u8 pattern_count = 0; 464 struct sk_buff *skb; 465 u8 status; 466 467 if (!msft_monitor_pattern_valid(monitor)) 468 return -EINVAL; 469 470 list_for_each_entry(entry, &monitor->patterns, list) { 471 pattern_count++; 472 total_size += sizeof(*pattern) + entry->length; 473 } 474 475 cp = kmalloc(total_size, GFP_KERNEL); 476 if (!cp) 477 return -ENOMEM; 478 479 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 480 cp->rssi_high = monitor->rssi.high_threshold; 481 cp->rssi_low = monitor->rssi.low_threshold; 482 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; 483 cp->rssi_sampling_period = monitor->rssi.sampling_period; 484 485 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 486 487 pattern_data = (void *)cp->data; 488 pattern_data->count = pattern_count; 489 490 list_for_each_entry(entry, &monitor->patterns, list) { 491 pattern = (void *)(pattern_data->data + offset); 492 /* the length also includes data_type and offset */ 493 pattern->length = entry->length + 2; 494 pattern->data_type = entry->ad_type; 495 pattern->start_byte = entry->offset; 496 memcpy(pattern->pattern, entry->value, entry->length); 497 offset += sizeof(*pattern) + entry->length; 498 } 499 500 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp, 501 HCI_CMD_TIMEOUT); 502 kfree(cp); 503 504 if (IS_ERR(skb)) 505 return PTR_ERR(skb); 506 507 status = skb->data[0]; 508 skb_pull(skb, 1); 509 510 msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb); 511 512 return status; 513 } 514 515 /* This function requires the caller holds hci_req_sync_lock */ 516 int msft_resume_sync(struct hci_dev *hdev) 517 { 518 struct msft_data *msft = hdev->msft_data; 519 struct adv_monitor *monitor; 520 int handle = 0; 521 522 if (!msft || !msft_monitor_supported(hdev)) 523 return 0; 524 525 msft->resuming = true; 526 527 while (1) { 528 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 529 if (!monitor) 530 break; 531 532 msft_add_monitor_sync(hdev, monitor); 533 534 handle++; 535 } 536 537 /* All monitors have been resumed */ 538 msft->resuming = false; 539 540 return 0; 541 } 542 543 void msft_do_open(struct hci_dev *hdev) 544 { 545 struct msft_data *msft = hdev->msft_data; 546 547 if (hdev->msft_opcode == HCI_OP_NOP) 548 return; 549 550 if (!msft) { 551 bt_dev_err(hdev, "MSFT extension not registered"); 552 return; 553 } 554 555 bt_dev_dbg(hdev, "Initialize MSFT extension"); 556 557 /* Reset existing MSFT data before re-reading */ 558 kfree(msft->evt_prefix); 559 msft->evt_prefix = NULL; 560 msft->evt_prefix_len = 0; 561 msft->features = 0; 562 563 if (!read_supported_features(hdev, msft)) { 564 hdev->msft_data = NULL; 565 kfree(msft); 566 return; 567 } 568 569 if (msft_monitor_supported(hdev)) { 570 msft->resuming = true; 571 msft_set_filter_enable(hdev, true); 572 /* Monitors get removed on power off, so we need to explicitly 573 * tell the controller to re-monitor. 574 */ 575 reregister_monitor(hdev, 0); 576 } 577 } 578 579 void msft_do_close(struct hci_dev *hdev) 580 { 581 struct msft_data *msft = hdev->msft_data; 582 struct msft_monitor_advertisement_handle_data *handle_data, *tmp; 583 struct adv_monitor *monitor; 584 585 if (!msft) 586 return; 587 588 bt_dev_dbg(hdev, "Cleanup of MSFT extension"); 589 590 /* The controller will silently remove all monitors on power off. 591 * Therefore, remove handle_data mapping and reset monitor state. 592 */ 593 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) { 594 monitor = idr_find(&hdev->adv_monitors_idr, 595 handle_data->mgmt_handle); 596 597 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED) 598 monitor->state = ADV_MONITOR_STATE_REGISTERED; 599 600 list_del(&handle_data->list); 601 kfree(handle_data); 602 } 603 604 hci_dev_lock(hdev); 605 606 /* Clear any devices that are being monitored and notify device lost */ 607 hdev->advmon_pend_notify = false; 608 msft_monitor_device_del(hdev, 0, NULL, 0, true); 609 610 hci_dev_unlock(hdev); 611 } 612 613 void msft_register(struct hci_dev *hdev) 614 { 615 struct msft_data *msft = NULL; 616 617 bt_dev_dbg(hdev, "Register MSFT extension"); 618 619 msft = kzalloc(sizeof(*msft), GFP_KERNEL); 620 if (!msft) { 621 bt_dev_err(hdev, "Failed to register MSFT extension"); 622 return; 623 } 624 625 INIT_LIST_HEAD(&msft->handle_map); 626 hdev->msft_data = msft; 627 } 628 629 void msft_unregister(struct hci_dev *hdev) 630 { 631 struct msft_data *msft = hdev->msft_data; 632 633 if (!msft) 634 return; 635 636 bt_dev_dbg(hdev, "Unregister MSFT extension"); 637 638 hdev->msft_data = NULL; 639 640 kfree(msft->evt_prefix); 641 kfree(msft); 642 } 643 644 /* This function requires the caller holds hdev->lock */ 645 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, 646 __u8 addr_type, __u16 mgmt_handle) 647 { 648 struct monitored_device *dev; 649 650 dev = kmalloc(sizeof(*dev), GFP_KERNEL); 651 if (!dev) { 652 bt_dev_err(hdev, "MSFT vendor event %u: no memory", 653 MSFT_EV_LE_MONITOR_DEVICE); 654 return; 655 } 656 657 bacpy(&dev->bdaddr, bdaddr); 658 dev->addr_type = addr_type; 659 dev->handle = mgmt_handle; 660 dev->notified = false; 661 662 INIT_LIST_HEAD(&dev->list); 663 list_add(&dev->list, &hdev->monitored_devices); 664 hdev->advmon_pend_notify = true; 665 } 666 667 /* This function requires the caller holds hdev->lock */ 668 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr, 669 __u8 addr_type, __u16 mgmt_handle) 670 { 671 if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type, 672 true)) { 673 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list", 674 MSFT_EV_LE_MONITOR_DEVICE, bdaddr); 675 } 676 } 677 678 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, 679 u8 ev, size_t len) 680 { 681 void *data; 682 683 data = skb_pull_data(skb, len); 684 if (!data) 685 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev); 686 687 return data; 688 } 689 690 /* This function requires the caller holds hdev->lock */ 691 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb) 692 { 693 struct msft_ev_le_monitor_device *ev; 694 struct msft_monitor_advertisement_handle_data *handle_data; 695 u8 addr_type; 696 697 ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev)); 698 if (!ev) 699 return; 700 701 bt_dev_dbg(hdev, 702 "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR", 703 MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle, 704 ev->monitor_state, &ev->bdaddr); 705 706 handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false); 707 if (!handle_data) 708 return; 709 710 switch (ev->addr_type) { 711 case ADDR_LE_DEV_PUBLIC: 712 addr_type = BDADDR_LE_PUBLIC; 713 break; 714 715 case ADDR_LE_DEV_RANDOM: 716 addr_type = BDADDR_LE_RANDOM; 717 break; 718 719 default: 720 bt_dev_err(hdev, 721 "MSFT vendor event 0x%02x: unknown addr type 0x%02x", 722 MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type); 723 return; 724 } 725 726 if (ev->monitor_state) 727 msft_device_found(hdev, &ev->bdaddr, addr_type, 728 handle_data->mgmt_handle); 729 else 730 msft_device_lost(hdev, &ev->bdaddr, addr_type, 731 handle_data->mgmt_handle); 732 } 733 734 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) 735 { 736 struct msft_data *msft = hdev->msft_data; 737 u8 *evt_prefix; 738 u8 *evt; 739 740 if (!msft) 741 return; 742 743 /* When the extension has defined an event prefix, check that it 744 * matches, and otherwise just return. 745 */ 746 if (msft->evt_prefix_len > 0) { 747 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len); 748 if (!evt_prefix) 749 return; 750 751 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len)) 752 return; 753 } 754 755 /* Every event starts at least with an event code and the rest of 756 * the data is variable and depends on the event code. 757 */ 758 if (skb->len < 1) 759 return; 760 761 evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt)); 762 if (!evt) 763 return; 764 765 hci_dev_lock(hdev); 766 767 switch (*evt) { 768 case MSFT_EV_LE_MONITOR_DEVICE: 769 msft_monitor_device_evt(hdev, skb); 770 break; 771 772 default: 773 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt); 774 break; 775 } 776 777 hci_dev_unlock(hdev); 778 } 779 780 __u64 msft_get_features(struct hci_dev *hdev) 781 { 782 struct msft_data *msft = hdev->msft_data; 783 784 return msft ? msft->features : 0; 785 } 786 787 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, 788 u8 status, u16 opcode, 789 struct sk_buff *skb) 790 { 791 struct msft_cp_le_set_advertisement_filter_enable *cp; 792 struct msft_rp_le_set_advertisement_filter_enable *rp; 793 struct msft_data *msft = hdev->msft_data; 794 795 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data; 796 if (skb->len < sizeof(*rp)) 797 return; 798 799 /* Error 0x0C would be returned if the filter enabled status is 800 * already set to whatever we were trying to set. 801 * Although the default state should be disabled, some controller set 802 * the initial value to enabled. Because there is no way to know the 803 * actual initial value before sending this command, here we also treat 804 * error 0x0C as success. 805 */ 806 if (status != 0x00 && status != 0x0C) 807 return; 808 809 hci_dev_lock(hdev); 810 811 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); 812 msft->filter_enabled = cp->enable; 813 814 if (status == 0x0C) 815 bt_dev_warn(hdev, "MSFT filter_enable is already %s", 816 cp->enable ? "on" : "off"); 817 818 hci_dev_unlock(hdev); 819 } 820 821 /* This function requires the caller holds hdev->lock */ 822 static int __msft_add_monitor_pattern(struct hci_dev *hdev, 823 struct adv_monitor *monitor) 824 { 825 struct msft_cp_le_monitor_advertisement *cp; 826 struct msft_le_monitor_advertisement_pattern_data *pattern_data; 827 struct msft_le_monitor_advertisement_pattern *pattern; 828 struct adv_pattern *entry; 829 struct hci_request req; 830 struct msft_data *msft = hdev->msft_data; 831 size_t total_size = sizeof(*cp) + sizeof(*pattern_data); 832 ptrdiff_t offset = 0; 833 u8 pattern_count = 0; 834 int err = 0; 835 836 if (!msft_monitor_pattern_valid(monitor)) 837 return -EINVAL; 838 839 list_for_each_entry(entry, &monitor->patterns, list) { 840 pattern_count++; 841 total_size += sizeof(*pattern) + entry->length; 842 } 843 844 cp = kmalloc(total_size, GFP_KERNEL); 845 if (!cp) 846 return -ENOMEM; 847 848 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 849 cp->rssi_high = monitor->rssi.high_threshold; 850 cp->rssi_low = monitor->rssi.low_threshold; 851 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; 852 cp->rssi_sampling_period = monitor->rssi.sampling_period; 853 854 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 855 856 pattern_data = (void *)cp->data; 857 pattern_data->count = pattern_count; 858 859 list_for_each_entry(entry, &monitor->patterns, list) { 860 pattern = (void *)(pattern_data->data + offset); 861 /* the length also includes data_type and offset */ 862 pattern->length = entry->length + 2; 863 pattern->data_type = entry->ad_type; 864 pattern->start_byte = entry->offset; 865 memcpy(pattern->pattern, entry->value, entry->length); 866 offset += sizeof(*pattern) + entry->length; 867 } 868 869 hci_req_init(&req, hdev); 870 hci_req_add(&req, hdev->msft_opcode, total_size, cp); 871 err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb); 872 kfree(cp); 873 874 if (!err) 875 msft->pending_add_handle = monitor->handle; 876 877 return err; 878 } 879 880 /* This function requires the caller holds hdev->lock */ 881 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) 882 { 883 struct msft_data *msft = hdev->msft_data; 884 885 if (!msft) 886 return -EOPNOTSUPP; 887 888 if (msft->resuming || msft->suspending) 889 return -EBUSY; 890 891 return __msft_add_monitor_pattern(hdev, monitor); 892 } 893 894 /* This function requires the caller holds hdev->lock */ 895 static int __msft_remove_monitor(struct hci_dev *hdev, 896 struct adv_monitor *monitor, u16 handle) 897 { 898 struct msft_cp_le_cancel_monitor_advertisement cp; 899 struct msft_monitor_advertisement_handle_data *handle_data; 900 struct hci_request req; 901 struct msft_data *msft = hdev->msft_data; 902 int err = 0; 903 904 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 905 906 /* If no matched handle, just remove without telling controller */ 907 if (!handle_data) 908 return -ENOENT; 909 910 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 911 cp.handle = handle_data->msft_handle; 912 913 hci_req_init(&req, hdev); 914 hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp); 915 err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb); 916 917 if (!err) 918 msft->pending_remove_handle = handle; 919 920 return err; 921 } 922 923 /* This function requires the caller holds hdev->lock */ 924 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, 925 u16 handle) 926 { 927 struct msft_data *msft = hdev->msft_data; 928 929 if (!msft) 930 return -EOPNOTSUPP; 931 932 if (msft->resuming || msft->suspending) 933 return -EBUSY; 934 935 return __msft_remove_monitor(hdev, monitor, handle); 936 } 937 938 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable) 939 { 940 struct hci_dev *hdev = req->hdev; 941 struct msft_cp_le_set_advertisement_filter_enable cp; 942 943 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE; 944 cp.enable = enable; 945 946 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp); 947 } 948 949 int msft_set_filter_enable(struct hci_dev *hdev, bool enable) 950 { 951 struct hci_request req; 952 struct msft_data *msft = hdev->msft_data; 953 int err; 954 955 if (!msft) 956 return -EOPNOTSUPP; 957 958 hci_req_init(&req, hdev); 959 msft_req_add_set_filter_enable(&req, enable); 960 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb); 961 962 return err; 963 } 964 965 bool msft_curve_validity(struct hci_dev *hdev) 966 { 967 return hdev->msft_curve_validity; 968 } 969