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