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
);
145 if (msft
->features
& MSFT_FEATURE_MASK_CURVE_VALIDITY
)
146 hdev
->msft_curve_validity
= true;
156 /* This function requires the caller holds hdev->lock */
157 static void reregister_monitor_on_restart(struct hci_dev
*hdev
, int handle
)
159 struct adv_monitor
*monitor
;
160 struct msft_data
*msft
= hdev
->msft_data
;
164 monitor
= idr_get_next(&hdev
->adv_monitors_idr
, &handle
);
166 /* All monitors have been reregistered */
167 msft
->reregistering
= false;
168 hci_update_background_scan(hdev
);
172 msft
->pending_add_handle
= (u16
)handle
;
173 err
= __msft_add_monitor_pattern(hdev
, monitor
);
175 /* If success, we return and wait for monitor added callback */
179 /* Otherwise remove the monitor and keep registering */
180 hci_free_adv_monitor(hdev
, monitor
);
185 void msft_do_open(struct hci_dev
*hdev
)
187 struct msft_data
*msft
;
189 if (hdev
->msft_opcode
== HCI_OP_NOP
)
192 bt_dev_dbg(hdev
, "Initialize MSFT extension");
194 msft
= kzalloc(sizeof(*msft
), GFP_KERNEL
);
198 if (!read_supported_features(hdev
, msft
)) {
203 INIT_LIST_HEAD(&msft
->handle_map
);
204 hdev
->msft_data
= msft
;
206 if (msft_monitor_supported(hdev
)) {
207 msft
->reregistering
= true;
208 msft_set_filter_enable(hdev
, true);
209 reregister_monitor_on_restart(hdev
, 0);
213 void msft_do_close(struct hci_dev
*hdev
)
215 struct msft_data
*msft
= hdev
->msft_data
;
216 struct msft_monitor_advertisement_handle_data
*handle_data
, *tmp
;
217 struct adv_monitor
*monitor
;
222 bt_dev_dbg(hdev
, "Cleanup of MSFT extension");
224 hdev
->msft_data
= NULL
;
226 list_for_each_entry_safe(handle_data
, tmp
, &msft
->handle_map
, list
) {
227 monitor
= idr_find(&hdev
->adv_monitors_idr
,
228 handle_data
->mgmt_handle
);
230 if (monitor
&& monitor
->state
== ADV_MONITOR_STATE_OFFLOADED
)
231 monitor
->state
= ADV_MONITOR_STATE_REGISTERED
;
233 list_del(&handle_data
->list
);
237 kfree(msft
->evt_prefix
);
241 void msft_vendor_evt(struct hci_dev
*hdev
, struct sk_buff
*skb
)
243 struct msft_data
*msft
= hdev
->msft_data
;
249 /* When the extension has defined an event prefix, check that it
250 * matches, and otherwise just return.
252 if (msft
->evt_prefix_len
> 0) {
253 if (skb
->len
< msft
->evt_prefix_len
)
256 if (memcmp(skb
->data
, msft
->evt_prefix
, msft
->evt_prefix_len
))
259 skb_pull(skb
, msft
->evt_prefix_len
);
262 /* Every event starts at least with an event code and the rest of
263 * the data is variable and depends on the event code.
271 bt_dev_dbg(hdev
, "MSFT vendor event %u", event
);
274 __u64
msft_get_features(struct hci_dev
*hdev
)
276 struct msft_data
*msft
= hdev
->msft_data
;
278 return msft
? msft
->features
: 0;
281 /* is_mgmt = true matches the handle exposed to userspace via mgmt.
282 * is_mgmt = false matches the handle used by the msft controller.
283 * This function requires the caller holds hdev->lock
285 static struct msft_monitor_advertisement_handle_data
*msft_find_handle_data
286 (struct hci_dev
*hdev
, u16 handle
, bool is_mgmt
)
288 struct msft_monitor_advertisement_handle_data
*entry
;
289 struct msft_data
*msft
= hdev
->msft_data
;
291 list_for_each_entry(entry
, &msft
->handle_map
, list
) {
292 if (is_mgmt
&& entry
->mgmt_handle
== handle
)
294 if (!is_mgmt
&& entry
->msft_handle
== handle
)
301 static void msft_le_monitor_advertisement_cb(struct hci_dev
*hdev
,
302 u8 status
, u16 opcode
,
305 struct msft_rp_le_monitor_advertisement
*rp
;
306 struct adv_monitor
*monitor
;
307 struct msft_monitor_advertisement_handle_data
*handle_data
;
308 struct msft_data
*msft
= hdev
->msft_data
;
312 monitor
= idr_find(&hdev
->adv_monitors_idr
, msft
->pending_add_handle
);
314 bt_dev_err(hdev
, "msft add advmon: monitor %u is not found!",
315 msft
->pending_add_handle
);
316 status
= HCI_ERROR_UNSPECIFIED
;
323 rp
= (struct msft_rp_le_monitor_advertisement
*)skb
->data
;
324 if (skb
->len
< sizeof(*rp
)) {
325 status
= HCI_ERROR_UNSPECIFIED
;
329 handle_data
= kmalloc(sizeof(*handle_data
), GFP_KERNEL
);
331 status
= HCI_ERROR_UNSPECIFIED
;
335 handle_data
->mgmt_handle
= monitor
->handle
;
336 handle_data
->msft_handle
= rp
->handle
;
337 INIT_LIST_HEAD(&handle_data
->list
);
338 list_add(&handle_data
->list
, &msft
->handle_map
);
340 monitor
->state
= ADV_MONITOR_STATE_OFFLOADED
;
343 if (status
&& monitor
)
344 hci_free_adv_monitor(hdev
, monitor
);
346 /* If in restart/reregister sequence, keep registering. */
347 if (msft
->reregistering
)
348 reregister_monitor_on_restart(hdev
,
349 msft
->pending_add_handle
+ 1);
351 hci_dev_unlock(hdev
);
353 if (!msft
->reregistering
)
354 hci_add_adv_patterns_monitor_complete(hdev
, status
);
357 static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev
*hdev
,
358 u8 status
, u16 opcode
,
361 struct msft_cp_le_cancel_monitor_advertisement
*cp
;
362 struct msft_rp_le_cancel_monitor_advertisement
*rp
;
363 struct adv_monitor
*monitor
;
364 struct msft_monitor_advertisement_handle_data
*handle_data
;
365 struct msft_data
*msft
= hdev
->msft_data
;
372 rp
= (struct msft_rp_le_cancel_monitor_advertisement
*)skb
->data
;
373 if (skb
->len
< sizeof(*rp
)) {
374 status
= HCI_ERROR_UNSPECIFIED
;
380 cp
= hci_sent_cmd_data(hdev
, hdev
->msft_opcode
);
381 handle_data
= msft_find_handle_data(hdev
, cp
->handle
, false);
384 monitor
= idr_find(&hdev
->adv_monitors_idr
,
385 handle_data
->mgmt_handle
);
387 hci_free_adv_monitor(hdev
, monitor
);
389 list_del(&handle_data
->list
);
393 /* If remove all monitors is required, we need to continue the process
394 * here because the earlier it was paused when waiting for the
395 * response from controller.
397 if (msft
->pending_remove_handle
== 0) {
398 pending
= hci_remove_all_adv_monitor(hdev
, &err
);
400 hci_dev_unlock(hdev
);
405 status
= HCI_ERROR_UNSPECIFIED
;
408 hci_dev_unlock(hdev
);
411 hci_remove_adv_monitor_complete(hdev
, status
);
414 static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev
*hdev
,
415 u8 status
, u16 opcode
,
418 struct msft_cp_le_set_advertisement_filter_enable
*cp
;
419 struct msft_rp_le_set_advertisement_filter_enable
*rp
;
420 struct msft_data
*msft
= hdev
->msft_data
;
422 rp
= (struct msft_rp_le_set_advertisement_filter_enable
*)skb
->data
;
423 if (skb
->len
< sizeof(*rp
))
426 /* Error 0x0C would be returned if the filter enabled status is
427 * already set to whatever we were trying to set.
428 * Although the default state should be disabled, some controller set
429 * the initial value to enabled. Because there is no way to know the
430 * actual initial value before sending this command, here we also treat
431 * error 0x0C as success.
433 if (status
!= 0x00 && status
!= 0x0C)
438 cp
= hci_sent_cmd_data(hdev
, hdev
->msft_opcode
);
439 msft
->filter_enabled
= cp
->enable
;
442 bt_dev_warn(hdev
, "MSFT filter_enable is already %s",
443 cp
->enable
? "on" : "off");
445 hci_dev_unlock(hdev
);
448 static bool msft_monitor_rssi_valid(struct adv_monitor
*monitor
)
450 struct adv_rssi_thresholds
*r
= &monitor
->rssi
;
452 if (r
->high_threshold
< MSFT_RSSI_THRESHOLD_VALUE_MIN
||
453 r
->high_threshold
> MSFT_RSSI_THRESHOLD_VALUE_MAX
||
454 r
->low_threshold
< MSFT_RSSI_THRESHOLD_VALUE_MIN
||
455 r
->low_threshold
> MSFT_RSSI_THRESHOLD_VALUE_MAX
)
458 /* High_threshold_timeout is not supported,
459 * once high_threshold is reached, events are immediately reported.
461 if (r
->high_threshold_timeout
!= 0)
464 if (r
->low_threshold_timeout
> MSFT_RSSI_LOW_TIMEOUT_MAX
)
467 /* Sampling period from 0x00 to 0xFF are all allowed */
471 static bool msft_monitor_pattern_valid(struct adv_monitor
*monitor
)
473 return msft_monitor_rssi_valid(monitor
);
474 /* No additional check needed for pattern-based monitor */
477 /* This function requires the caller holds hdev->lock */
478 static int __msft_add_monitor_pattern(struct hci_dev
*hdev
,
479 struct adv_monitor
*monitor
)
481 struct msft_cp_le_monitor_advertisement
*cp
;
482 struct msft_le_monitor_advertisement_pattern_data
*pattern_data
;
483 struct msft_le_monitor_advertisement_pattern
*pattern
;
484 struct adv_pattern
*entry
;
485 struct hci_request req
;
486 struct msft_data
*msft
= hdev
->msft_data
;
487 size_t total_size
= sizeof(*cp
) + sizeof(*pattern_data
);
488 ptrdiff_t offset
= 0;
489 u8 pattern_count
= 0;
492 if (!msft_monitor_pattern_valid(monitor
))
495 list_for_each_entry(entry
, &monitor
->patterns
, list
) {
497 total_size
+= sizeof(*pattern
) + entry
->length
;
500 cp
= kmalloc(total_size
, GFP_KERNEL
);
504 cp
->sub_opcode
= MSFT_OP_LE_MONITOR_ADVERTISEMENT
;
505 cp
->rssi_high
= monitor
->rssi
.high_threshold
;
506 cp
->rssi_low
= monitor
->rssi
.low_threshold
;
507 cp
->rssi_low_interval
= (u8
)monitor
->rssi
.low_threshold_timeout
;
508 cp
->rssi_sampling_period
= monitor
->rssi
.sampling_period
;
510 cp
->cond_type
= MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN
;
512 pattern_data
= (void *)cp
->data
;
513 pattern_data
->count
= pattern_count
;
515 list_for_each_entry(entry
, &monitor
->patterns
, list
) {
516 pattern
= (void *)(pattern_data
->data
+ offset
);
517 /* the length also includes data_type and offset */
518 pattern
->length
= entry
->length
+ 2;
519 pattern
->data_type
= entry
->ad_type
;
520 pattern
->start_byte
= entry
->offset
;
521 memcpy(pattern
->pattern
, entry
->value
, entry
->length
);
522 offset
+= sizeof(*pattern
) + entry
->length
;
525 hci_req_init(&req
, hdev
);
526 hci_req_add(&req
, hdev
->msft_opcode
, total_size
, cp
);
527 err
= hci_req_run_skb(&req
, msft_le_monitor_advertisement_cb
);
531 msft
->pending_add_handle
= monitor
->handle
;
536 /* This function requires the caller holds hdev->lock */
537 int msft_add_monitor_pattern(struct hci_dev
*hdev
, struct adv_monitor
*monitor
)
539 struct msft_data
*msft
= hdev
->msft_data
;
544 if (msft
->reregistering
)
547 return __msft_add_monitor_pattern(hdev
, monitor
);
550 /* This function requires the caller holds hdev->lock */
551 int msft_remove_monitor(struct hci_dev
*hdev
, struct adv_monitor
*monitor
,
554 struct msft_cp_le_cancel_monitor_advertisement cp
;
555 struct msft_monitor_advertisement_handle_data
*handle_data
;
556 struct hci_request req
;
557 struct msft_data
*msft
= hdev
->msft_data
;
563 if (msft
->reregistering
)
566 handle_data
= msft_find_handle_data(hdev
, monitor
->handle
, true);
568 /* If no matched handle, just remove without telling controller */
572 cp
.sub_opcode
= MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT
;
573 cp
.handle
= handle_data
->msft_handle
;
575 hci_req_init(&req
, hdev
);
576 hci_req_add(&req
, hdev
->msft_opcode
, sizeof(cp
), &cp
);
577 err
= hci_req_run_skb(&req
, msft_le_cancel_monitor_advertisement_cb
);
580 msft
->pending_remove_handle
= handle
;
585 void msft_req_add_set_filter_enable(struct hci_request
*req
, bool enable
)
587 struct hci_dev
*hdev
= req
->hdev
;
588 struct msft_cp_le_set_advertisement_filter_enable cp
;
590 cp
.sub_opcode
= MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE
;
593 hci_req_add(req
, hdev
->msft_opcode
, sizeof(cp
), &cp
);
596 int msft_set_filter_enable(struct hci_dev
*hdev
, bool enable
)
598 struct hci_request req
;
599 struct msft_data
*msft
= hdev
->msft_data
;
605 hci_req_init(&req
, hdev
);
606 msft_req_add_set_filter_enable(&req
, enable
);
607 err
= hci_req_run_skb(&req
, msft_le_set_advertisement_filter_enable_cb
);
612 bool msft_curve_validity(struct hci_dev
*hdev
)
614 return hdev
->msft_curve_validity
;