Commit | Line | Data |
---|---|---|
145373cb MC |
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> | |
a2a4dedf | 8 | #include <net/bluetooth/mgmt.h> |
145373cb | 9 | |
a2a4dedf AP |
10 | #include "hci_request.h" |
11 | #include "mgmt_util.h" | |
145373cb MC |
12 | #include "msft.h" |
13 | ||
a2a4dedf AP |
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 | ||
145373cb MC |
18 | #define MSFT_OP_READ_SUPPORTED_FEATURES 0x00 |
19 | struct msft_cp_read_supported_features { | |
20 | __u8 sub_opcode; | |
21 | } __packed; | |
b08eadd2 | 22 | |
145373cb MC |
23 | struct msft_rp_read_supported_features { |
24 | __u8 status; | |
25 | __u8 sub_opcode; | |
26 | __le64 features; | |
27 | __u8 evt_prefix_len; | |
b08eadd2 | 28 | __u8 evt_prefix[]; |
145373cb MC |
29 | } __packed; |
30 | ||
a2a4dedf AP |
31 | #define MSFT_OP_LE_MONITOR_ADVERTISEMENT 0x03 |
32 | #define MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN 0x01 | |
33 | struct msft_le_monitor_advertisement_pattern { | |
34 | __u8 length; | |
35 | __u8 data_type; | |
36 | __u8 start_byte; | |
07d85dbe | 37 | __u8 pattern[]; |
a2a4dedf AP |
38 | }; |
39 | ||
40 | struct msft_le_monitor_advertisement_pattern_data { | |
41 | __u8 count; | |
07d85dbe | 42 | __u8 data[]; |
a2a4dedf AP |
43 | }; |
44 | ||
45 | struct 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; | |
07d85dbe | 52 | __u8 data[]; |
a2a4dedf AP |
53 | } __packed; |
54 | ||
55 | struct msft_rp_le_monitor_advertisement { | |
56 | __u8 status; | |
57 | __u8 sub_opcode; | |
58 | __u8 handle; | |
59 | } __packed; | |
60 | ||
66bd095a AP |
61 | #define MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT 0x04 |
62 | struct msft_cp_le_cancel_monitor_advertisement { | |
63 | __u8 sub_opcode; | |
64 | __u8 handle; | |
65 | } __packed; | |
66 | ||
67 | struct msft_rp_le_cancel_monitor_advertisement { | |
68 | __u8 status; | |
69 | __u8 sub_opcode; | |
70 | } __packed; | |
71 | ||
394566bf AP |
72 | #define MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE 0x05 |
73 | struct msft_cp_le_set_advertisement_filter_enable { | |
74 | __u8 sub_opcode; | |
75 | __u8 enable; | |
76 | } __packed; | |
77 | ||
78 | struct msft_rp_le_set_advertisement_filter_enable { | |
79 | __u8 status; | |
80 | __u8 sub_opcode; | |
81 | } __packed; | |
82 | ||
3368aa35 MM |
83 | #define MSFT_EV_LE_MONITOR_DEVICE 0x02 |
84 | struct msft_ev_le_monitor_device { | |
85 | __u8 addr_type; | |
86 | bdaddr_t bdaddr; | |
87 | __u8 monitor_handle; | |
88 | __u8 monitor_state; | |
89 | } __packed; | |
90 | ||
a2a4dedf AP |
91 | struct msft_monitor_advertisement_handle_data { |
92 | __u8 msft_handle; | |
93 | __u16 mgmt_handle; | |
94 | struct list_head list; | |
95 | }; | |
96 | ||
145373cb MC |
97 | struct msft_data { |
98 | __u64 features; | |
99 | __u8 evt_prefix_len; | |
100 | __u8 *evt_prefix; | |
a2a4dedf | 101 | struct list_head handle_map; |
182ee45d | 102 | __u8 resuming; |
ce81843b | 103 | __u8 suspending; |
394566bf | 104 | __u8 filter_enabled; |
145373cb MC |
105 | }; |
106 | ||
a2a4dedf AP |
107 | bool msft_monitor_supported(struct hci_dev *hdev) |
108 | { | |
109 | return !!(msft_get_features(hdev) & MSFT_FEATURE_MASK_LE_ADV_MONITOR); | |
110 | } | |
111 | ||
145373cb MC |
112 | static bool read_supported_features(struct hci_dev *hdev, |
113 | struct msft_data *msft) | |
114 | { | |
115 | struct msft_cp_read_supported_features cp; | |
116 | struct msft_rp_read_supported_features *rp; | |
117 | struct sk_buff *skb; | |
118 | ||
119 | cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES; | |
120 | ||
121 | skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, | |
122 | HCI_CMD_TIMEOUT); | |
ce78e557 SH |
123 | if (IS_ERR_OR_NULL(skb)) { |
124 | if (!skb) | |
125 | skb = ERR_PTR(-EIO); | |
126 | ||
145373cb MC |
127 | bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)", |
128 | PTR_ERR(skb)); | |
129 | return false; | |
130 | } | |
131 | ||
132 | if (skb->len < sizeof(*rp)) { | |
133 | bt_dev_err(hdev, "MSFT supported features length mismatch"); | |
134 | goto failed; | |
135 | } | |
136 | ||
137 | rp = (struct msft_rp_read_supported_features *)skb->data; | |
138 | ||
139 | if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES) | |
140 | goto failed; | |
141 | ||
142 | if (rp->evt_prefix_len > 0) { | |
143 | msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len, | |
144 | GFP_KERNEL); | |
145 | if (!msft->evt_prefix) | |
146 | goto failed; | |
147 | } | |
148 | ||
149 | msft->evt_prefix_len = rp->evt_prefix_len; | |
150 | msft->features = __le64_to_cpu(rp->features); | |
151 | ||
a61d6718 MH |
152 | if (msft->features & MSFT_FEATURE_MASK_CURVE_VALIDITY) |
153 | hdev->msft_curve_validity = true; | |
154 | ||
145373cb MC |
155 | kfree_skb(skb); |
156 | return true; | |
157 | ||
158 | failed: | |
159 | kfree_skb(skb); | |
160 | return false; | |
161 | } | |
162 | ||
182ee45d LAD |
163 | /* is_mgmt = true matches the handle exposed to userspace via mgmt. |
164 | * is_mgmt = false matches the handle used by the msft controller. | |
165 | * This function requires the caller holds hdev->lock | |
166 | */ | |
167 | static struct msft_monitor_advertisement_handle_data *msft_find_handle_data | |
168 | (struct hci_dev *hdev, u16 handle, bool is_mgmt) | |
169 | { | |
170 | struct msft_monitor_advertisement_handle_data *entry; | |
171 | struct msft_data *msft = hdev->msft_data; | |
172 | ||
173 | list_for_each_entry(entry, &msft->handle_map, list) { | |
174 | if (is_mgmt && entry->mgmt_handle == handle) | |
175 | return entry; | |
176 | if (!is_mgmt && entry->msft_handle == handle) | |
177 | return entry; | |
178 | } | |
179 | ||
180 | return NULL; | |
181 | } | |
182 | ||
3368aa35 MM |
183 | /* This function requires the caller holds hdev->lock */ |
184 | static int msft_monitor_device_del(struct hci_dev *hdev, __u16 mgmt_handle, | |
8d7f1677 MM |
185 | bdaddr_t *bdaddr, __u8 addr_type, |
186 | bool notify) | |
3368aa35 MM |
187 | { |
188 | struct monitored_device *dev, *tmp; | |
189 | int count = 0; | |
190 | ||
191 | list_for_each_entry_safe(dev, tmp, &hdev->monitored_devices, list) { | |
192 | /* mgmt_handle == 0 indicates remove all devices, whereas, | |
193 | * bdaddr == NULL indicates remove all devices matching the | |
194 | * mgmt_handle. | |
195 | */ | |
196 | if ((!mgmt_handle || dev->handle == mgmt_handle) && | |
197 | (!bdaddr || (!bacmp(bdaddr, &dev->bdaddr) && | |
198 | addr_type == dev->addr_type))) { | |
8d7f1677 MM |
199 | if (notify && dev->notified) { |
200 | mgmt_adv_monitor_device_lost(hdev, dev->handle, | |
201 | &dev->bdaddr, | |
202 | dev->addr_type); | |
203 | } | |
204 | ||
3368aa35 MM |
205 | list_del(&dev->list); |
206 | kfree(dev); | |
207 | count++; | |
208 | } | |
209 | } | |
210 | ||
211 | return count; | |
212 | } | |
213 | ||
b747a836 MM |
214 | static int msft_le_monitor_advertisement_cb(struct hci_dev *hdev, u16 opcode, |
215 | struct adv_monitor *monitor, | |
216 | struct sk_buff *skb) | |
182ee45d LAD |
217 | { |
218 | struct msft_rp_le_monitor_advertisement *rp; | |
182ee45d LAD |
219 | struct msft_monitor_advertisement_handle_data *handle_data; |
220 | struct msft_data *msft = hdev->msft_data; | |
b747a836 | 221 | int status = 0; |
182ee45d LAD |
222 | |
223 | hci_dev_lock(hdev); | |
224 | ||
b747a836 MM |
225 | rp = (struct msft_rp_le_monitor_advertisement *)skb->data; |
226 | if (skb->len < sizeof(*rp)) { | |
182ee45d LAD |
227 | status = HCI_ERROR_UNSPECIFIED; |
228 | goto unlock; | |
229 | } | |
230 | ||
b747a836 | 231 | status = rp->status; |
182ee45d LAD |
232 | if (status) |
233 | goto unlock; | |
234 | ||
182ee45d LAD |
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 | ||
248 | unlock: | |
b747a836 | 249 | if (status) |
182ee45d LAD |
250 | hci_free_adv_monitor(hdev, monitor); |
251 | ||
252 | hci_dev_unlock(hdev); | |
253 | ||
b747a836 | 254 | return status; |
182ee45d LAD |
255 | } |
256 | ||
7cf5c297 MM |
257 | static int msft_le_cancel_monitor_advertisement_cb(struct hci_dev *hdev, |
258 | u16 opcode, | |
259 | struct adv_monitor *monitor, | |
260 | struct sk_buff *skb) | |
ce81843b | 261 | { |
182ee45d | 262 | struct msft_rp_le_cancel_monitor_advertisement *rp; |
182ee45d | 263 | struct msft_monitor_advertisement_handle_data *handle_data; |
ce81843b | 264 | struct msft_data *msft = hdev->msft_data; |
7cf5c297 | 265 | int status = 0; |
182ee45d LAD |
266 | |
267 | rp = (struct msft_rp_le_cancel_monitor_advertisement *)skb->data; | |
268 | if (skb->len < sizeof(*rp)) { | |
269 | status = HCI_ERROR_UNSPECIFIED; | |
270 | goto done; | |
271 | } | |
272 | ||
7cf5c297 MM |
273 | status = rp->status; |
274 | if (status) | |
275 | goto done; | |
276 | ||
182ee45d LAD |
277 | hci_dev_lock(hdev); |
278 | ||
7cf5c297 | 279 | handle_data = msft_find_handle_data(hdev, monitor->handle, true); |
182ee45d LAD |
280 | |
281 | if (handle_data) { | |
7cf5c297 | 282 | if (monitor->state == ADV_MONITOR_STATE_OFFLOADED) |
182ee45d LAD |
283 | monitor->state = ADV_MONITOR_STATE_REGISTERED; |
284 | ||
285 | /* Do not free the monitor if it is being removed due to | |
286 | * suspend. It will be re-monitored on resume. | |
287 | */ | |
7cf5c297 | 288 | if (!msft->suspending) { |
182ee45d LAD |
289 | hci_free_adv_monitor(hdev, monitor); |
290 | ||
37b63c68 MM |
291 | /* Clear any monitored devices by this Adv Monitor */ |
292 | msft_monitor_device_del(hdev, handle_data->mgmt_handle, | |
293 | NULL, 0, false); | |
294 | } | |
3368aa35 | 295 | |
182ee45d LAD |
296 | list_del(&handle_data->list); |
297 | kfree(handle_data); | |
298 | } | |
299 | ||
182ee45d LAD |
300 | hci_dev_unlock(hdev); |
301 | ||
302 | done: | |
7cf5c297 | 303 | return status; |
182ee45d LAD |
304 | } |
305 | ||
7cf5c297 | 306 | /* This function requires the caller holds hci_req_sync_lock */ |
182ee45d LAD |
307 | static int msft_remove_monitor_sync(struct hci_dev *hdev, |
308 | struct adv_monitor *monitor) | |
309 | { | |
310 | struct msft_cp_le_cancel_monitor_advertisement cp; | |
311 | struct msft_monitor_advertisement_handle_data *handle_data; | |
312 | struct sk_buff *skb; | |
182ee45d LAD |
313 | |
314 | handle_data = msft_find_handle_data(hdev, monitor->handle, true); | |
315 | ||
316 | /* If no matched handle, just remove without telling controller */ | |
317 | if (!handle_data) | |
318 | return -ENOENT; | |
319 | ||
320 | cp.sub_opcode = MSFT_OP_LE_CANCEL_MONITOR_ADVERTISEMENT; | |
321 | cp.handle = handle_data->msft_handle; | |
322 | ||
323 | skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp, | |
324 | HCI_CMD_TIMEOUT); | |
ce78e557 SH |
325 | if (IS_ERR_OR_NULL(skb)) { |
326 | if (!skb) | |
327 | return -EIO; | |
182ee45d | 328 | return PTR_ERR(skb); |
ce78e557 | 329 | } |
182ee45d | 330 | |
7cf5c297 MM |
331 | return msft_le_cancel_monitor_advertisement_cb(hdev, hdev->msft_opcode, |
332 | monitor, skb); | |
182ee45d LAD |
333 | } |
334 | ||
335 | /* This function requires the caller holds hci_req_sync_lock */ | |
336 | int msft_suspend_sync(struct hci_dev *hdev) | |
337 | { | |
338 | struct msft_data *msft = hdev->msft_data; | |
339 | struct adv_monitor *monitor; | |
340 | int handle = 0; | |
341 | ||
342 | if (!msft || !msft_monitor_supported(hdev)) | |
343 | return 0; | |
344 | ||
345 | msft->suspending = true; | |
346 | ||
347 | while (1) { | |
348 | monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); | |
349 | if (!monitor) | |
350 | break; | |
351 | ||
352 | msft_remove_monitor_sync(hdev, monitor); | |
ce81843b | 353 | |
ce81843b MM |
354 | handle++; |
355 | } | |
182ee45d LAD |
356 | |
357 | /* All monitors have been removed */ | |
358 | msft->suspending = false; | |
359 | ||
360 | return 0; | |
ce81843b MM |
361 | } |
362 | ||
182ee45d | 363 | static bool msft_monitor_rssi_valid(struct adv_monitor *monitor) |
ce81843b | 364 | { |
182ee45d | 365 | struct adv_rssi_thresholds *r = &monitor->rssi; |
ce81843b | 366 | |
182ee45d LAD |
367 | if (r->high_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || |
368 | r->high_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX || | |
369 | r->low_threshold < MSFT_RSSI_THRESHOLD_VALUE_MIN || | |
370 | r->low_threshold > MSFT_RSSI_THRESHOLD_VALUE_MAX) | |
371 | return false; | |
ce81843b | 372 | |
182ee45d LAD |
373 | /* High_threshold_timeout is not supported, |
374 | * once high_threshold is reached, events are immediately reported. | |
375 | */ | |
376 | if (r->high_threshold_timeout != 0) | |
377 | return false; | |
378 | ||
379 | if (r->low_threshold_timeout > MSFT_RSSI_LOW_TIMEOUT_MAX) | |
380 | return false; | |
381 | ||
382 | /* Sampling period from 0x00 to 0xFF are all allowed */ | |
383 | return true; | |
384 | } | |
385 | ||
386 | static bool msft_monitor_pattern_valid(struct adv_monitor *monitor) | |
387 | { | |
388 | return msft_monitor_rssi_valid(monitor); | |
389 | /* No additional check needed for pattern-based monitor */ | |
390 | } | |
391 | ||
392 | static int msft_add_monitor_sync(struct hci_dev *hdev, | |
393 | struct adv_monitor *monitor) | |
394 | { | |
395 | struct msft_cp_le_monitor_advertisement *cp; | |
396 | struct msft_le_monitor_advertisement_pattern_data *pattern_data; | |
397 | struct msft_le_monitor_advertisement_pattern *pattern; | |
398 | struct adv_pattern *entry; | |
399 | size_t total_size = sizeof(*cp) + sizeof(*pattern_data); | |
400 | ptrdiff_t offset = 0; | |
401 | u8 pattern_count = 0; | |
402 | struct sk_buff *skb; | |
182ee45d LAD |
403 | |
404 | if (!msft_monitor_pattern_valid(monitor)) | |
405 | return -EINVAL; | |
406 | ||
407 | list_for_each_entry(entry, &monitor->patterns, list) { | |
408 | pattern_count++; | |
409 | total_size += sizeof(*pattern) + entry->length; | |
ce81843b | 410 | } |
182ee45d LAD |
411 | |
412 | cp = kmalloc(total_size, GFP_KERNEL); | |
413 | if (!cp) | |
414 | return -ENOMEM; | |
415 | ||
416 | cp->sub_opcode = MSFT_OP_LE_MONITOR_ADVERTISEMENT; | |
417 | cp->rssi_high = monitor->rssi.high_threshold; | |
418 | cp->rssi_low = monitor->rssi.low_threshold; | |
419 | cp->rssi_low_interval = (u8)monitor->rssi.low_threshold_timeout; | |
420 | cp->rssi_sampling_period = monitor->rssi.sampling_period; | |
421 | ||
422 | cp->cond_type = MSFT_MONITOR_ADVERTISEMENT_TYPE_PATTERN; | |
423 | ||
424 | pattern_data = (void *)cp->data; | |
425 | pattern_data->count = pattern_count; | |
426 | ||
427 | list_for_each_entry(entry, &monitor->patterns, list) { | |
428 | pattern = (void *)(pattern_data->data + offset); | |
429 | /* the length also includes data_type and offset */ | |
430 | pattern->length = entry->length + 2; | |
431 | pattern->data_type = entry->ad_type; | |
432 | pattern->start_byte = entry->offset; | |
433 | memcpy(pattern->pattern, entry->value, entry->length); | |
434 | offset += sizeof(*pattern) + entry->length; | |
435 | } | |
436 | ||
437 | skb = __hci_cmd_sync(hdev, hdev->msft_opcode, total_size, cp, | |
438 | HCI_CMD_TIMEOUT); | |
439 | kfree(cp); | |
440 | ||
ce78e557 SH |
441 | if (IS_ERR_OR_NULL(skb)) { |
442 | if (!skb) | |
443 | return -EIO; | |
182ee45d | 444 | return PTR_ERR(skb); |
ce78e557 | 445 | } |
182ee45d | 446 | |
b747a836 MM |
447 | return msft_le_monitor_advertisement_cb(hdev, hdev->msft_opcode, |
448 | monitor, skb); | |
449 | } | |
182ee45d | 450 | |
b747a836 MM |
451 | /* This function requires the caller holds hci_req_sync_lock */ |
452 | static void reregister_monitor(struct hci_dev *hdev) | |
453 | { | |
454 | struct adv_monitor *monitor; | |
455 | struct msft_data *msft = hdev->msft_data; | |
456 | int handle = 0; | |
182ee45d | 457 | |
b747a836 MM |
458 | if (!msft) |
459 | return; | |
460 | ||
461 | msft->resuming = true; | |
462 | ||
463 | while (1) { | |
464 | monitor = idr_get_next(&hdev->adv_monitors_idr, &handle); | |
465 | if (!monitor) | |
466 | break; | |
467 | ||
468 | msft_add_monitor_sync(hdev, monitor); | |
469 | ||
470 | handle++; | |
471 | } | |
472 | ||
473 | /* All monitors have been reregistered */ | |
474 | msft->resuming = false; | |
ce81843b MM |
475 | } |
476 | ||
182ee45d LAD |
477 | /* This function requires the caller holds hci_req_sync_lock */ |
478 | int msft_resume_sync(struct hci_dev *hdev) | |
ce81843b MM |
479 | { |
480 | struct msft_data *msft = hdev->msft_data; | |
481 | ||
182ee45d LAD |
482 | if (!msft || !msft_monitor_supported(hdev)) |
483 | return 0; | |
ce81843b | 484 | |
37b63c68 MM |
485 | hci_dev_lock(hdev); |
486 | ||
487 | /* Clear already tracked devices on resume. Once the monitors are | |
488 | * reregistered, devices in range will be found again after resume. | |
489 | */ | |
490 | hdev->advmon_pend_notify = false; | |
491 | msft_monitor_device_del(hdev, 0, NULL, 0, true); | |
492 | ||
493 | hci_dev_unlock(hdev); | |
494 | ||
b747a836 | 495 | reregister_monitor(hdev); |
182ee45d LAD |
496 | |
497 | return 0; | |
ce81843b MM |
498 | } |
499 | ||
b747a836 | 500 | /* This function requires the caller holds hci_req_sync_lock */ |
145373cb MC |
501 | void msft_do_open(struct hci_dev *hdev) |
502 | { | |
5031ffcc | 503 | struct msft_data *msft = hdev->msft_data; |
145373cb MC |
504 | |
505 | if (hdev->msft_opcode == HCI_OP_NOP) | |
506 | return; | |
507 | ||
5031ffcc MC |
508 | if (!msft) { |
509 | bt_dev_err(hdev, "MSFT extension not registered"); | |
510 | return; | |
511 | } | |
512 | ||
145373cb MC |
513 | bt_dev_dbg(hdev, "Initialize MSFT extension"); |
514 | ||
5031ffcc MC |
515 | /* Reset existing MSFT data before re-reading */ |
516 | kfree(msft->evt_prefix); | |
517 | msft->evt_prefix = NULL; | |
518 | msft->evt_prefix_len = 0; | |
519 | msft->features = 0; | |
145373cb MC |
520 | |
521 | if (!read_supported_features(hdev, msft)) { | |
5031ffcc | 522 | hdev->msft_data = NULL; |
145373cb MC |
523 | kfree(msft); |
524 | return; | |
525 | } | |
526 | ||
4a37682c | 527 | if (msft_monitor_supported(hdev)) { |
182ee45d | 528 | msft->resuming = true; |
394566bf | 529 | msft_set_filter_enable(hdev, true); |
5031ffcc MC |
530 | /* Monitors get removed on power off, so we need to explicitly |
531 | * tell the controller to re-monitor. | |
532 | */ | |
b747a836 | 533 | reregister_monitor(hdev); |
4a37682c | 534 | } |
145373cb MC |
535 | } |
536 | ||
537 | void msft_do_close(struct hci_dev *hdev) | |
538 | { | |
539 | struct msft_data *msft = hdev->msft_data; | |
a2a4dedf | 540 | struct msft_monitor_advertisement_handle_data *handle_data, *tmp; |
4a37682c | 541 | struct adv_monitor *monitor; |
145373cb MC |
542 | |
543 | if (!msft) | |
544 | return; | |
545 | ||
546 | bt_dev_dbg(hdev, "Cleanup of MSFT extension"); | |
547 | ||
5031ffcc MC |
548 | /* The controller will silently remove all monitors on power off. |
549 | * Therefore, remove handle_data mapping and reset monitor state. | |
550 | */ | |
a2a4dedf | 551 | list_for_each_entry_safe(handle_data, tmp, &msft->handle_map, list) { |
4a37682c AP |
552 | monitor = idr_find(&hdev->adv_monitors_idr, |
553 | handle_data->mgmt_handle); | |
554 | ||
555 | if (monitor && monitor->state == ADV_MONITOR_STATE_OFFLOADED) | |
556 | monitor->state = ADV_MONITOR_STATE_REGISTERED; | |
557 | ||
a2a4dedf AP |
558 | list_del(&handle_data->list); |
559 | kfree(handle_data); | |
560 | } | |
3368aa35 MM |
561 | |
562 | hci_dev_lock(hdev); | |
563 | ||
8d7f1677 MM |
564 | /* Clear any devices that are being monitored and notify device lost */ |
565 | hdev->advmon_pend_notify = false; | |
566 | msft_monitor_device_del(hdev, 0, NULL, 0, true); | |
3368aa35 MM |
567 | |
568 | hci_dev_unlock(hdev); | |
5031ffcc MC |
569 | } |
570 | ||
571 | void msft_register(struct hci_dev *hdev) | |
572 | { | |
573 | struct msft_data *msft = NULL; | |
574 | ||
575 | bt_dev_dbg(hdev, "Register MSFT extension"); | |
576 | ||
577 | msft = kzalloc(sizeof(*msft), GFP_KERNEL); | |
578 | if (!msft) { | |
579 | bt_dev_err(hdev, "Failed to register MSFT extension"); | |
580 | return; | |
581 | } | |
582 | ||
583 | INIT_LIST_HEAD(&msft->handle_map); | |
584 | hdev->msft_data = msft; | |
585 | } | |
586 | ||
587 | void msft_unregister(struct hci_dev *hdev) | |
588 | { | |
589 | struct msft_data *msft = hdev->msft_data; | |
590 | ||
591 | if (!msft) | |
592 | return; | |
593 | ||
594 | bt_dev_dbg(hdev, "Unregister MSFT extension"); | |
595 | ||
596 | hdev->msft_data = NULL; | |
a2a4dedf | 597 | |
145373cb MC |
598 | kfree(msft->evt_prefix); |
599 | kfree(msft); | |
600 | } | |
601 | ||
3368aa35 MM |
602 | /* This function requires the caller holds hdev->lock */ |
603 | static void msft_device_found(struct hci_dev *hdev, bdaddr_t *bdaddr, | |
604 | __u8 addr_type, __u16 mgmt_handle) | |
605 | { | |
606 | struct monitored_device *dev; | |
607 | ||
608 | dev = kmalloc(sizeof(*dev), GFP_KERNEL); | |
609 | if (!dev) { | |
610 | bt_dev_err(hdev, "MSFT vendor event %u: no memory", | |
611 | MSFT_EV_LE_MONITOR_DEVICE); | |
612 | return; | |
613 | } | |
614 | ||
615 | bacpy(&dev->bdaddr, bdaddr); | |
616 | dev->addr_type = addr_type; | |
617 | dev->handle = mgmt_handle; | |
618 | dev->notified = false; | |
619 | ||
620 | INIT_LIST_HEAD(&dev->list); | |
621 | list_add(&dev->list, &hdev->monitored_devices); | |
8d7f1677 | 622 | hdev->advmon_pend_notify = true; |
3368aa35 MM |
623 | } |
624 | ||
625 | /* This function requires the caller holds hdev->lock */ | |
626 | static void msft_device_lost(struct hci_dev *hdev, bdaddr_t *bdaddr, | |
627 | __u8 addr_type, __u16 mgmt_handle) | |
628 | { | |
8d7f1677 MM |
629 | if (!msft_monitor_device_del(hdev, mgmt_handle, bdaddr, addr_type, |
630 | true)) { | |
3368aa35 MM |
631 | bt_dev_err(hdev, "MSFT vendor event %u: dev %pMR not in list", |
632 | MSFT_EV_LE_MONITOR_DEVICE, bdaddr); | |
633 | } | |
634 | } | |
635 | ||
636 | static void *msft_skb_pull(struct hci_dev *hdev, struct sk_buff *skb, | |
637 | u8 ev, size_t len) | |
638 | { | |
639 | void *data; | |
640 | ||
641 | data = skb_pull_data(skb, len); | |
642 | if (!data) | |
643 | bt_dev_err(hdev, "Malformed MSFT vendor event: 0x%02x", ev); | |
644 | ||
645 | return data; | |
646 | } | |
647 | ||
648 | /* This function requires the caller holds hdev->lock */ | |
649 | static void msft_monitor_device_evt(struct hci_dev *hdev, struct sk_buff *skb) | |
650 | { | |
651 | struct msft_ev_le_monitor_device *ev; | |
652 | struct msft_monitor_advertisement_handle_data *handle_data; | |
653 | u8 addr_type; | |
654 | ||
655 | ev = msft_skb_pull(hdev, skb, MSFT_EV_LE_MONITOR_DEVICE, sizeof(*ev)); | |
656 | if (!ev) | |
657 | return; | |
658 | ||
659 | bt_dev_dbg(hdev, | |
660 | "MSFT vendor event 0x%02x: handle 0x%04x state %d addr %pMR", | |
661 | MSFT_EV_LE_MONITOR_DEVICE, ev->monitor_handle, | |
662 | ev->monitor_state, &ev->bdaddr); | |
663 | ||
664 | handle_data = msft_find_handle_data(hdev, ev->monitor_handle, false); | |
5201d23c SH |
665 | if (!handle_data) |
666 | return; | |
3368aa35 MM |
667 | |
668 | switch (ev->addr_type) { | |
669 | case ADDR_LE_DEV_PUBLIC: | |
670 | addr_type = BDADDR_LE_PUBLIC; | |
671 | break; | |
672 | ||
673 | case ADDR_LE_DEV_RANDOM: | |
674 | addr_type = BDADDR_LE_RANDOM; | |
675 | break; | |
676 | ||
677 | default: | |
678 | bt_dev_err(hdev, | |
679 | "MSFT vendor event 0x%02x: unknown addr type 0x%02x", | |
680 | MSFT_EV_LE_MONITOR_DEVICE, ev->addr_type); | |
681 | return; | |
682 | } | |
683 | ||
684 | if (ev->monitor_state) | |
685 | msft_device_found(hdev, &ev->bdaddr, addr_type, | |
686 | handle_data->mgmt_handle); | |
687 | else | |
688 | msft_device_lost(hdev, &ev->bdaddr, addr_type, | |
689 | handle_data->mgmt_handle); | |
690 | } | |
691 | ||
3e54c589 | 692 | void msft_vendor_evt(struct hci_dev *hdev, void *data, struct sk_buff *skb) |
145373cb MC |
693 | { |
694 | struct msft_data *msft = hdev->msft_data; | |
3368aa35 MM |
695 | u8 *evt_prefix; |
696 | u8 *evt; | |
145373cb MC |
697 | |
698 | if (!msft) | |
699 | return; | |
700 | ||
701 | /* When the extension has defined an event prefix, check that it | |
702 | * matches, and otherwise just return. | |
703 | */ | |
704 | if (msft->evt_prefix_len > 0) { | |
3368aa35 MM |
705 | evt_prefix = msft_skb_pull(hdev, skb, 0, msft->evt_prefix_len); |
706 | if (!evt_prefix) | |
145373cb MC |
707 | return; |
708 | ||
3368aa35 | 709 | if (memcmp(evt_prefix, msft->evt_prefix, msft->evt_prefix_len)) |
145373cb | 710 | return; |
145373cb MC |
711 | } |
712 | ||
713 | /* Every event starts at least with an event code and the rest of | |
714 | * the data is variable and depends on the event code. | |
715 | */ | |
716 | if (skb->len < 1) | |
717 | return; | |
718 | ||
3368aa35 MM |
719 | evt = msft_skb_pull(hdev, skb, 0, sizeof(*evt)); |
720 | if (!evt) | |
721 | return; | |
722 | ||
723 | hci_dev_lock(hdev); | |
724 | ||
725 | switch (*evt) { | |
726 | case MSFT_EV_LE_MONITOR_DEVICE: | |
727 | msft_monitor_device_evt(hdev, skb); | |
728 | break; | |
145373cb | 729 | |
3368aa35 MM |
730 | default: |
731 | bt_dev_dbg(hdev, "MSFT vendor event 0x%02x", *evt); | |
732 | break; | |
733 | } | |
734 | ||
735 | hci_dev_unlock(hdev); | |
145373cb | 736 | } |
e5e1e7fd MC |
737 | |
738 | __u64 msft_get_features(struct hci_dev *hdev) | |
739 | { | |
740 | struct msft_data *msft = hdev->msft_data; | |
741 | ||
a2a4dedf AP |
742 | return msft ? msft->features : 0; |
743 | } | |
744 | ||
394566bf AP |
745 | static void msft_le_set_advertisement_filter_enable_cb(struct hci_dev *hdev, |
746 | u8 status, u16 opcode, | |
747 | struct sk_buff *skb) | |
748 | { | |
749 | struct msft_cp_le_set_advertisement_filter_enable *cp; | |
750 | struct msft_rp_le_set_advertisement_filter_enable *rp; | |
751 | struct msft_data *msft = hdev->msft_data; | |
752 | ||
753 | rp = (struct msft_rp_le_set_advertisement_filter_enable *)skb->data; | |
754 | if (skb->len < sizeof(*rp)) | |
755 | return; | |
756 | ||
757 | /* Error 0x0C would be returned if the filter enabled status is | |
758 | * already set to whatever we were trying to set. | |
759 | * Although the default state should be disabled, some controller set | |
760 | * the initial value to enabled. Because there is no way to know the | |
761 | * actual initial value before sending this command, here we also treat | |
762 | * error 0x0C as success. | |
763 | */ | |
764 | if (status != 0x00 && status != 0x0C) | |
765 | return; | |
766 | ||
767 | hci_dev_lock(hdev); | |
768 | ||
769 | cp = hci_sent_cmd_data(hdev, hdev->msft_opcode); | |
770 | msft->filter_enabled = cp->enable; | |
771 | ||
772 | if (status == 0x0C) | |
773 | bt_dev_warn(hdev, "MSFT filter_enable is already %s", | |
774 | cp->enable ? "on" : "off"); | |
775 | ||
776 | hci_dev_unlock(hdev); | |
777 | } | |
778 | ||
b747a836 | 779 | /* This function requires the caller holds hci_req_sync_lock */ |
4a37682c AP |
780 | int msft_add_monitor_pattern(struct hci_dev *hdev, struct adv_monitor *monitor) |
781 | { | |
782 | struct msft_data *msft = hdev->msft_data; | |
783 | ||
784 | if (!msft) | |
785 | return -EOPNOTSUPP; | |
786 | ||
182ee45d | 787 | if (msft->resuming || msft->suspending) |
4a37682c AP |
788 | return -EBUSY; |
789 | ||
b747a836 | 790 | return msft_add_monitor_sync(hdev, monitor); |
4a37682c AP |
791 | } |
792 | ||
7cf5c297 MM |
793 | /* This function requires the caller holds hci_req_sync_lock */ |
794 | int msft_remove_monitor(struct hci_dev *hdev, struct adv_monitor *monitor) | |
ce81843b MM |
795 | { |
796 | struct msft_data *msft = hdev->msft_data; | |
797 | ||
798 | if (!msft) | |
799 | return -EOPNOTSUPP; | |
800 | ||
182ee45d | 801 | if (msft->resuming || msft->suspending) |
ce81843b MM |
802 | return -EBUSY; |
803 | ||
7cf5c297 | 804 | return msft_remove_monitor_sync(hdev, monitor); |
ce81843b MM |
805 | } |
806 | ||
bf6a4e30 | 807 | void msft_req_add_set_filter_enable(struct hci_request *req, bool enable) |
394566bf | 808 | { |
bf6a4e30 | 809 | struct hci_dev *hdev = req->hdev; |
394566bf | 810 | struct msft_cp_le_set_advertisement_filter_enable cp; |
bf6a4e30 HC |
811 | |
812 | cp.sub_opcode = MSFT_OP_LE_SET_ADVERTISEMENT_FILTER_ENABLE; | |
813 | cp.enable = enable; | |
814 | ||
815 | hci_req_add(req, hdev->msft_opcode, sizeof(cp), &cp); | |
816 | } | |
817 | ||
818 | int msft_set_filter_enable(struct hci_dev *hdev, bool enable) | |
819 | { | |
394566bf AP |
820 | struct hci_request req; |
821 | struct msft_data *msft = hdev->msft_data; | |
822 | int err; | |
823 | ||
824 | if (!msft) | |
825 | return -EOPNOTSUPP; | |
826 | ||
394566bf | 827 | hci_req_init(&req, hdev); |
bf6a4e30 | 828 | msft_req_add_set_filter_enable(&req, enable); |
394566bf AP |
829 | err = hci_req_run_skb(&req, msft_le_set_advertisement_filter_enable_cb); |
830 | ||
831 | return err; | |
832 | } | |
a61d6718 MH |
833 | |
834 | bool msft_curve_validity(struct hci_dev *hdev) | |
835 | { | |
836 | return hdev->msft_curve_validity; | |
837 | } |