blob: 6a943634b31a26ab0ddf54d2b89472b155c86415 [file] [log] [blame]
Miao-chen Chou145373c2020-04-03 21:44:01 +02001// 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>
Archie Pusakaa2a4ded2021-01-22 16:36:12 +08008#include <net/bluetooth/mgmt.h>
Miao-chen Chou145373c2020-04-03 21:44:01 +02009
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080010#include "hci_request.h"
11#include "mgmt_util.h"
Miao-chen Chou145373c2020-04-03 21:44:01 +020012#include "msft.h"
13
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080014#define MSFT_RSSI_THRESHOLD_VALUE_MIN -127
15#define MSFT_RSSI_THRESHOLD_VALUE_MAX 20
16#define MSFT_RSSI_LOW_TIMEOUT_MAX 0x3C
17
Miao-chen Chou145373c2020-04-03 21:44:01 +020018#define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
19struct msft_cp_read_supported_features {
20 __u8 sub_opcode;
21} __packed;
Gustavo A. R. Silvab08eadd2020-08-31 08:25:42 -050022
Miao-chen Chou145373c2020-04-03 21:44:01 +020023struct msft_rp_read_supported_features {
24 __u8 status;
25 __u8 sub_opcode;
26 __le64 features;
27 __u8 evt_prefix_len;
Gustavo A. R. Silvab08eadd2020-08-31 08:25:42 -050028 __u8 evt_prefix[];
Miao-chen Chou145373c2020-04-03 21:44:01 +020029} __packed;
30
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080031#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;
Qiheng Lin07d85db2021-04-10 10:19:35 +080037 __u8 pattern[];
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080038};
39
40struct msft_le_monitor_advertisement_pattern_data {
41 __u8 count;
Qiheng Lin07d85db2021-04-10 10:19:35 +080042 __u8 data[];
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080043};
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;
Qiheng Lin07d85db2021-04-10 10:19:35 +080052 __u8 data[];
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080053} __packed;
54
55struct msft_rp_le_monitor_advertisement {
56 __u8 status;
57 __u8 sub_opcode;
58 __u8 handle;
59} __packed;
60
Archie Pusaka66bd0952021-01-22 16:36:13 +080061#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
Archie Pusaka394566b2021-01-22 16:36:15 +080072#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
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080083struct msft_monitor_advertisement_handle_data {
84 __u8 msft_handle;
85 __u16 mgmt_handle;
86 struct list_head list;
87};
88
Miao-chen Chou145373c2020-04-03 21:44:01 +020089struct msft_data {
90 __u64 features;
91 __u8 evt_prefix_len;
92 __u8 *evt_prefix;
Archie Pusakaa2a4ded2021-01-22 16:36:12 +080093 struct list_head handle_map;
94 __u16 pending_add_handle;
Archie Pusaka66bd0952021-01-22 16:36:13 +080095 __u16 pending_remove_handle;
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -070096 __u8 resuming;
Manish Mandlikce818432021-09-21 14:47:10 -070097 __u8 suspending;
Archie Pusaka394566b2021-01-22 16:36:15 +080098 __u8 filter_enabled;
Miao-chen Chou145373c2020-04-03 21:44:01 +020099};
100
Archie Pusaka4a376822021-01-22 16:36:14 +0800101static int __msft_add_monitor_pattern(struct hci_dev *hdev,
102 struct adv_monitor *monitor);
Manish Mandlikce818432021-09-21 14:47:10 -0700103static int __msft_remove_monitor(struct hci_dev *hdev,
104 struct adv_monitor *monitor, u16 handle);
Archie Pusaka4a376822021-01-22 16:36:14 +0800105
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800106bool msft_monitor_supported(struct hci_dev *hdev)
107{
108 return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR);
109}
110
Miao-chen Chou145373c2020-04-03 21:44:01 +0200111static bool read_supported_features(struct hci_dev *hdev,
112 struct msft_data *msft)
113{
114 struct msft_cp_read_supported_features cp;
115 struct msft_rp_read_supported_features *rp;
116 struct sk_buff *skb;
117
118 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
119
120 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
121 HCI_CMD_TIMEOUT);
122 if (IS_ERR(skb)) {
123 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
124 PTR_ERR(skb));
125 return false;
126 }
127
128 if (skb->len < sizeof(*rp)) {
129 bt_dev_err(hdev, "MSFT supported features length mismatch");
130 goto failed;
131 }
132
133 rp = (struct msft_rp_read_supported_features *)skb->data;
134
135 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
136 goto failed;
137
138 if (rp->evt_prefix_len > 0) {
139 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
140 GFP_KERNEL);
141 if (!msft->evt_prefix)
142 goto failed;
143 }
144
145 msft->evt_prefix_len = rp->evt_prefix_len;
146 msft->features = __le64_to_cpu(rp->features);
147
Marcel Holtmanna61d6712021-04-06 21:55:56 +0200148 if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY)
149 hdev->msft_curve_validity = true;
150
Miao-chen Chou145373c2020-04-03 21:44:01 +0200151 kfree_skb(skb);
152 return true;
153
154failed:
155 kfree_skb(skb);
156 return false;
157}
158
Manish Mandlikce818432021-09-21 14:47:10 -0700159static void reregister_monitor(struct hci_dev *hdev, int handle)
Archie Pusaka4a376822021-01-22 16:36:14 +0800160{
161 struct adv_monitor *monitor;
162 struct msft_data *msft = hdev->msft_data;
163 int err;
164
165 while (1) {
166 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
167 if (!monitor) {
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700168 /* All monitors have been resumed */
169 msft->resuming = false;
Luiz Augusto von Dentz5bee2fd2021-10-27 16:58:43 -0700170 hci_update_passive_scan(hdev);
Archie Pusaka4a376822021-01-22 16:36:14 +0800171 return;
172 }
173
174 msft->pending_add_handle = (u16)handle;
175 err = __msft_add_monitor_pattern(hdev, monitor);
176
177 /* If success, we return and wait for monitor added callback */
178 if (!err)
179 return;
180
181 /* Otherwise remove the monitor and keep registering */
182 hci_free_adv_monitor(hdev, monitor);
183 handle++;
184 }
185}
186
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700187/* 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
190 */
191static struct msft_monitor_advertisement_handle_data *msft_find_handle_data
192 (struct hci_dev *hdev, u16 handle, bool is_mgmt)
Manish Mandlikce818432021-09-21 14:47:10 -0700193{
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700194 struct msft_monitor_advertisement_handle_data *entry;
195 struct msft_data *msft = hdev->msft_data;
196
197 list_for_each_entry(entry, &msft->handle_map, list) {
198 if (is_mgmt && entry->mgmt_handle == handle)
199 return entry;
200 if (!is_mgmt && entry->msft_handle == handle)
201 return entry;
202 }
203
204 return NULL;
205}
206
207static void msft_le_monitor_advertisement_cb(struct hci_dev *hdev,
208 u8 status, u16 opcode,
209 struct sk_buff *skb)
210{
211 struct msft_rp_le_monitor_advertisement *rp;
Manish Mandlikce818432021-09-21 14:47:10 -0700212 struct adv_monitor *monitor;
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700213 struct msft_monitor_advertisement_handle_data *handle_data;
214 struct msft_data *msft = hdev->msft_data;
215
216 hci_dev_lock(hdev);
217
218 monitor = idr_find(&hdev->adv_monitors_idr, msft->pending_add_handle);
219 if (!monitor) {
220 bt_dev_err(hdev, "msft add advmon: monitor %u is not found!",
221 msft->pending_add_handle);
222 status = HCI_ERROR_UNSPECIFIED;
223 goto unlock;
224 }
225
226 if (status)
227 goto unlock;
228
229 rp = (struct msft_rp_le_monitor_advertisement *)skb->data;
230 if (skb->len < sizeof(*rp)) {
231 status = HCI_ERROR_UNSPECIFIED;
232 goto unlock;
233 }
234
235 handle_data = kmalloc(sizeof(*handle_data), GFP_KERNEL);
236 if (!handle_data) {
237 status = HCI_ERROR_UNSPECIFIED;
238 goto unlock;
239 }
240
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);
245
246 monitor->state = ADV_MONITOR_STATE_OFFLOADED;
247
248unlock:
249 if (status && monitor)
250 hci_free_adv_monitor(hdev, monitor);
251
252 hci_dev_unlock(hdev);
253
254 if (!msft->resuming)
255 hci_add_adv_patterns_monitor_complete(hdev, status);
256}
257
258static void msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev,
259 u8 status, u16 opcode,
260 struct sk_buff *skb)
261{
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;
Manish Mandlikce818432021-09-21 14:47:10 -0700266 struct msft_data *msft = hdev->msft_data;
267 int err;
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700268 bool pending;
Manish Mandlikce818432021-09-21 14:47:10 -0700269
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700270 if (status)
271 goto done;
272
273 rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data;
274 if (skb->len < sizeof(*rp)) {
275 status = HCI_ERROR_UNSPECIFIED;
276 goto done;
277 }
278
279 hci_dev_lock(hdev);
280
281 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
282 handle_data = msft_find_handle_data(hdev, cp->handle, false);
283
284 if (handle_data) {
285 monitor = idr_find(&hdev->adv_monitors_idr,
286 handle_data->mgmt_handle);
287
288 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
289 monitor->state = ADV_MONITOR_STATE_REGISTERED;
290
291 /* Do not free the monitor if it is being removed due to
292 * suspend. It will be re-monitored on resume.
293 */
294 if (monitor && !msft->suspending)
295 hci_free_adv_monitor(hdev, monitor);
296
297 list_del(&handle_data->list);
298 kfree(handle_data);
299 }
300
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.
304 */
305 if (msft->pending_remove_handle == 0) {
306 pending = hci_remove_all_adv_monitor(hdev, &err);
307 if (pending) {
308 hci_dev_unlock(hdev);
Manish Mandlikce818432021-09-21 14:47:10 -0700309 return;
310 }
311
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700312 if (err)
313 status = HCI_ERROR_UNSPECIFIED;
314 }
Manish Mandlikce818432021-09-21 14:47:10 -0700315
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700316 hci_dev_unlock(hdev);
Manish Mandlikce818432021-09-21 14:47:10 -0700317
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700318done:
319 if (!msft->suspending)
320 hci_remove_adv_monitor_complete(hdev, status);
321}
322
323static int msft_remove_monitor_sync(struct hci_dev *hdev,
324 struct adv_monitor *monitor)
325{
326 struct msft_cp_le_cancel_monitor_advertisement cp;
327 struct msft_monitor_advertisement_handle_data *handle_data;
328 struct sk_buff *skb;
329 u8 status;
330
331 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
332
333 /* If no matched handle, just remove without telling controller */
334 if (!handle_data)
335 return -ENOENT;
336
337 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
338 cp.handle = handle_data->msft_handle;
339
340 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
341 HCI_CMD_TIMEOUT);
342 if (IS_ERR(skb))
343 return PTR_ERR(skb);
344
345 status = skb->data[0];
346 skb_pull(skb, 1);
347
348 msft_le_cancel_monitor_advertisement_cb(hdev, status, hdev->msft_opcode,
349 skb);
350
351 return status;
352}
353
354/* This function requires the caller holds hci_req_sync_lock */
355int msft_suspend_sync(struct hci_dev *hdev)
356{
357 struct msft_data *msft = hdev->msft_data;
358 struct adv_monitor *monitor;
359 int handle = 0;
360
361 if (!msft || !msft_monitor_supported(hdev))
362 return 0;
363
364 msft->suspending = true;
365
366 while (1) {
367 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
368 if (!monitor)
369 break;
370
371 msft_remove_monitor_sync(hdev, monitor);
372
Manish Mandlikce818432021-09-21 14:47:10 -0700373 handle++;
374 }
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700375
376 /* All monitors have been removed */
377 msft->suspending = false;
378
379 return 0;
Manish Mandlikce818432021-09-21 14:47:10 -0700380}
381
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700382static bool msft_monitor_rssi_valid(struct adv_monitor *monitor)
Manish Mandlikce818432021-09-21 14:47:10 -0700383{
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700384 struct adv_rssi_thresholds *r = &monitor->rssi;
Manish Mandlikce818432021-09-21 14:47:10 -0700385
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700386 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)
390 return false;
Manish Mandlikce818432021-09-21 14:47:10 -0700391
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700392 /* High_threshold_timeout is not supported,
393 * once high_threshold is reached, events are immediately reported.
394 */
395 if (r->high_threshold_timeout != 0)
396 return false;
397
398 if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX)
399 return false;
400
401 /* Sampling period from 0x00 to 0xFF are all allowed */
402 return true;
Manish Mandlikce818432021-09-21 14:47:10 -0700403}
404
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700405static bool msft_monitor_pattern_valid(struct adv_monitor *monitor)
406{
407 return msft_monitor_rssi_valid(monitor);
408 /* No additional check needed for pattern-based monitor */
409}
410
411static int msft_add_monitor_sync(struct hci_dev *hdev,
412 struct adv_monitor *monitor)
413{
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;
421 struct sk_buff *skb;
422 u8 status;
423
424 if (!msft_monitor_pattern_valid(monitor))
425 return -EINVAL;
426
427 list_for_each_entry(entry, &monitor->patterns, list) {
428 pattern_count++;
429 total_size += sizeof(*pattern) + entry->length;
430 }
431
432 cp = kmalloc(total_size, GFP_KERNEL);
433 if (!cp)
434 return -ENOMEM;
435
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;
441
442 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
443
444 pattern_data = (void *)cp->data;
445 pattern_data->count = pattern_count;
446
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;
455 }
456
457 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp,
458 HCI_CMD_TIMEOUT);
459 kfree(cp);
460
461 if (IS_ERR(skb))
462 return PTR_ERR(skb);
463
464 status = skb->data[0];
465 skb_pull(skb, 1);
466
467 msft_le_monitor_advertisement_cb(hdev, status, hdev->msft_opcode, skb);
468
469 return status;
470}
471
472/* This function requires the caller holds hci_req_sync_lock */
473int msft_resume_sync(struct hci_dev *hdev)
Manish Mandlikce818432021-09-21 14:47:10 -0700474{
475 struct msft_data *msft = hdev->msft_data;
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700476 struct adv_monitor *monitor;
477 int handle = 0;
Manish Mandlikce818432021-09-21 14:47:10 -0700478
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700479 if (!msft || !msft_monitor_supported(hdev))
480 return 0;
Manish Mandlikce818432021-09-21 14:47:10 -0700481
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700482 msft->resuming = true;
483
484 while (1) {
485 monitor = idr_get_next(&hdev->adv_monitors_idr, &handle);
486 if (!monitor)
487 break;
488
489 msft_add_monitor_sync(hdev, monitor);
490
491 handle++;
Manish Mandlikce818432021-09-21 14:47:10 -0700492 }
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700493
494 /* All monitors have been resumed */
495 msft->resuming = false;
496
497 return 0;
Manish Mandlikce818432021-09-21 14:47:10 -0700498}
499
Miao-chen Chou145373c2020-04-03 21:44:01 +0200500void msft_do_open(struct hci_dev *hdev)
501{
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700502 struct msft_data *msft = hdev->msft_data;
Miao-chen Chou145373c2020-04-03 21:44:01 +0200503
504 if (hdev->msft_opcode == HCI_OP_NOP)
505 return;
506
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700507 if (!msft) {
508 bt_dev_err(hdev, "MSFT extension not registered");
509 return;
510 }
511
Miao-chen Chou145373c2020-04-03 21:44:01 +0200512 bt_dev_dbg(hdev, "Initialize MSFT extension");
513
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700514 /* Reset existing MSFT data before re-reading */
515 kfree(msft->evt_prefix);
516 msft->evt_prefix = NULL;
517 msft->evt_prefix_len = 0;
518 msft->features = 0;
Miao-chen Chou145373c2020-04-03 21:44:01 +0200519
520 if (!read_supported_features(hdev, msft)) {
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700521 hdev->msft_data = NULL;
Miao-chen Chou145373c2020-04-03 21:44:01 +0200522 kfree(msft);
523 return;
524 }
525
Archie Pusaka4a376822021-01-22 16:36:14 +0800526 if (msft_monitor_supported(hdev)) {
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700527 msft->resuming = true;
Archie Pusaka394566b2021-01-22 16:36:15 +0800528 msft_set_filter_enable(hdev, true);
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700529 /* Monitors get removed on power off, so we need to explicitly
530 * tell the controller to re-monitor.
531 */
Manish Mandlikce818432021-09-21 14:47:10 -0700532 reregister_monitor(hdev, 0);
Archie Pusaka4a376822021-01-22 16:36:14 +0800533 }
Miao-chen Chou145373c2020-04-03 21:44:01 +0200534}
535
536void msft_do_close(struct hci_dev *hdev)
537{
538 struct msft_data *msft = hdev->msft_data;
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800539 struct msft_monitor_advertisement_handle_data *handle_data, *tmp;
Archie Pusaka4a376822021-01-22 16:36:14 +0800540 struct adv_monitor *monitor;
Miao-chen Chou145373c2020-04-03 21:44:01 +0200541
542 if (!msft)
543 return;
544
545 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
546
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700547 /* The controller will silently remove all monitors on power off.
548 * Therefore, remove handle_data mapping and reset monitor state.
549 */
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800550 list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) {
Archie Pusaka4a376822021-01-22 16:36:14 +0800551 monitor = idr_find(&hdev->adv_monitors_idr,
552 handle_data->mgmt_handle);
553
554 if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED)
555 monitor->state = ADV_MONITOR_STATE_REGISTERED;
556
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800557 list_del(&handle_data->list);
558 kfree(handle_data);
559 }
Miao-chen Chou5031ffc2021-09-09 14:10:23 -0700560}
561
562void msft_register(struct hci_dev *hdev)
563{
564 struct msft_data *msft = NULL;
565
566 bt_dev_dbg(hdev, "Register MSFT extension");
567
568 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
569 if (!msft) {
570 bt_dev_err(hdev, "Failed to register MSFT extension");
571 return;
572 }
573
574 INIT_LIST_HEAD(&msft->handle_map);
575 hdev->msft_data = msft;
576}
577
578void msft_unregister(struct hci_dev *hdev)
579{
580 struct msft_data *msft = hdev->msft_data;
581
582 if (!msft)
583 return;
584
585 bt_dev_dbg(hdev, "Unregister MSFT extension");
586
587 hdev->msft_data = NULL;
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800588
Miao-chen Chou145373c2020-04-03 21:44:01 +0200589 kfree(msft->evt_prefix);
590 kfree(msft);
591}
592
Luiz Augusto von Dentz3e54c582021-12-01 10:55:03 -0800593void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb)
Miao-chen Chou145373c2020-04-03 21:44:01 +0200594{
595 struct msft_data *msft = hdev->msft_data;
596 u8 event;
597
598 if (!msft)
599 return;
600
601 /* When the extension has defined an event prefix, check that it
602 * matches, and otherwise just return.
603 */
604 if (msft->evt_prefix_len > 0) {
605 if (skb->len < msft->evt_prefix_len)
606 return;
607
608 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
609 return;
610
611 skb_pull(skb, msft->evt_prefix_len);
612 }
613
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.
616 */
617 if (skb->len < 1)
618 return;
619
620 event = *skb->data;
621 skb_pull(skb, 1);
622
623 bt_dev_dbg(hdev, "MSFT vendor event %u", event);
624}
Miao-chen Choue5e1e7f2020-06-17 16:39:13 +0200625
626__u64 msft_get_features(struct hci_dev *hdev)
627{
628 struct msft_data *msft = hdev->msft_data;
629
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800630 return msft ? msft->features : 0;
631}
632
Archie Pusaka394566b2021-01-22 16:36:15 +0800633static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev,
634 u8 status, u16 opcode,
635 struct sk_buff *skb)
636{
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;
640
641 rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data;
642 if (skb->len < sizeof(*rp))
643 return;
644
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.
651 */
652 if (status != 0x00 && status != 0x0C)
653 return;
654
655 hci_dev_lock(hdev);
656
657 cp = hci_sent_cmd_data(hdev, hdev->msft_opcode);
658 msft->filter_enabled = cp->enable;
659
660 if (status == 0x0C)
661 bt_dev_warn(hdev, "MSFT filter_enable is already %s",
662 cp->enable ? "on" : "off");
663
664 hci_dev_unlock(hdev);
665}
666
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800667/* This function requires the caller holds hdev->lock */
Archie Pusaka4a376822021-01-22 16:36:14 +0800668static int __msft_add_monitor_pattern(struct hci_dev *hdev,
669 struct adv_monitor *monitor)
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800670{
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;
680 int err = 0;
681
Archie Pusakaa2a4ded2021-01-22 16:36:12 +0800682 if (!msft_monitor_pattern_valid(monitor))
683 return -EINVAL;
684
685 list_for_each_entry(entry, &monitor->patterns, list) {
686 pattern_count++;
687 total_size += sizeof(*pattern) + entry->length;
688 }
689
690 cp = kmalloc(total_size, GFP_KERNEL);
691 if (!cp)
692 return -ENOMEM;
693
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;
699
700 cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN;
701
702 pattern_data = (void *)cp->data;
703 pattern_data->count = pattern_count;
704
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;
713 }
714
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);
718 kfree(cp);
719
720 if (!err)
721 msft->pending_add_handle = monitor->handle;
722
723 return err;
Miao-chen Choue5e1e7f2020-06-17 16:39:13 +0200724}
Archie Pusaka66bd0952021-01-22 16:36:13 +0800725
726/* This function requires the caller holds hdev->lock */
Archie Pusaka4a376822021-01-22 16:36:14 +0800727int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor)
728{
729 struct msft_data *msft = hdev->msft_data;
730
731 if (!msft)
732 return -EOPNOTSUPP;
733
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700734 if (msft->resuming || msft->suspending)
Archie Pusaka4a376822021-01-22 16:36:14 +0800735 return -EBUSY;
736
737 return __msft_add_monitor_pattern(hdev, monitor);
738}
739
740/* This function requires the caller holds hdev->lock */
Manish Mandlikce818432021-09-21 14:47:10 -0700741static int __msft_remove_monitor(struct hci_dev *hdev,
742 struct adv_monitor *monitor, u16 handle)
Archie Pusaka66bd0952021-01-22 16:36:13 +0800743{
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;
748 int err = 0;
749
Archie Pusaka66bd0952021-01-22 16:36:13 +0800750 handle_data = msft_find_handle_data(hdev, monitor->handle, true);
751
752 /* If no matched handle, just remove without telling controller */
753 if (!handle_data)
754 return -ENOENT;
755
756 cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT;
757 cp.handle = handle_data->msft_handle;
758
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);
762
763 if (!err)
764 msft->pending_remove_handle = handle;
765
766 return err;
767}
Archie Pusaka394566b2021-01-22 16:36:15 +0800768
Manish Mandlikce818432021-09-21 14:47:10 -0700769/* This function requires the caller holds hdev->lock */
770int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor,
771 u16 handle)
772{
773 struct msft_data *msft = hdev->msft_data;
774
775 if (!msft)
776 return -EOPNOTSUPP;
777
Luiz Augusto von Dentz182ee452021-10-27 16:59:00 -0700778 if (msft->resuming || msft->suspending)
Manish Mandlikce818432021-09-21 14:47:10 -0700779 return -EBUSY;
780
781 return __msft_remove_monitor(hdev, monitor, handle);
782}
783
Howard Chungbf6a4e32021-01-22 16:36:17 +0800784void msft_req_add_set_filter_enable(struct hci_request *req, bool enable)
785{
786 struct hci_dev *hdev = req->hdev;
787 struct msft_cp_le_set_advertisement_filter_enable cp;
788
789 cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE;
790 cp.enable = enable;
791
792 hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp);
793}
794
Archie Pusaka394566b2021-01-22 16:36:15 +0800795int msft_set_filter_enable(struct hci_dev *hdev, bool enable)
796{
Archie Pusaka394566b2021-01-22 16:36:15 +0800797 struct hci_request req;
798 struct msft_data *msft = hdev->msft_data;
799 int err;
800
801 if (!msft)
802 return -EOPNOTSUPP;
803
Archie Pusaka394566b2021-01-22 16:36:15 +0800804 hci_req_init(&req, hdev);
Howard Chungbf6a4e32021-01-22 16:36:17 +0800805 msft_req_add_set_filter_enable(&req, enable);
Archie Pusaka394566b2021-01-22 16:36:15 +0800806 err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb);
807
808 return err;
809}
Marcel Holtmanna61d6712021-04-06 21:55:56 +0200810
811bool msft_curve_validity(struct hci_dev *hdev)
812{
813 return hdev->msft_curve_validity;
814}