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[0]; 38 }; 39 40 struct msft_le_monitor_advertisement_pattern_data { 41 __u8 count; 42 __u8 data[0]; 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[0]; 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 struct msft_monitor_advertisement_handle_data { 84 __u8 msft_handle; 85 __u16 mgmt_handle; 86 struct list_head list; 87 }; 88 89 struct msft_data { 90 __u64 features; 91 __u8 evt_prefix_len; 92 __u8 *evt_prefix; 93 struct list_head handle_map; 94 __u16 pending_add_handle; 95 __u16 pending_remove_handle; 96 __u8 reregistering; 97 __u8 filter_enabled; 98 }; 99 100 static int __msft_add_monitor_pattern(struct hci_dev *hdev, 101 struct adv_monitor *monitor); 102 103 bool msft_monitor_supported(struct hci_dev *hdev) 104 { 105 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR); 106 } 107 108 static bool read_supported_features(struct hci_dev *hdev, 109 struct msft_data *msft) 110 { 111 struct msft_cp_read_supported_features cp; 112 struct msft_rp_read_supported_features *rp; 113 struct sk_buff *skb; 114 115 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES; 116 117 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, 118 HCI_CMD_TIMEOUT); 119 if (IS_ERR(skb)) { 120 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)", 121 PTR_ERR(skb)); 122 return false; 123 } 124 125 if (skb->len < sizeof(*rp)) { 126 bt_dev_err(hdev, "MSFT supported features length mismatch"); 127 goto failed; 128 } 129 130 rp = (struct msft_rp_read_supported_features *)skb->data; 131 132 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES) 133 goto failed; 134 135 if (rp->evt_prefix_len > 0) { 136 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len, 137 GFP_KERNEL); 138 if (!msft->evt_prefix) 139 goto failed; 140 } 141 142 msft->evt_prefix_len = rp->evt_prefix_len; 143 msft->features = __le64_to_cpu(rp->features); 144 145 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY) 146 hdev->msft_curve_validity = true; 147 148 kfree_skb(skb); 149 return true; 150 151 failed: 152 kfree_skb(skb); 153 return false; 154 } 155 156 /* This function requires the caller holds hdev->lock */ 157 static void reregister_monitor_on_restart(struct hci_dev *hdev, int handle) 158 { 159 struct adv_monitor *monitor; 160 struct msft_data *msft = hdev->msft_data; 161 int err; 162 163 while (1) { 164 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); 165 if (!monitor) { 166 /* All monitors have been reregistered */ 167 msft->reregistering = false; 168 hci_update_background_scan(hdev); 169 return; 170 } 171 172 msft->pending_add_handle = (u16)handle; 173 err = __msft_add_monitor_pattern(hdev, monitor); 174 175 /* If success, we return and wait for monitor added callback */ 176 if (!err) 177 return; 178 179 /* Otherwise remove the monitor and keep registering */ 180 hci_free_adv_monitor(hdev, monitor); 181 handle++; 182 } 183 } 184 185 void msft_do_open(struct hci_dev *hdev) 186 { 187 struct msft_data *msft; 188 189 if (hdev->msft_opcode == HCI_OP_NOP) 190 return; 191 192 bt_dev_dbg(hdev, "Initialize MSFT extension"); 193 194 msft = kzalloc(sizeof(*msft), GFP_KERNEL); 195 if (!msft) 196 return; 197 198 if (!read_supported_features(hdev, msft)) { 199 kfree(msft); 200 return; 201 } 202 203 INIT_LIST_HEAD(&msft->handle_map); 204 hdev->msft_data = msft; 205 206 if (msft_monitor_supported(hdev)) { 207 msft->reregistering = true; 208 msft_set_filter_enable(hdev, true); 209 reregister_monitor_on_restart(hdev, 0); 210 } 211 } 212 213 void msft_do_close(struct hci_dev *hdev) 214 { 215 struct msft_data *msft = hdev->msft_data; 216 struct msft_monitor_advertisement_handle_data *handle_data, *tmp; 217 struct adv_monitor *monitor; 218 219 if (!msft) 220 return; 221 222 bt_dev_dbg(hdev, "Cleanup of MSFT extension"); 223 224 hdev->msft_data = NULL; 225 226 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) { 227 monitor = idr_find(&hdev->adv_monitors_idr, 228 handle_data->mgmt_handle); 229 230 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED) 231 monitor->state = ADV_MONITOR_STATE_REGISTERED; 232 233 list_del(&handle_data->list); 234 kfree(handle_data); 235 } 236 237 kfree(msft->evt_prefix); 238 kfree(msft); 239 } 240 241 void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb) 242 { 243 struct msft_data *msft = hdev->msft_data; 244 u8 event; 245 246 if (!msft) 247 return; 248 249 /* When the extension has defined an event prefix, check that it 250 * matches, and otherwise just return. 251 */ 252 if (msft->evt_prefix_len > 0) { 253 if (skb->len < msft->evt_prefix_len) 254 return; 255 256 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len)) 257 return; 258 259 skb_pull(skb, msft->evt_prefix_len); 260 } 261 262 /* Every event starts at least with an event code and the rest of 263 * the data is variable and depends on the event code. 264 */ 265 if (skb->len < 1) 266 return; 267 268 event = *skb->data; 269 skb_pull(skb, 1); 270 271 bt_dev_dbg(hdev, "MSFT vendor event %u", event); 272 } 273 274 __u64 msft_get_features(struct hci_dev *hdev) 275 { 276 struct msft_data *msft = hdev->msft_data; 277 278 return msft ? msft->features : 0; 279 } 280 281 /* is_mgmt = true matches the handle exposed to userspace via mgmt. 282 * is_mgmt = false matches the handle used by the msft controller. 283 * This function requires the caller holds hdev->lock 284 */ 285 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data 286 (struct hci_dev *hdev, u16 handle, bool is_mgmt) 287 { 288 struct msft_monitor_advertisement_handle_data *entry; 289 struct msft_data *msft = hdev->msft_data; 290 291 list_for_each_entry(entry, &msft->handle_map, list) { 292 if (is_mgmt && entry->mgmt_handle == handle) 293 return entry; 294 if (!is_mgmt && entry->msft_handle == handle) 295 return entry; 296 } 297 298 return NULL; 299 } 300 301 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev, 302 u8 status, u16 opcode, 303 struct sk_buff *skb) 304 { 305 struct msft_rp_le_monitor_advertisement *rp; 306 struct adv_monitor *monitor; 307 struct msft_monitor_advertisement_handle_data *handle_data; 308 struct msft_data *msft = hdev->msft_data; 309 310 hci_dev_lock(hdev); 311 312 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle); 313 if (!monitor) { 314 bt_dev_err(hdev, "msft add advmon: monitor %d is not found!", 315 msft->pending_add_handle); 316 status = HCI_ERROR_UNSPECIFIED; 317 goto unlock; 318 } 319 320 if (status) 321 goto unlock; 322 323 rp = (struct msft_rp_le_monitor_advertisement *)skb->data; 324 if (skb->len < sizeof(*rp)) { 325 status = HCI_ERROR_UNSPECIFIED; 326 goto unlock; 327 } 328 329 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL); 330 if (!handle_data) { 331 status = HCI_ERROR_UNSPECIFIED; 332 goto unlock; 333 } 334 335 handle_data->mgmt_handle = monitor->handle; 336 handle_data->msft_handle = rp->handle; 337 INIT_LIST_HEAD(&handle_data->list); 338 list_add(&handle_data->list, &msft->handle_map); 339 340 monitor->state = ADV_MONITOR_STATE_OFFLOADED; 341 342 unlock: 343 if (status && monitor) 344 hci_free_adv_monitor(hdev, monitor); 345 346 /* If in restart/reregister sequence, keep registering. */ 347 if (msft->reregistering) 348 reregister_monitor_on_restart(hdev, 349 msft->pending_add_handle + 1); 350 351 hci_dev_unlock(hdev); 352 353 if (!msft->reregistering) 354 hci_add_adv_patterns_monitor_complete(hdev, status); 355 } 356 357 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, 358 u8 status, u16 opcode, 359 struct sk_buff *skb) 360 { 361 struct msft_cp_le_cancel_monitor_advertisement *cp; 362 struct msft_rp_le_cancel_monitor_advertisement *rp; 363 struct adv_monitor *monitor; 364 struct msft_monitor_advertisement_handle_data *handle_data; 365 struct msft_data *msft = hdev->msft_data; 366 int err; 367 bool pending; 368 369 if (status) 370 goto done; 371 372 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data; 373 if (skb->len < sizeof(*rp)) { 374 status = HCI_ERROR_UNSPECIFIED; 375 goto done; 376 } 377 378 hci_dev_lock(hdev); 379 380 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); 381 handle_data = msft_find_handle_data(hdev, cp->handle, false); 382 383 if (handle_data) { 384 monitor = idr_find(&hdev->adv_monitors_idr, 385 handle_data->mgmt_handle); 386 if (monitor) 387 hci_free_adv_monitor(hdev, monitor); 388 389 list_del(&handle_data->list); 390 kfree(handle_data); 391 } 392 393 /* If remove all monitors is required, we need to continue the process 394 * here because the earlier it was paused when waiting for the 395 * response from controller. 396 */ 397 if (msft->pending_remove_handle == 0) { 398 pending = hci_remove_all_adv_monitor(hdev, &err); 399 if (pending) { 400 hci_dev_unlock(hdev); 401 return; 402 } 403 404 if (err) 405 status = HCI_ERROR_UNSPECIFIED; 406 } 407 408 hci_dev_unlock(hdev); 409 410 done: 411 hci_remove_adv_monitor_complete(hdev, status); 412 } 413 414 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, 415 u8 status, u16 opcode, 416 struct sk_buff *skb) 417 { 418 struct msft_cp_le_set_advertisement_filter_enable *cp; 419 struct msft_rp_le_set_advertisement_filter_enable *rp; 420 struct msft_data *msft = hdev->msft_data; 421 422 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data; 423 if (skb->len < sizeof(*rp)) 424 return; 425 426 /* Error 0x0C would be returned if the filter enabled status is 427 * already set to whatever we were trying to set. 428 * Although the default state should be disabled, some controller set 429 * the initial value to enabled. Because there is no way to know the 430 * actual initial value before sending this command, here we also treat 431 * error 0x0C as success. 432 */ 433 if (status != 0x00 && status != 0x0C) 434 return; 435 436 hci_dev_lock(hdev); 437 438 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); 439 msft->filter_enabled = cp->enable; 440 441 if (status == 0x0C) 442 bt_dev_warn(hdev, "MSFT filter_enable is already %s", 443 cp->enable ? "on" : "off"); 444 445 hci_dev_unlock(hdev); 446 } 447 448 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor) 449 { 450 struct adv_rssi_thresholds *r = &monitor->rssi; 451 452 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 453 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX || 454 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || 455 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX) 456 return false; 457 458 /* High_threshold_timeout is not supported, 459 * once high_threshold is reached, events are immediately reported. 460 */ 461 if (r->high_threshold_timeout != 0) 462 return false; 463 464 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX) 465 return false; 466 467 /* Sampling period from 0x00 to 0xFF are all allowed */ 468 return true; 469 } 470 471 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor) 472 { 473 return msft_monitor_rssi_valid(monitor); 474 /* No additional check needed for pattern-based monitor */ 475 } 476 477 /* This function requires the caller holds hdev->lock */ 478 static int __msft_add_monitor_pattern(struct hci_dev *hdev, 479 struct adv_monitor *monitor) 480 { 481 struct msft_cp_le_monitor_advertisement *cp; 482 struct msft_le_monitor_advertisement_pattern_data *pattern_data; 483 struct msft_le_monitor_advertisement_pattern *pattern; 484 struct adv_pattern *entry; 485 struct hci_request req; 486 struct msft_data *msft = hdev->msft_data; 487 size_t total_size = sizeof(*cp) + sizeof(*pattern_data); 488 ptrdiff_t offset = 0; 489 u8 pattern_count = 0; 490 int err = 0; 491 492 if (!msft_monitor_pattern_valid(monitor)) 493 return -EINVAL; 494 495 list_for_each_entry(entry, &monitor->patterns, list) { 496 pattern_count++; 497 total_size += sizeof(*pattern) + entry->length; 498 } 499 500 cp = kmalloc(total_size, GFP_KERNEL); 501 if (!cp) 502 return -ENOMEM; 503 504 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; 505 cp->rssi_high = monitor->rssi.high_threshold; 506 cp->rssi_low = monitor->rssi.low_threshold; 507 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; 508 cp->rssi_sampling_period = monitor->rssi.sampling_period; 509 510 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; 511 512 pattern_data = (void *)cp->data; 513 pattern_data->count = pattern_count; 514 515 list_for_each_entry(entry, &monitor->patterns, list) { 516 pattern = (void *)(pattern_data->data + offset); 517 /* the length also includes data_type and offset */ 518 pattern->length = entry->length + 2; 519 pattern->data_type = entry->ad_type; 520 pattern->start_byte = entry->offset; 521 memcpy(pattern->pattern, entry->value, entry->length); 522 offset += sizeof(*pattern) + entry->length; 523 } 524 525 hci_req_init(&req, hdev); 526 hci_req_add(&req, hdev->msft_opcode, total_size, cp); 527 err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb); 528 kfree(cp); 529 530 if (!err) 531 msft->pending_add_handle = monitor->handle; 532 533 return err; 534 } 535 536 /* This function requires the caller holds hdev->lock */ 537 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) 538 { 539 struct msft_data *msft = hdev->msft_data; 540 541 if (!msft) 542 return -EOPNOTSUPP; 543 544 if (msft->reregistering) 545 return -EBUSY; 546 547 return __msft_add_monitor_pattern(hdev, monitor); 548 } 549 550 /* This function requires the caller holds hdev->lock */ 551 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor, 552 u16 handle) 553 { 554 struct msft_cp_le_cancel_monitor_advertisement cp; 555 struct msft_monitor_advertisement_handle_data *handle_data; 556 struct hci_request req; 557 struct msft_data *msft = hdev->msft_data; 558 int err = 0; 559 560 if (!msft) 561 return -EOPNOTSUPP; 562 563 if (msft->reregistering) 564 return -EBUSY; 565 566 handle_data = msft_find_handle_data(hdev, monitor->handle, true); 567 568 /* If no matched handle, just remove without telling controller */ 569 if (!handle_data) 570 return -ENOENT; 571 572 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; 573 cp.handle = handle_data->msft_handle; 574 575 hci_req_init(&req, hdev); 576 hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp); 577 err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb); 578 579 if (!err) 580 msft->pending_remove_handle = handle; 581 582 return err; 583 } 584 585 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable) 586 { 587 struct hci_dev *hdev = req->hdev; 588 struct msft_cp_le_set_advertisement_filter_enable cp; 589 590 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE; 591 cp.enable = enable; 592 593 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp); 594 } 595 596 int msft_set_filter_enable(struct hci_dev *hdev, bool enable) 597 { 598 struct hci_request req; 599 struct msft_data *msft = hdev->msft_data; 600 int err; 601 602 if (!msft) 603 return -EOPNOTSUPP; 604 605 hci_req_init(&req, hdev); 606 msft_req_add_set_filter_enable(&req, enable); 607 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb); 608 609 return err; 610 } 611 612 bool msft_curve_validity(struct hci_dev *hdev) 613 { 614 return hdev->msft_curve_validity; 615 } 616