1 // SPDX-License-Identifier: GPL-2.0-only
3 * Copyright (C) 2020 Google Corporation
6 #include <net/bluetooth/bluetooth.h>
7 #include <net/bluetooth/hci_core.h>
8 #include <net/bluetooth/mgmt.h>
10 #include "hci_request.h"
11 #include "mgmt_util.h"
14 #define MSFT_RSSI_THRESHOLD_VALUE_MIN -127
15 #define MSFT_RSSI_THRESHOLD_VALUE_MAX 20
16 #define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C
18 #define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
19 struct msft_cp_read_supported_features {
23 struct msft_rp_read_supported_features {
31 #define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03
32 #define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01
33 struct msft_le_monitor_advertisement_pattern {
40 struct msft_le_monitor_advertisement_pattern_data {
45 struct msft_cp_le_monitor_advertisement {
49 __u8 rssi_low_interval;
50 __u8 rssi_sampling_period;
55 struct msft_rp_le_monitor_advertisement {
61 #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04
62 struct msft_cp_le_cancel_monitor_advertisement {
67 struct msft_rp_le_cancel_monitor_advertisement {
72 #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05
73 struct msft_cp_le_set_advertisement_filter_enable {
78 struct msft_rp_le_set_advertisement_filter_enable {
83 struct msft_monitor_advertisement_handle_data {
86 struct list_head list;
93 struct list_head handle_map;
94 __u16 pending_add_handle;
95 __u16 pending_remove_handle;
101 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
102 struct adv_monitor *monitor);
103 static int __msft_remove_monitor(struct hci_dev *hdev,
104 struct adv_monitor *monitor, u16 handle);
106 bool msft_monitor_supported(struct hci_dev *hdev)
108 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
111 static bool read_supported_features(struct hci_dev *hdev,
112 struct msft_data *msft)
114 struct msft_cp_read_supported_features cp;
115 struct msft_rp_read_supported_features *rp;
118 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
120 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
123 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
128 if (skb->len < sizeof(*rp)) {
129 bt_dev_err(hdev, "MSFT supported features length mismatch");
133 rp = (struct msft_rp_read_supported_features *)skb->data;
135 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
138 if (rp->evt_prefix_len > 0) {
139 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
141 if (!msft->evt_prefix)
145 msft->evt_prefix_len = rp->evt_prefix_len;
146 msft->features = __le64_to_cpu(rp->features);
148 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
149 hdev->msft_curve_validity = true;
159 /* This function requires the caller holds hdev->lock */
160 static void reregister_monitor(struct hci_dev *hdev, int handle)
162 struct adv_monitor *monitor;
163 struct msft_data *msft = hdev->msft_data;
167 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
169 /* All monitors have been reregistered */
170 msft->reregistering = false;
171 hci_update_background_scan(hdev);
175 msft->pending_add_handle = (u16)handle;
176 err = __msft_add_monitor_pattern(hdev, monitor);
178 /* If success, we return and wait for monitor added callback */
182 /* Otherwise remove the monitor and keep registering */
183 hci_free_adv_monitor(hdev, monitor);
188 /* This function requires the caller holds hdev->lock */
189 static void remove_monitor_on_suspend(struct hci_dev *hdev, int handle)
191 struct adv_monitor *monitor;
192 struct msft_data *msft = hdev->msft_data;
196 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
198 /* All monitors have been removed */
199 msft->suspending = false;
200 hci_update_background_scan(hdev);
204 msft->pending_remove_handle = (u16)handle;
205 err = __msft_remove_monitor(hdev, monitor, handle);
207 /* If success, return and wait for monitor removed callback */
211 /* Otherwise free the monitor and keep removing */
212 hci_free_adv_monitor(hdev, monitor);
217 /* This function requires the caller holds hdev->lock */
218 void msft_suspend(struct hci_dev *hdev)
220 struct msft_data *msft = hdev->msft_data;
225 if (msft_monitor_supported(hdev)) {
226 msft->suspending = true;
227 /* Quitely remove all monitors on suspend to avoid waking up
230 remove_monitor_on_suspend(hdev, 0);
234 /* This function requires the caller holds hdev->lock */
235 void msft_resume(struct hci_dev *hdev)
237 struct msft_data *msft = hdev->msft_data;
242 if (msft_monitor_supported(hdev)) {
243 msft->reregistering = true;
244 /* Monitors are removed on suspend, so we need to add all
245 * monitors on resume.
247 reregister_monitor(hdev, 0);
251 void msft_do_open(struct hci_dev *hdev)
253 struct msft_data *msft = hdev->msft_data;
255 if (hdev->msft_opcode == HCI_OP_NOP)
259 bt_dev_err(hdev, "MSFT extension not registered");
263 bt_dev_dbg(hdev, "Initialize MSFT extension");
265 /* Reset existing MSFT data before re-reading */
266 kfree(msft->evt_prefix);
267 msft->evt_prefix = NULL;
268 msft->evt_prefix_len = 0;
271 if (!read_supported_features(hdev, msft)) {
272 hdev->msft_data = NULL;
277 if (msft_monitor_supported(hdev)) {
278 msft->reregistering = true;
279 msft_set_filter_enable(hdev, true);
280 /* Monitors get removed on power off, so we need to explicitly
281 * tell the controller to re-monitor.
283 reregister_monitor(hdev, 0);
287 void msft_do_close(struct hci_dev *hdev)
289 struct msft_data *msft = hdev->msft_data;
290 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
291 struct adv_monitor *monitor;
296 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
298 /* The controller will silently remove all monitors on power off.
299 * Therefore, remove handle_data mapping and reset monitor state.
301 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
302 monitor = idr_find(&hdev->adv_monitors_idr,
303 handle_data->mgmt_handle);
305 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
306 monitor->state = ADV_MONITOR_STATE_REGISTERED;
308 list_del(&handle_data->list);
313 void msft_register(struct hci_dev *hdev)
315 struct msft_data *msft = NULL;
317 bt_dev_dbg(hdev, "Register MSFT extension");
319 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
321 bt_dev_err(hdev, "Failed to register MSFT extension");
325 INIT_LIST_HEAD(&msft->handle_map);
326 hdev->msft_data = msft;
329 void msft_unregister(struct hci_dev *hdev)
331 struct msft_data *msft = hdev->msft_data;
336 bt_dev_dbg(hdev, "Unregister MSFT extension");
338 hdev->msft_data = NULL;
340 kfree(msft->evt_prefix);
344 void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
346 struct msft_data *msft = hdev->msft_data;
352 /* When the extension has defined an event prefix, check that it
353 * matches, and otherwise just return.
355 if (msft->evt_prefix_len > 0) {
356 if (skb->len < msft->evt_prefix_len)
359 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
362 skb_pull(skb, msft->evt_prefix_len);
365 /* Every event starts at least with an event code and the rest of
366 * the data is variable and depends on the event code.
374 bt_dev_dbg(hdev, "MSFT vendor event %u", event);
377 __u64 msft_get_features(struct hci_dev *hdev)
379 struct msft_data *msft = hdev->msft_data;
381 return msft ? msft->features : 0;
384 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
385 * is_mgmt = false matches the handle used by the msft controller.
386 * This function requires the caller holds hdev->lock
388 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
389 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
391 struct msft_monitor_advertisement_handle_data *entry;
392 struct msft_data *msft = hdev->msft_data;
394 list_for_each_entry(entry, &msft->handle_map, list) {
395 if (is_mgmt && entry->mgmt_handle == handle)
397 if (!is_mgmt && entry->msft_handle == handle)
404 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
405 u8 status, u16 opcode,
408 struct msft_rp_le_monitor_advertisement *rp;
409 struct adv_monitor *monitor;
410 struct msft_monitor_advertisement_handle_data *handle_data;
411 struct msft_data *msft = hdev->msft_data;
415 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
417 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
418 msft->pending_add_handle);
419 status = HCI_ERROR_UNSPECIFIED;
426 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
427 if (skb->len < sizeof(*rp)) {
428 status = HCI_ERROR_UNSPECIFIED;
432 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
434 status = HCI_ERROR_UNSPECIFIED;
438 handle_data->mgmt_handle = monitor->handle;
439 handle_data->msft_handle = rp->handle;
440 INIT_LIST_HEAD(&handle_data->list);
441 list_add(&handle_data->list, &msft->handle_map);
443 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
446 if (status && monitor)
447 hci_free_adv_monitor(hdev, monitor);
449 /* If in restart/reregister sequence, keep registering. */
450 if (msft->reregistering)
451 reregister_monitor(hdev, msft->pending_add_handle + 1);
453 hci_dev_unlock(hdev);
455 if (!msft->reregistering)
456 hci_add_adv_patterns_monitor_complete(hdev, status);
459 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
460 u8 status, u16 opcode,
463 struct msft_cp_le_cancel_monitor_advertisement *cp;
464 struct msft_rp_le_cancel_monitor_advertisement *rp;
465 struct adv_monitor *monitor;
466 struct msft_monitor_advertisement_handle_data *handle_data;
467 struct msft_data *msft = hdev->msft_data;
474 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
475 if (skb->len < sizeof(*rp)) {
476 status = HCI_ERROR_UNSPECIFIED;
482 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
483 handle_data = msft_find_handle_data(hdev, cp->handle, false);
486 monitor = idr_find(&hdev->adv_monitors_idr,
487 handle_data->mgmt_handle);
489 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
490 monitor->state = ADV_MONITOR_STATE_REGISTERED;
492 /* Do not free the monitor if it is being removed due to
493 * suspend. It will be re-monitored on resume.
495 if (monitor && !msft->suspending)
496 hci_free_adv_monitor(hdev, monitor);
498 list_del(&handle_data->list);
502 /* If in suspend/remove sequence, keep removing. */
503 if (msft->suspending)
504 remove_monitor_on_suspend(hdev,
505 msft->pending_remove_handle + 1);
507 /* If remove all monitors is required, we need to continue the process
508 * here because the earlier it was paused when waiting for the
509 * response from controller.
511 if (msft->pending_remove_handle == 0) {
512 pending = hci_remove_all_adv_monitor(hdev, &err);
514 hci_dev_unlock(hdev);
519 status = HCI_ERROR_UNSPECIFIED;
522 hci_dev_unlock(hdev);
525 if (!msft->suspending)
526 hci_remove_adv_monitor_complete(hdev, status);
529 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
530 u8 status, u16 opcode,
533 struct msft_cp_le_set_advertisement_filter_enable *cp;
534 struct msft_rp_le_set_advertisement_filter_enable *rp;
535 struct msft_data *msft = hdev->msft_data;
537 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
538 if (skb->len < sizeof(*rp))
541 /* Error 0x0C would be returned if the filter enabled status is
542 * already set to whatever we were trying to set.
543 * Although the default state should be disabled, some controller set
544 * the initial value to enabled. Because there is no way to know the
545 * actual initial value before sending this command, here we also treat
546 * error 0x0C as success.
548 if (status != 0x00 && status != 0x0C)
553 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
554 msft->filter_enabled = cp->enable;
557 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
558 cp->enable ? "on" : "off");
560 hci_dev_unlock(hdev);
563 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
565 struct adv_rssi_thresholds *r = &monitor->rssi;
567 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
568 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
569 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
570 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
573 /* High_threshold_timeout is not supported,
574 * once high_threshold is reached, events are immediately reported.
576 if (r->high_threshold_timeout != 0)
579 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
582 /* Sampling period from 0x00 to 0xFF are all allowed */
586 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
588 return msft_monitor_rssi_valid(monitor);
589 /* No additional check needed for pattern-based monitor */
592 /* This function requires the caller holds hdev->lock */
593 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
594 struct adv_monitor *monitor)
596 struct msft_cp_le_monitor_advertisement *cp;
597 struct msft_le_monitor_advertisement_pattern_data *pattern_data;
598 struct msft_le_monitor_advertisement_pattern *pattern;
599 struct adv_pattern *entry;
600 struct hci_request req;
601 struct msft_data *msft = hdev->msft_data;
602 size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
603 ptrdiff_t offset = 0;
604 u8 pattern_count = 0;
607 if (!msft_monitor_pattern_valid(monitor))
610 list_for_each_entry(entry, &monitor->patterns, list) {
612 total_size += sizeof(*pattern) + entry->length;
615 cp = kmalloc(total_size, GFP_KERNEL);
619 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
620 cp->rssi_high = monitor->rssi.high_threshold;
621 cp->rssi_low = monitor->rssi.low_threshold;
622 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
623 cp->rssi_sampling_period = monitor->rssi.sampling_period;
625 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
627 pattern_data = (void *)cp->data;
628 pattern_data->count = pattern_count;
630 list_for_each_entry(entry, &monitor->patterns, list) {
631 pattern = (void *)(pattern_data->data + offset);
632 /* the length also includes data_type and offset */
633 pattern->length = entry->length + 2;
634 pattern->data_type = entry->ad_type;
635 pattern->start_byte = entry->offset;
636 memcpy(pattern->pattern, entry->value, entry->length);
637 offset += sizeof(*pattern) + entry->length;
640 hci_req_init(&req, hdev);
641 hci_req_add(&req, hdev->msft_opcode, total_size, cp);
642 err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
646 msft->pending_add_handle = monitor->handle;
651 /* This function requires the caller holds hdev->lock */
652 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
654 struct msft_data *msft = hdev->msft_data;
659 if (msft->reregistering || msft->suspending)
662 return __msft_add_monitor_pattern(hdev, monitor);
665 /* This function requires the caller holds hdev->lock */
666 static int __msft_remove_monitor(struct hci_dev *hdev,
667 struct adv_monitor *monitor, u16 handle)
669 struct msft_cp_le_cancel_monitor_advertisement cp;
670 struct msft_monitor_advertisement_handle_data *handle_data;
671 struct hci_request req;
672 struct msft_data *msft = hdev->msft_data;
675 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
677 /* If no matched handle, just remove without telling controller */
681 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
682 cp.handle = handle_data->msft_handle;
684 hci_req_init(&req, hdev);
685 hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
686 err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
689 msft->pending_remove_handle = handle;
694 /* This function requires the caller holds hdev->lock */
695 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
698 struct msft_data *msft = hdev->msft_data;
703 if (msft->reregistering || msft->suspending)
706 return __msft_remove_monitor(hdev, monitor, handle);
709 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
711 struct hci_dev *hdev = req->hdev;
712 struct msft_cp_le_set_advertisement_filter_enable cp;
714 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
717 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
720 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
722 struct hci_request req;
723 struct msft_data *msft = hdev->msft_data;
729 hci_req_init(&req, hdev);
730 msft_req_add_set_filter_enable(&req, enable);
731 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
736 bool msft_curve_validity(struct hci_dev *hdev)
738 return hdev->msft_curve_validity;