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;
100 static int __msft_add_monitor_pattern(struct hci_dev *hdev,
101 struct adv_monitor *monitor);
103 bool msft_monitor_supported(struct hci_dev *hdev)
105 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
108 static bool read_supported_features(struct hci_dev *hdev,
109 struct msft_data *msft)
111 struct msft_cp_read_supported_features cp;
112 struct msft_rp_read_supported_features *rp;
115 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
117 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
120 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
125 if (skb->len < sizeof(*rp)) {
126 bt_dev_err(hdev, "MSFT supported features length mismatch");
130 rp = (struct msft_rp_read_supported_features *)skb->data;
132 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
135 if (rp->evt_prefix_len > 0) {
136 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
138 if (!msft->evt_prefix)
142 msft->evt_prefix_len = rp->evt_prefix_len;
143 msft->features = __le64_to_cpu(rp->features);
153 /* This function requires the caller holds hdev->lock */
154 static void reregister_monitor_on_restart(struct hci_dev *hdev, int handle)
156 struct adv_monitor *monitor;
157 struct msft_data *msft = hdev->msft_data;
161 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
163 /* All monitors have been reregistered */
164 msft->reregistering = false;
165 hci_update_background_scan(hdev);
169 msft->pending_add_handle = (u16)handle;
170 err = __msft_add_monitor_pattern(hdev, monitor);
172 /* If success, we return and wait for monitor added callback */
176 /* Otherwise remove the monitor and keep registering */
177 hci_free_adv_monitor(hdev, monitor);
182 void msft_do_open(struct hci_dev *hdev)
184 struct msft_data *msft;
186 if (hdev->msft_opcode == HCI_OP_NOP)
189 bt_dev_dbg(hdev, "Initialize MSFT extension");
191 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
195 if (!read_supported_features(hdev, msft)) {
200 INIT_LIST_HEAD(&msft->handle_map);
201 hdev->msft_data = msft;
203 if (msft_monitor_supported(hdev)) {
204 msft->reregistering = true;
205 msft_set_filter_enable(hdev, true);
206 reregister_monitor_on_restart(hdev, 0);
210 void msft_do_close(struct hci_dev *hdev)
212 struct msft_data *msft = hdev->msft_data;
213 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
214 struct adv_monitor *monitor;
219 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
221 hdev->msft_data = NULL;
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);
227 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
228 monitor->state = ADV_MONITOR_STATE_REGISTERED;
230 list_del(&handle_data->list);
234 kfree(msft->evt_prefix);
238 void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
240 struct msft_data *msft = hdev->msft_data;
246 /* When the extension has defined an event prefix, check that it
247 * matches, and otherwise just return.
249 if (msft->evt_prefix_len > 0) {
250 if (skb->len < msft->evt_prefix_len)
253 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
256 skb_pull(skb, msft->evt_prefix_len);
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.
268 bt_dev_dbg(hdev, "MSFT vendor event %u", event);
271 __u64 msft_get_features(struct hci_dev *hdev)
273 struct msft_data *msft = hdev->msft_data;
275 return msft ? msft->features : 0;
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
282 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
283 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
285 struct msft_monitor_advertisement_handle_data *entry;
286 struct msft_data *msft = hdev->msft_data;
288 list_for_each_entry(entry, &msft->handle_map, list) {
289 if (is_mgmt && entry->mgmt_handle == handle)
291 if (!is_mgmt && entry->msft_handle == handle)
298 static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
299 u8 status, u16 opcode,
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;
309 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
311 bt_dev_err(hdev, "msft add advmon: monitor %d is not found!",
312 msft->pending_add_handle);
313 status = HCI_ERROR_UNSPECIFIED;
320 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
321 if (skb->len < sizeof(*rp)) {
322 status = HCI_ERROR_UNSPECIFIED;
326 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
328 status = HCI_ERROR_UNSPECIFIED;
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);
337 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
340 if (status && monitor)
341 hci_free_adv_monitor(hdev, monitor);
343 /* If in restart/reregister sequence, keep registering. */
344 if (msft->reregistering)
345 reregister_monitor_on_restart(hdev,
346 msft->pending_add_handle + 1);
348 hci_dev_unlock(hdev);
350 if (!msft->reregistering)
351 hci_add_adv_patterns_monitor_complete(hdev, status);
354 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
355 u8 status, u16 opcode,
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;
369 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
370 if (skb->len < sizeof(*rp)) {
371 status = HCI_ERROR_UNSPECIFIED;
377 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
378 handle_data = msft_find_handle_data(hdev, cp->handle, false);
381 monitor = idr_find(&hdev->adv_monitors_idr,
382 handle_data->mgmt_handle);
384 hci_free_adv_monitor(hdev, monitor);
386 list_del(&handle_data->list);
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.
394 if (msft->pending_remove_handle == 0) {
395 pending = hci_remove_all_adv_monitor(hdev, &err);
397 hci_dev_unlock(hdev);
402 status = HCI_ERROR_UNSPECIFIED;
405 hci_dev_unlock(hdev);
408 hci_remove_adv_monitor_complete(hdev, status);
411 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
412 u8 status, u16 opcode,
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;
419 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
420 if (skb->len < sizeof(*rp))
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.
430 if (status != 0x00 && status != 0x0C)
435 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
436 msft->filter_enabled = cp->enable;
439 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
440 cp->enable ? "on" : "off");
442 hci_dev_unlock(hdev);
445 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
447 struct adv_rssi_thresholds *r = &monitor->rssi;
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)
455 /* High_threshold_timeout is not supported,
456 * once high_threshold is reached, events are immediately reported.
458 if (r->high_threshold_timeout != 0)
461 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
464 /* Sampling period from 0x00 to 0xFF are all allowed */
468 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
470 return msft_monitor_rssi_valid(monitor);
471 /* No additional check needed for pattern-based monitor */
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)
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;
489 if (!msft_monitor_pattern_valid(monitor))
492 list_for_each_entry(entry, &monitor->patterns, list) {
494 total_size += sizeof(*pattern) + entry->length;
497 cp = kmalloc(total_size, GFP_KERNEL);
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;
507 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
509 pattern_data = (void *)cp->data;
510 pattern_data->count = pattern_count;
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;
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);
528 msft->pending_add_handle = monitor->handle;
533 /* This function requires the caller holds hdev->lock */
534 int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
536 struct msft_data *msft = hdev->msft_data;
541 if (msft->reregistering)
544 return __msft_add_monitor_pattern(hdev, monitor);
547 /* This function requires the caller holds hdev->lock */
548 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
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;
560 if (msft->reregistering)
563 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
565 /* If no matched handle, just remove without telling controller */
569 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
570 cp.handle = handle_data->msft_handle;
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);
577 msft->pending_remove_handle = handle;
582 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
584 struct hci_dev *hdev = req->hdev;
585 struct msft_cp_le_set_advertisement_filter_enable cp;
587 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
590 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
593 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
595 struct hci_request req;
596 struct msft_data *msft = hdev->msft_data;
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);