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 #define MSFT_EV_LE_MONITOR_DEVICE 0x02
84 struct msft_ev_le_monitor_device {
91 struct msft_monitor_advertisement_handle_data {
94 struct list_head list;
101 struct list_head handle_map;
107 bool msft_monitor_supported(struct hci_dev *hdev)
109 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
112 static bool read_supported_features(struct hci_dev *hdev,
113 struct msft_data *msft)
115 struct msft_cp_read_supported_features cp;
116 struct msft_rp_read_supported_features *rp;
119 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
121 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
124 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
129 if (skb->len < sizeof(*rp)) {
130 bt_dev_err(hdev, "MSFT supported features length mismatch");
134 rp = (struct msft_rp_read_supported_features *)skb->data;
136 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
139 if (rp->evt_prefix_len > 0) {
140 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
142 if (!msft->evt_prefix)
146 msft->evt_prefix_len = rp->evt_prefix_len;
147 msft->features = __le64_to_cpu(rp->features);
149 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
150 hdev->msft_curve_validity = true;
160 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
161 * is_mgmt = false matches the handle used by the msft controller.
162 * This function requires the caller holds hdev->lock
164 static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
165 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
167 struct msft_monitor_advertisement_handle_data *entry;
168 struct msft_data *msft = hdev->msft_data;
170 list_for_each_entry(entry, &msft->handle_map, list) {
171 if (is_mgmt && entry->mgmt_handle == handle)
173 if (!is_mgmt && entry->msft_handle == handle)
180 /* This function requires the caller holds hdev->lock */
181 static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle,
182 bdaddr_t *bdaddr, __u8 addr_type,
185 struct monitored_device *dev, *tmp;
188 list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) {
189 /* mgmt_handle == 0 indicates remove all devices, whereas,
190 * bdaddr == NULL indicates remove all devices matching the
193 if ((!mgmt_handle || dev->handle == mgmt_handle) &&
194 (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) &&
195 addr_type == dev->addr_type))) {
196 if (notify && dev->notified) {
197 mgmt_adv_monitor_device_lost(hdev, dev->handle,
202 list_del(&dev->list);
211 static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode,
212 struct adv_monitor *monitor,
215 struct msft_rp_le_monitor_advertisement *rp;
216 struct msft_monitor_advertisement_handle_data *handle_data;
217 struct msft_data *msft = hdev->msft_data;
222 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
223 if (skb->len < sizeof(*rp)) {
224 status = HCI_ERROR_UNSPECIFIED;
232 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
234 status = HCI_ERROR_UNSPECIFIED;
238 handle_data->mgmt_handle = monitor->handle;
239 handle_data->msft_handle = rp->handle;
240 INIT_LIST_HEAD(&handle_data->list);
241 list_add(&handle_data->list, &msft->handle_map);
243 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
247 hci_free_adv_monitor(hdev, monitor);
249 hci_dev_unlock(hdev);
254 static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
256 struct adv_monitor *monitor,
259 struct msft_rp_le_cancel_monitor_advertisement *rp;
260 struct msft_monitor_advertisement_handle_data *handle_data;
261 struct msft_data *msft = hdev->msft_data;
264 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
265 if (skb->len < sizeof(*rp)) {
266 status = HCI_ERROR_UNSPECIFIED;
276 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
279 if (monitor->state == ADV_MONITOR_STATE_OFFLOADED)
280 monitor->state = ADV_MONITOR_STATE_REGISTERED;
282 /* Do not free the monitor if it is being removed due to
283 * suspend. It will be re-monitored on resume.
285 if (!msft->suspending) {
286 hci_free_adv_monitor(hdev, monitor);
288 /* Clear any monitored devices by this Adv Monitor */
289 msft_monitor_device_del(hdev, handle_data->mgmt_handle,
293 list_del(&handle_data->list);
297 hci_dev_unlock(hdev);
303 /* This function requires the caller holds hci_req_sync_lock */
304 static int msft_remove_monitor_sync(struct hci_dev *hdev,
305 struct adv_monitor *monitor)
307 struct msft_cp_le_cancel_monitor_advertisement cp;
308 struct msft_monitor_advertisement_handle_data *handle_data;
311 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
313 /* If no matched handle, just remove without telling controller */
317 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
318 cp.handle = handle_data->msft_handle;
320 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
325 return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode,
329 /* This function requires the caller holds hci_req_sync_lock */
330 int msft_suspend_sync(struct hci_dev *hdev)
332 struct msft_data *msft = hdev->msft_data;
333 struct adv_monitor *monitor;
336 if (!msft || !msft_monitor_supported(hdev))
339 msft->suspending = true;
342 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
346 msft_remove_monitor_sync(hdev, monitor);
351 /* All monitors have been removed */
352 msft->suspending = false;
357 static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
359 struct adv_rssi_thresholds *r = &monitor->rssi;
361 if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
362 r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX ||
363 r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN ||
364 r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX)
367 /* High_threshold_timeout is not supported,
368 * once high_threshold is reached, events are immediately reported.
370 if (r->high_threshold_timeout != 0)
373 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
376 /* Sampling period from 0x00 to 0xFF are all allowed */
380 static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
382 return msft_monitor_rssi_valid(monitor);
383 /* No additional check needed for pattern-based monitor */
386 static int msft_add_monitor_sync(struct hci_dev *hdev,
387 struct adv_monitor *monitor)
389 struct msft_cp_le_monitor_advertisement *cp;
390 struct msft_le_monitor_advertisement_pattern_data *pattern_data;
391 struct msft_le_monitor_advertisement_pattern *pattern;
392 struct adv_pattern *entry;
393 size_t total_size = sizeof(*cp) + sizeof(*pattern_data);
394 ptrdiff_t offset = 0;
395 u8 pattern_count = 0;
398 if (!msft_monitor_pattern_valid(monitor))
401 list_for_each_entry(entry, &monitor->patterns, list) {
403 total_size += sizeof(*pattern) + entry->length;
406 cp = kmalloc(total_size, GFP_KERNEL);
410 cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT;
411 cp->rssi_high = monitor->rssi.high_threshold;
412 cp->rssi_low = monitor->rssi.low_threshold;
413 cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout;
414 cp->rssi_sampling_period = monitor->rssi.sampling_period;
416 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
418 pattern_data = (void *)cp->data;
419 pattern_data->count = pattern_count;
421 list_for_each_entry(entry, &monitor->patterns, list) {
422 pattern = (void *)(pattern_data->data + offset);
423 /* the length also includes data_type and offset */
424 pattern->length = entry->length + 2;
425 pattern->data_type = entry->ad_type;
426 pattern->start_byte = entry->offset;
427 memcpy(pattern->pattern, entry->value, entry->length);
428 offset += sizeof(*pattern) + entry->length;
431 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
438 return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode,
442 /* This function requires the caller holds hci_req_sync_lock */
443 static void reregister_monitor(struct hci_dev *hdev)
445 struct adv_monitor *monitor;
446 struct msft_data *msft = hdev->msft_data;
452 msft->resuming = true;
455 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
459 msft_add_monitor_sync(hdev, monitor);
464 /* All monitors have been reregistered */
465 msft->resuming = false;
468 /* This function requires the caller holds hci_req_sync_lock */
469 int msft_resume_sync(struct hci_dev *hdev)
471 struct msft_data *msft = hdev->msft_data;
473 if (!msft || !msft_monitor_supported(hdev))
478 /* Clear already tracked devices on resume. Once the monitors are
479 * reregistered, devices in range will be found again after resume.
481 hdev->advmon_pend_notify = false;
482 msft_monitor_device_del(hdev, 0, NULL, 0, true);
484 hci_dev_unlock(hdev);
486 reregister_monitor(hdev);
491 /* This function requires the caller holds hci_req_sync_lock */
492 void msft_do_open(struct hci_dev *hdev)
494 struct msft_data *msft = hdev->msft_data;
496 if (hdev->msft_opcode == HCI_OP_NOP)
500 bt_dev_err(hdev, "MSFT extension not registered");
504 bt_dev_dbg(hdev, "Initialize MSFT extension");
506 /* Reset existing MSFT data before re-reading */
507 kfree(msft->evt_prefix);
508 msft->evt_prefix = NULL;
509 msft->evt_prefix_len = 0;
512 if (!read_supported_features(hdev, msft)) {
513 hdev->msft_data = NULL;
518 if (msft_monitor_supported(hdev)) {
519 msft->resuming = true;
520 msft_set_filter_enable(hdev, true);
521 /* Monitors get removed on power off, so we need to explicitly
522 * tell the controller to re-monitor.
524 reregister_monitor(hdev);
528 void msft_do_close(struct hci_dev *hdev)
530 struct msft_data *msft = hdev->msft_data;
531 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
532 struct adv_monitor *monitor;
537 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
539 /* The controller will silently remove all monitors on power off.
540 * Therefore, remove handle_data mapping and reset monitor state.
542 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
543 monitor = idr_find(&hdev->adv_monitors_idr,
544 handle_data->mgmt_handle);
546 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
547 monitor->state = ADV_MONITOR_STATE_REGISTERED;
549 list_del(&handle_data->list);
555 /* Clear any devices that are being monitored and notify device lost */
556 hdev->advmon_pend_notify = false;
557 msft_monitor_device_del(hdev, 0, NULL, 0, true);
559 hci_dev_unlock(hdev);
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 /* This function requires the caller holds hdev->lock */
594 static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr,
595 __u8 addr_type, __u16 mgmt_handle)
597 struct monitored_device *dev;
599 dev = kmalloc(sizeof(*dev), GFP_KERNEL);
601 bt_dev_err(hdev, "MSFT vendor event %u: no memory",
602 MSFT_EV_LE_MONITOR_DEVICE);
606 bacpy(&dev->bdaddr, bdaddr);
607 dev->addr_type = addr_type;
608 dev->handle = mgmt_handle;
609 dev->notified = false;
611 INIT_LIST_HEAD(&dev->list);
612 list_add(&dev->list, &hdev->monitored_devices);
613 hdev->advmon_pend_notify = true;
616 /* This function requires the caller holds hdev->lock */
617 static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr,
618 __u8 addr_type, __u16 mgmt_handle)
620 if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type,
622 bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list",
623 MSFT_EV_LE_MONITOR_DEVICE, bdaddr);
627 static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb,
632 data = skb_pull_data(skb, len);
634 bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev);
639 /* This function requires the caller holds hdev->lock */
640 static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb)
642 struct msft_ev_le_monitor_device *ev;
643 struct msft_monitor_advertisement_handle_data *handle_data;
646 ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev));
651 "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR",
652 MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle,
653 ev->monitor_state, &ev->bdaddr);
655 handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false);
659 switch (ev->addr_type) {
660 case ADDR_LE_DEV_PUBLIC:
661 addr_type = BDADDR_LE_PUBLIC;
664 case ADDR_LE_DEV_RANDOM:
665 addr_type = BDADDR_LE_RANDOM;
670 "MSFT vendor event 0x%02x: unknown addr type 0x%02x",
671 MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type);
675 if (ev->monitor_state)
676 msft_device_found(hdev, &ev->bdaddr, addr_type,
677 handle_data->mgmt_handle);
679 msft_device_lost(hdev, &ev->bdaddr, addr_type,
680 handle_data->mgmt_handle);
683 void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
685 struct msft_data *msft = hdev->msft_data;
692 /* When the extension has defined an event prefix, check that it
693 * matches, and otherwise just return.
695 if (msft->evt_prefix_len > 0) {
696 evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len);
700 if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len))
704 /* Every event starts at least with an event code and the rest of
705 * the data is variable and depends on the event code.
710 evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt));
717 case MSFT_EV_LE_MONITOR_DEVICE:
718 msft_monitor_device_evt(hdev, skb);
722 bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt);
726 hci_dev_unlock(hdev);
729 __u64 msft_get_features(struct hci_dev *hdev)
731 struct msft_data *msft = hdev->msft_data;
733 return msft ? msft->features : 0;
736 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
737 u8 status, u16 opcode,
740 struct msft_cp_le_set_advertisement_filter_enable *cp;
741 struct msft_rp_le_set_advertisement_filter_enable *rp;
742 struct msft_data *msft = hdev->msft_data;
744 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
745 if (skb->len < sizeof(*rp))
748 /* Error 0x0C would be returned if the filter enabled status is
749 * already set to whatever we were trying to set.
750 * Although the default state should be disabled, some controller set
751 * the initial value to enabled. Because there is no way to know the
752 * actual initial value before sending this command, here we also treat
753 * error 0x0C as success.
755 if (status != 0x00 && status != 0x0C)
760 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
761 msft->filter_enabled = cp->enable;
764 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
765 cp->enable ? "on" : "off");
767 hci_dev_unlock(hdev);
770 /* This function requires the caller holds hci_req_sync_lock */
771 int msft_add_monitor_pattern(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_add_monitor_sync(hdev, monitor);
784 /* This function requires the caller holds hci_req_sync_lock */
785 int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor)
787 struct msft_data *msft = hdev->msft_data;
792 if (msft->resuming || msft->suspending)
795 return msft_remove_monitor_sync(hdev, monitor);
798 void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
800 struct hci_dev *hdev = req->hdev;
801 struct msft_cp_le_set_advertisement_filter_enable cp;
803 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
806 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
809 int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
811 struct hci_request req;
812 struct msft_data *msft = hdev->msft_data;
818 hci_req_init(&req, hdev);
819 msft_req_add_set_filter_enable(&req, enable);
820 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
825 bool msft_curve_validity(struct hci_dev *hdev)
827 return hdev->msft_curve_validity;