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 static void reregister_monitor(struct hci_dev *hdev, int handle)
161 struct adv_monitor *monitor;
162 struct msft_data *msft = hdev->msft_data;
166 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
168 /* All monitors have been resumed */
169 msft->resuming = false;
170 hci_update_passive_scan(hdev);
174 msft->pending_add_handle = (u16)handle;
175 err = __msft_add_monitor_pattern(hdev, monitor);
177 /* If success, we return and wait for monitor added callback */
181 /* Otherwise remove the monitor and keep registering */
182 hci_free_adv_monitor(hdev, monitor);
187 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
188 * is_mgmt = false matches the handle used by the msft controller.
189 * This function requires the caller holds hdev->lock
191 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
192 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
194 struct msft_monitor_advertisement_handle_data *entry;
195 struct msft_data *msft = hdev->msft_data;
197 list_for_each_entry(entry, &msft->handle_map, list) {
198 if (is_mgmt && entry->mgmt_handle == handle)
200 if (!is_mgmt && entry->msft_handle == handle)
207 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
208 u8 status, u16 opcode,
211 struct msft_rp_le_monitor_advertisement *rp;
212 struct adv_monitor *monitor;
213 struct msft_monitor_advertisement_handle_data *handle_data;
214 struct msft_data *msft = hdev->msft_data;
218 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
220 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
221 msft->pending_add_handle);
222 status = HCI_ERROR_UNSPECIFIED;
229 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
230 if (skb->len < sizeof(*rp)) {
231 status = HCI_ERROR_UNSPECIFIED;
235 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
237 status = HCI_ERROR_UNSPECIFIED;
241 handle_data->mgmt_handle = monitor->handle;
242 handle_data->msft_handle = rp->handle;
243 INIT_LIST_HEAD(&handle_data->list);
244 list_add(&handle_data->list, &msft->handle_map);
246 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
249 if (status && monitor)
250 hci_free_adv_monitor(hdev, monitor);
252 hci_dev_unlock(hdev);
255 hci_add_adv_patterns_monitor_complete(hdev, status);
258 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
259 u8 status, u16 opcode,
262 struct msft_cp_le_cancel_monitor_advertisement *cp;
263 struct msft_rp_le_cancel_monitor_advertisement *rp;
264 struct adv_monitor *monitor;
265 struct msft_monitor_advertisement_handle_data *handle_data;
266 struct msft_data *msft = hdev->msft_data;
273 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
274 if (skb->len < sizeof(*rp)) {
275 status = HCI_ERROR_UNSPECIFIED;
281 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
282 handle_data = msft_find_handle_data(hdev, cp->handle, false);
285 monitor = idr_find(&hdev->adv_monitors_idr,
286 handle_data->mgmt_handle);
288 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
289 monitor->state = ADV_MONITOR_STATE_REGISTERED;
291 /* Do not free the monitor if it is being removed due to
292 * suspend. It will be re-monitored on resume.
294 if (monitor && !msft->suspending)
295 hci_free_adv_monitor(hdev, monitor);
297 list_del(&handle_data->list);
301 /* If remove all monitors is required, we need to continue the process
302 * here because the earlier it was paused when waiting for the
303 * response from controller.
305 if (msft->pending_remove_handle == 0) {
306 pending = hci_remove_all_adv_monitor(hdev, &err);
308 hci_dev_unlock(hdev);
313 status = HCI_ERROR_UNSPECIFIED;
316 hci_dev_unlock(hdev);
319 if (!msft->suspending)
320 hci_remove_adv_monitor_complete(hdev, status);
323 static int msft_remove_monitor_sync(struct hci_dev *hdev,
324 struct adv_monitor *monitor)
326 struct msft_cp_le_cancel_monitor_advertisement cp;
327 struct msft_monitor_advertisement_handle_data *handle_data;
331 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
333 /* If no matched handle, just remove without telling controller */
337 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
338 cp.handle = handle_data->msft_handle;
340 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
345 status = skb->data[0];
348 msft_le_cancel_monitor_advertisement_cb(hdev, status, hdev->msft_opcode,
354 /* This function requires the caller holds hci_req_sync_lock */
355 int msft_suspend_sync(struct hci_dev *hdev)
357 struct msft_data *msft = hdev->msft_data;
358 struct adv_monitor *monitor;
361 if (!msft || !msft_monitor_supported(hdev))
364 msft->suspending = true;
367 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
371 msft_remove_monitor_sync(hdev, monitor);
376 /* All monitors have been removed */
377 msft->suspending = false;
382 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
384 struct adv_rssi_thresholds *r = &monitor->rssi;
386 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
387 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
388 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
389 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
392 /* High_threshold_timeout is not supported,
393 * once high_threshold is reached, events are immediately reported.
395 if (r->high_threshold_timeout != 0)
398 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
401 /* Sampling period from 0x00 to 0xFF are all allowed */
405 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
407 return msft_monitor_rssi_valid(monitor);
408 /* No additional check needed for pattern-based monitor */
411 static int msft_add_monitor_sync(struct hci_dev *hdev,
412 struct adv_monitor *monitor)
414 struct msft_cp_le_monitor_advertisement *cp;
415 struct msft_le_monitor_advertisement_pattern_data *pattern_data;
416 struct msft_le_monitor_advertisement_pattern *pattern;
417 struct adv_pattern *entry;
418 size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
419 ptrdiff_t offset = 0;
420 u8 pattern_count = 0;
424 if (!msft_monitor_pattern_valid(monitor))
427 list_for_each_entry(entry, &monitor->patterns, list) {
429 total_size += sizeof(*pattern) + entry->length;
432 cp = kmalloc(total_size, GFP_KERNEL);
436 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
437 cp->rssi_high = monitor->rssi.high_threshold;
438 cp->rssi_low = monitor->rssi.low_threshold;
439 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
440 cp->rssi_sampling_period = monitor->rssi.sampling_period;
442 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
444 pattern_data = (void *)cp->data;
445 pattern_data->count = pattern_count;
447 list_for_each_entry(entry, &monitor->patterns, list) {
448 pattern = (void *)(pattern_data->data + offset);
449 /* the length also includes data_type and offset */
450 pattern->length = entry->length + 2;
451 pattern->data_type = entry->ad_type;
452 pattern->start_byte = entry->offset;
453 memcpy(pattern->pattern, entry->value, entry->length);
454 offset += sizeof(*pattern) + entry->length;
457 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
464 status = skb->data[0];
467 msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb);
472 /* This function requires the caller holds hci_req_sync_lock */
473 int msft_resume_sync(struct hci_dev *hdev)
475 struct msft_data *msft = hdev->msft_data;
476 struct adv_monitor *monitor;
479 if (!msft || !msft_monitor_supported(hdev))
482 msft->resuming = true;
485 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
489 msft_add_monitor_sync(hdev, monitor);
494 /* All monitors have been resumed */
495 msft->resuming = false;
500 void msft_do_open(struct hci_dev *hdev)
502 struct msft_data *msft = hdev->msft_data;
504 if (hdev->msft_opcode == HCI_OP_NOP)
508 bt_dev_err(hdev, "MSFT extension not registered");
512 bt_dev_dbg(hdev, "Initialize MSFT extension");
514 /* Reset existing MSFT data before re-reading */
515 kfree(msft->evt_prefix);
516 msft->evt_prefix = NULL;
517 msft->evt_prefix_len = 0;
520 if (!read_supported_features(hdev, msft)) {
521 hdev->msft_data = NULL;
526 if (msft_monitor_supported(hdev)) {
527 msft->resuming = true;
528 msft_set_filter_enable(hdev, true);
529 /* Monitors get removed on power off, so we need to explicitly
530 * tell the controller to re-monitor.
532 reregister_monitor(hdev, 0);
536 void msft_do_close(struct hci_dev *hdev)
538 struct msft_data *msft = hdev->msft_data;
539 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
540 struct adv_monitor *monitor;
545 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
547 /* The controller will silently remove all monitors on power off.
548 * Therefore, remove handle_data mapping and reset monitor state.
550 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
551 monitor = idr_find(&hdev->adv_monitors_idr,
552 handle_data->mgmt_handle);
554 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
555 monitor->state = ADV_MONITOR_STATE_REGISTERED;
557 list_del(&handle_data->list);
562 void msft_register(struct hci_dev *hdev)
564 struct msft_data *msft = NULL;
566 bt_dev_dbg(hdev, "Register MSFT extension");
568 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
570 bt_dev_err(hdev, "Failed to register MSFT extension");
574 INIT_LIST_HEAD(&msft->handle_map);
575 hdev->msft_data = msft;
578 void msft_unregister(struct hci_dev *hdev)
580 struct msft_data *msft = hdev->msft_data;
585 bt_dev_dbg(hdev, "Unregister MSFT extension");
587 hdev->msft_data = NULL;
589 kfree(msft->evt_prefix);
593 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
595 struct msft_data *msft = hdev->msft_data;
601 /* When the extension has defined an event prefix, check that it
602 * matches, and otherwise just return.
604 if (msft->evt_prefix_len > 0) {
605 if (skb->len < msft->evt_prefix_len)
608 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
611 skb_pull(skb, msft->evt_prefix_len);
614 /* Every event starts at least with an event code and the rest of
615 * the data is variable and depends on the event code.
623 bt_dev_dbg(hdev, "MSFT vendor event %u", event);
626 __u64 msft_get_features(struct hci_dev *hdev)
628 struct msft_data *msft = hdev->msft_data;
630 return msft ? msft->features : 0;
633 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
634 u8 status, u16 opcode,
637 struct msft_cp_le_set_advertisement_filter_enable *cp;
638 struct msft_rp_le_set_advertisement_filter_enable *rp;
639 struct msft_data *msft = hdev->msft_data;
641 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
642 if (skb->len < sizeof(*rp))
645 /* Error 0x0C would be returned if the filter enabled status is
646 * already set to whatever we were trying to set.
647 * Although the default state should be disabled, some controller set
648 * the initial value to enabled. Because there is no way to know the
649 * actual initial value before sending this command, here we also treat
650 * error 0x0C as success.
652 if (status != 0x00 && status != 0x0C)
657 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
658 msft->filter_enabled = cp->enable;
661 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
662 cp->enable ? "on" : "off");
664 hci_dev_unlock(hdev);
667 /* This function requires the caller holds hdev->lock */
668 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
669 struct adv_monitor *monitor)
671 struct msft_cp_le_monitor_advertisement *cp;
672 struct msft_le_monitor_advertisement_pattern_data *pattern_data;
673 struct msft_le_monitor_advertisement_pattern *pattern;
674 struct adv_pattern *entry;
675 struct hci_request req;
676 struct msft_data *msft = hdev->msft_data;
677 size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
678 ptrdiff_t offset = 0;
679 u8 pattern_count = 0;
682 if (!msft_monitor_pattern_valid(monitor))
685 list_for_each_entry(entry, &monitor->patterns, list) {
687 total_size += sizeof(*pattern) + entry->length;
690 cp = kmalloc(total_size, GFP_KERNEL);
694 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
695 cp->rssi_high = monitor->rssi.high_threshold;
696 cp->rssi_low = monitor->rssi.low_threshold;
697 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
698 cp->rssi_sampling_period = monitor->rssi.sampling_period;
700 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
702 pattern_data = (void *)cp->data;
703 pattern_data->count = pattern_count;
705 list_for_each_entry(entry, &monitor->patterns, list) {
706 pattern = (void *)(pattern_data->data + offset);
707 /* the length also includes data_type and offset */
708 pattern->length = entry->length + 2;
709 pattern->data_type = entry->ad_type;
710 pattern->start_byte = entry->offset;
711 memcpy(pattern->pattern, entry->value, entry->length);
712 offset += sizeof(*pattern) + entry->length;
715 hci_req_init(&req, hdev);
716 hci_req_add(&req, hdev->msft_opcode, total_size, cp);
717 err = hci_req_run_skb(&req, msft_le_monitor_advertisement_cb);
721 msft->pending_add_handle = monitor->handle;
726 /* This function requires the caller holds hdev->lock */
727 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
729 struct msft_data *msft = hdev->msft_data;
734 if (msft->resuming || msft->suspending)
737 return __msft_add_monitor_pattern(hdev, monitor);
740 /* This function requires the caller holds hdev->lock */
741 static int __msft_remove_monitor(struct hci_dev *hdev,
742 struct adv_monitor *monitor, u16 handle)
744 struct msft_cp_le_cancel_monitor_advertisement cp;
745 struct msft_monitor_advertisement_handle_data *handle_data;
746 struct hci_request req;
747 struct msft_data *msft = hdev->msft_data;
750 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
752 /* If no matched handle, just remove without telling controller */
756 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
757 cp.handle = handle_data->msft_handle;
759 hci_req_init(&req, hdev);
760 hci_req_add(&req, hdev->msft_opcode, sizeof(cp), &cp);
761 err = hci_req_run_skb(&req, msft_le_cancel_monitor_advertisement_cb);
764 msft->pending_remove_handle = handle;
769 /* This function requires the caller holds hdev->lock */
770 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
773 struct msft_data *msft = hdev->msft_data;
778 if (msft->resuming || msft->suspending)
781 return __msft_remove_monitor(hdev, monitor, handle);
784 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
786 struct hci_dev *hdev = req->hdev;
787 struct msft_cp_le_set_advertisement_filter_enable cp;
789 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
792 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
795 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
797 struct hci_request req;
798 struct msft_data *msft = hdev->msft_data;
804 hci_req_init(&req, hdev);
805 msft_req_add_set_filter_enable(&req, enable);
806 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
811 bool msft_curve_validity(struct hci_dev *hdev)
813 return hdev->msft_curve_validity;