Loading...
Note: File does not exist in v4.6.
1// SPDX-License-Identifier: GPL-2.0-only
2/*
3 * Copyright (C) 2020 Google Corporation
4 */
5
6#include <net/bluetooth/bluetooth.h>
7#include <net/bluetooth/hci_core.h>
8#include <net/bluetooth/mgmt.h>
9
10#include "hci_request.h"
11#include "mgmt_util.h"
12#include "msft.h"
13
14#define MSFT_RSSI_THRESHOLD_VALUE_MIN -127
15#define MSFT_RSSI_THRESHOLD_VALUE_MAX 20
16#define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C
17
18#define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
19struct msft_cp_read_supported_features {
20 __u8 sub_opcode;
21} __packed;
22
23struct msft_rp_read_supported_features {
24 __u8 status;
25 __u8 sub_opcode;
26 __le64 features;
27 __u8 evt_prefix_len;
28 __u8 evt_prefix[];
29} __packed;
30
31#define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03
32#define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01
33struct msft_le_monitor_advertisement_pattern {
34 __u8 length;
35 __u8 data_type;
36 __u8 start_byte;
37 __u8 pattern[];
38};
39
40struct msft_le_monitor_advertisement_pattern_data {
41 __u8 count;
42 __u8 data[];
43};
44
45struct msft_cp_le_monitor_advertisement {
46 __u8 sub_opcode;
47 __s8 rssi_high;
48 __s8 rssi_low;
49 __u8 rssi_low_interval;
50 __u8 rssi_sampling_period;
51 __u8 cond_type;
52 __u8 data[];
53} __packed;
54
55struct msft_rp_le_monitor_advertisement {
56 __u8 status;
57 __u8 sub_opcode;
58 __u8 handle;
59} __packed;
60
61#define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04
62struct msft_cp_le_cancel_monitor_advertisement {
63 __u8 sub_opcode;
64 __u8 handle;
65} __packed;
66
67struct msft_rp_le_cancel_monitor_advertisement {
68 __u8 status;
69 __u8 sub_opcode;
70} __packed;
71
72#define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05
73struct msft_cp_le_set_advertisement_filter_enable {
74 __u8 sub_opcode;
75 __u8 enable;
76} __packed;
77
78struct msft_rp_le_set_advertisement_filter_enable {
79 __u8 status;
80 __u8 sub_opcode;
81} __packed;
82
83struct msft_monitor_advertisement_handle_data {
84 __u8 msft_handle;
85 __u16 mgmt_handle;
86 struct list_head list;
87};
88
89struct msft_data {
90 __u64 features;
91 __u8 evt_prefix_len;
92 __u8 *evt_prefix;
93 struct list_head handle_map;
94 __u16 pending_add_handle;
95 __u16 pending_remove_handle;
96 __u8 reregistering;
97 __u8 filter_enabled;
98};
99
100static int __msft_add_monitor_pattern(struct hci_dev *hdev,
101 struct adv_monitor *monitor);
102
103bool msft_monitor_supported(struct hci_dev *hdev)
104{
105 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
106}
107
108static bool read_supported_features(struct hci_dev *hdev,
109 struct msft_data *msft)
110{
111 struct msft_cp_read_supported_features cp;
112 struct msft_rp_read_supported_features *rp;
113 struct sk_buff *skb;
114
115 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
116
117 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
118 HCI_CMD_TIMEOUT);
119 if (IS_ERR(skb)) {
120 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
121 PTR_ERR(skb));
122 return false;
123 }
124
125 if (skb->len < sizeof(*rp)) {
126 bt_dev_err(hdev, "MSFT supported features length mismatch");
127 goto failed;
128 }
129
130 rp = (struct msft_rp_read_supported_features *)skb->data;
131
132 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
133 goto failed;
134
135 if (rp->evt_prefix_len > 0) {
136 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
137 GFP_KERNEL);
138 if (!msft->evt_prefix)
139 goto failed;
140 }
141
142 msft->evt_prefix_len = rp->evt_prefix_len;
143 msft->features = __le64_to_cpu(rp->features);
144
145 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
146 hdev->msft_curve_validity = true;
147
148 kfree_skb(skb);
149 return true;
150
151failed:
152 kfree_skb(skb);
153 return false;
154}
155
156/* This function requires the caller holds hdev->lock */
157static void reregister_monitor_on_restart(struct hci_dev *hdev, int handle)
158{
159 struct adv_monitor *monitor;
160 struct msft_data *msft = hdev->msft_data;
161 int err;
162
163 while (1) {
164 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
165 if (!monitor) {
166 /* All monitors have been reregistered */
167 msft->reregistering = false;
168 hci_update_background_scan(hdev);
169 return;
170 }
171
172 msft->pending_add_handle = (u16)handle;
173 err = __msft_add_monitor_pattern(hdev, monitor);
174
175 /* If success, we return and wait for monitor added callback */
176 if (!err)
177 return;
178
179 /* Otherwise remove the monitor and keep registering */
180 hci_free_adv_monitor(hdev, monitor);
181 handle++;
182 }
183}
184
185void msft_do_open(struct hci_dev *hdev)
186{
187 struct msft_data *msft;
188
189 if (hdev->msft_opcode == HCI_OP_NOP)
190 return;
191
192 bt_dev_dbg(hdev, "Initialize MSFT extension");
193
194 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
195 if (!msft)
196 return;
197
198 if (!read_supported_features(hdev, msft)) {
199 kfree(msft);
200 return;
201 }
202
203 INIT_LIST_HEAD(&msft->handle_map);
204 hdev->msft_data = msft;
205
206 if (msft_monitor_supported(hdev)) {
207 msft->reregistering = true;
208 msft_set_filter_enable(hdev, true);
209 reregister_monitor_on_restart(hdev, 0);
210 }
211}
212
213void msft_do_close(struct hci_dev *hdev)
214{
215 struct msft_data *msft = hdev->msft_data;
216 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
217 struct adv_monitor *monitor;
218
219 if (!msft)
220 return;
221
222 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
223
224 hdev->msft_data = NULL;
225
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);
229
230 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
231 monitor->state = ADV_MONITOR_STATE_REGISTERED;
232
233 list_del(&handle_data->list);
234 kfree(handle_data);
235 }
236
237 kfree(msft->evt_prefix);
238 kfree(msft);
239}
240
241void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
242{
243 struct msft_data *msft = hdev->msft_data;
244 u8 event;
245
246 if (!msft)
247 return;
248
249 /* When the extension has defined an event prefix, check that it
250 * matches, and otherwise just return.
251 */
252 if (msft->evt_prefix_len > 0) {
253 if (skb->len < msft->evt_prefix_len)
254 return;
255
256 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
257 return;
258
259 skb_pull(skb, msft->evt_prefix_len);
260 }
261
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.
264 */
265 if (skb->len < 1)
266 return;
267
268 event = *skb->data;
269 skb_pull(skb, 1);
270
271 bt_dev_dbg(hdev, "MSFT vendor event %u", event);
272}
273
274__u64 msft_get_features(struct hci_dev *hdev)
275{
276 struct msft_data *msft = hdev->msft_data;
277
278 return msft ? msft->features : 0;
279}
280
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
284 */
285static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
286 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
287{
288 struct msft_monitor_advertisement_handle_data *entry;
289 struct msft_data *msft = hdev->msft_data;
290
291 list_for_each_entry(entry, &msft->handle_map, list) {
292 if (is_mgmt && entry->mgmt_handle == handle)
293 return entry;
294 if (!is_mgmt && entry->msft_handle == handle)
295 return entry;
296 }
297
298 return NULL;
299}
300
301static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
302 u8 status, u16 opcode,
303 struct sk_buff *skb)
304{
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;
309
310 hci_dev_lock(hdev);
311
312 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
313 if (!monitor) {
314 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
315 msft->pending_add_handle);
316 status = HCI_ERROR_UNSPECIFIED;
317 goto unlock;
318 }
319
320 if (status)
321 goto unlock;
322
323 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
324 if (skb->len < sizeof(*rp)) {
325 status = HCI_ERROR_UNSPECIFIED;
326 goto unlock;
327 }
328
329 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
330 if (!handle_data) {
331 status = HCI_ERROR_UNSPECIFIED;
332 goto unlock;
333 }
334
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);
339
340 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
341
342unlock:
343 if (status && monitor)
344 hci_free_adv_monitor(hdev, monitor);
345
346 /* If in restart/reregister sequence, keep registering. */
347 if (msft->reregistering)
348 reregister_monitor_on_restart(hdev,
349 msft->pending_add_handle + 1);
350
351 hci_dev_unlock(hdev);
352
353 if (!msft->reregistering)
354 hci_add_adv_patterns_monitor_complete(hdev, status);
355}
356
357static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
358 u8 status, u16 opcode,
359 struct sk_buff *skb)
360{
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;
366 int err;
367 bool pending;
368
369 if (status)
370 goto done;
371
372 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
373 if (skb->len < sizeof(*rp)) {
374 status = HCI_ERROR_UNSPECIFIED;
375 goto done;
376 }
377
378 hci_dev_lock(hdev);
379
380 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
381 handle_data = msft_find_handle_data(hdev, cp->handle, false);
382
383 if (handle_data) {
384 monitor = idr_find(&hdev->adv_monitors_idr,
385 handle_data->mgmt_handle);
386 if (monitor)
387 hci_free_adv_monitor(hdev, monitor);
388
389 list_del(&handle_data->list);
390 kfree(handle_data);
391 }
392
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.
396 */
397 if (msft->pending_remove_handle == 0) {
398 pending = hci_remove_all_adv_monitor(hdev, &err);
399 if (pending) {
400 hci_dev_unlock(hdev);
401 return;
402 }
403
404 if (err)
405 status = HCI_ERROR_UNSPECIFIED;
406 }
407
408 hci_dev_unlock(hdev);
409
410done:
411 hci_remove_adv_monitor_complete(hdev, status);
412}
413
414static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
415 u8 status, u16 opcode,
416 struct sk_buff *skb)
417{
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;
421
422 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
423 if (skb->len < sizeof(*rp))
424 return;
425
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.
432 */
433 if (status != 0x00 && status != 0x0C)
434 return;
435
436 hci_dev_lock(hdev);
437
438 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
439 msft->filter_enabled = cp->enable;
440
441 if (status == 0x0C)
442 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
443 cp->enable ? "on" : "off");
444
445 hci_dev_unlock(hdev);
446}
447
448static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
449{
450 struct adv_rssi_thresholds *r = &monitor->rssi;
451
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)
456 return false;
457
458 /* High_threshold_timeout is not supported,
459 * once high_threshold is reached, events are immediately reported.
460 */
461 if (r->high_threshold_timeout != 0)
462 return false;
463
464 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
465 return false;
466
467 /* Sampling period from 0x00 to 0xFF are all allowed */
468 return true;
469}
470
471static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
472{
473 return msft_monitor_rssi_valid(monitor);
474 /* No additional check needed for pattern-based monitor */
475}
476
477/* This function requires the caller holds hdev->lock */
478static int __msft_add_monitor_pattern(struct hci_dev *hdev,
479 struct adv_monitor *monitor)
480{
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;
490 int err = 0;
491
492 if (!msft_monitor_pattern_valid(monitor))
493 return -EINVAL;
494
495 list_for_each_entry(entry, &monitor->patterns, list) {
496 pattern_count++;
497 total_size += sizeof(*pattern) + entry->length;
498 }
499
500 cp = kmalloc(total_size, GFP_KERNEL);
501 if (!cp)
502 return -ENOMEM;
503
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;
509
510 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
511
512 pattern_data = (void *)cp->data;
513 pattern_data->count = pattern_count;
514
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;
523 }
524
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);
528 kfree(cp);
529
530 if (!err)
531 msft->pending_add_handle = monitor->handle;
532
533 return err;
534}
535
536/* This function requires the caller holds hdev->lock */
537int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
538{
539 struct msft_data *msft = hdev->msft_data;
540
541 if (!msft)
542 return -EOPNOTSUPP;
543
544 if (msft->reregistering)
545 return -EBUSY;
546
547 return __msft_add_monitor_pattern(hdev, monitor);
548}
549
550/* This function requires the caller holds hdev->lock */
551int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
552 u16 handle)
553{
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;
558 int err = 0;
559
560 if (!msft)
561 return -EOPNOTSUPP;
562
563 if (msft->reregistering)
564 return -EBUSY;
565
566 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
567
568 /* If no matched handle, just remove without telling controller */
569 if (!handle_data)
570 return -ENOENT;
571
572 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
573 cp.handle = handle_data->msft_handle;
574
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);
578
579 if (!err)
580 msft->pending_remove_handle = handle;
581
582 return err;
583}
584
585void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
586{
587 struct hci_dev *hdev = req->hdev;
588 struct msft_cp_le_set_advertisement_filter_enable cp;
589
590 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
591 cp.enable = enable;
592
593 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
594}
595
596int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
597{
598 struct hci_request req;
599 struct msft_data *msft = hdev->msft_data;
600 int err;
601
602 if (!msft)
603 return -EOPNOTSUPP;
604
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);
608
609 return err;
610}
611
612bool msft_curve_validity(struct hci_dev *hdev)
613{
614 return hdev->msft_curve_validity;
615}