1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * BlueZ - Bluetooth protocol stack for Linux |
4 | * |
5 | * Copyright (C) 2021 Intel Corporation |
6 | */ |
7 | |
8 | #include <net/bluetooth/bluetooth.h> |
9 | #include <net/bluetooth/hci_core.h> |
10 | #include <net/bluetooth/mgmt.h> |
11 | |
12 | #include "eir.h" |
13 | |
14 | #define PNP_INFO_SVCLASS_ID 0x1200 |
15 | |
16 | u8 eir_append_local_name(struct hci_dev *hdev, u8 *ptr, u8 ad_len) |
17 | { |
18 | size_t short_len; |
19 | size_t complete_len; |
20 | |
21 | /* no space left for name (+ type + len) */ |
22 | if ((max_adv_len(hdev) - ad_len) < HCI_MAX_SHORT_NAME_LENGTH + 2) |
23 | return ad_len; |
24 | |
25 | /* use complete name if present and fits */ |
26 | complete_len = strnlen(p: hdev->dev_name, maxlen: sizeof(hdev->dev_name)); |
27 | if (complete_len && complete_len <= HCI_MAX_SHORT_NAME_LENGTH) |
28 | return eir_append_data(eir: ptr, eir_len: ad_len, EIR_NAME_COMPLETE, |
29 | data: hdev->dev_name, data_len: complete_len); |
30 | |
31 | /* use short name if present */ |
32 | short_len = strnlen(p: hdev->short_name, maxlen: sizeof(hdev->short_name)); |
33 | if (short_len) |
34 | return eir_append_data(eir: ptr, eir_len: ad_len, EIR_NAME_SHORT, |
35 | data: hdev->short_name, |
36 | data_len: short_len); |
37 | |
38 | /* use shortened full name if present, we already know that name |
39 | * is longer then HCI_MAX_SHORT_NAME_LENGTH |
40 | */ |
41 | if (complete_len) |
42 | return eir_append_data(eir: ptr, eir_len: ad_len, EIR_NAME_SHORT, |
43 | data: hdev->dev_name, |
44 | HCI_MAX_SHORT_NAME_LENGTH); |
45 | |
46 | return ad_len; |
47 | } |
48 | |
49 | u8 eir_append_appearance(struct hci_dev *hdev, u8 *ptr, u8 ad_len) |
50 | { |
51 | return eir_append_le16(eir: ptr, eir_len: ad_len, EIR_APPEARANCE, data: hdev->appearance); |
52 | } |
53 | |
54 | u8 eir_append_service_data(u8 *eir, u16 eir_len, u16 uuid, u8 *data, |
55 | u8 data_len) |
56 | { |
57 | eir[eir_len++] = sizeof(u8) + sizeof(uuid) + data_len; |
58 | eir[eir_len++] = EIR_SERVICE_DATA; |
59 | put_unaligned_le16(val: uuid, p: &eir[eir_len]); |
60 | eir_len += sizeof(uuid); |
61 | memcpy(&eir[eir_len], data, data_len); |
62 | eir_len += data_len; |
63 | |
64 | return eir_len; |
65 | } |
66 | |
67 | static u8 *create_uuid16_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) |
68 | { |
69 | u8 *ptr = data, *uuids_start = NULL; |
70 | struct bt_uuid *uuid; |
71 | |
72 | if (len < 4) |
73 | return ptr; |
74 | |
75 | list_for_each_entry(uuid, &hdev->uuids, list) { |
76 | u16 uuid16; |
77 | |
78 | if (uuid->size != 16) |
79 | continue; |
80 | |
81 | uuid16 = get_unaligned_le16(p: &uuid->uuid[12]); |
82 | if (uuid16 < 0x1100) |
83 | continue; |
84 | |
85 | if (uuid16 == PNP_INFO_SVCLASS_ID) |
86 | continue; |
87 | |
88 | if (!uuids_start) { |
89 | uuids_start = ptr; |
90 | uuids_start[0] = 1; |
91 | uuids_start[1] = EIR_UUID16_ALL; |
92 | ptr += 2; |
93 | } |
94 | |
95 | /* Stop if not enough space to put next UUID */ |
96 | if ((ptr - data) + sizeof(u16) > len) { |
97 | uuids_start[1] = EIR_UUID16_SOME; |
98 | break; |
99 | } |
100 | |
101 | *ptr++ = (uuid16 & 0x00ff); |
102 | *ptr++ = (uuid16 & 0xff00) >> 8; |
103 | uuids_start[0] += sizeof(uuid16); |
104 | } |
105 | |
106 | return ptr; |
107 | } |
108 | |
109 | static u8 *create_uuid32_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) |
110 | { |
111 | u8 *ptr = data, *uuids_start = NULL; |
112 | struct bt_uuid *uuid; |
113 | |
114 | if (len < 6) |
115 | return ptr; |
116 | |
117 | list_for_each_entry(uuid, &hdev->uuids, list) { |
118 | if (uuid->size != 32) |
119 | continue; |
120 | |
121 | if (!uuids_start) { |
122 | uuids_start = ptr; |
123 | uuids_start[0] = 1; |
124 | uuids_start[1] = EIR_UUID32_ALL; |
125 | ptr += 2; |
126 | } |
127 | |
128 | /* Stop if not enough space to put next UUID */ |
129 | if ((ptr - data) + sizeof(u32) > len) { |
130 | uuids_start[1] = EIR_UUID32_SOME; |
131 | break; |
132 | } |
133 | |
134 | memcpy(ptr, &uuid->uuid[12], sizeof(u32)); |
135 | ptr += sizeof(u32); |
136 | uuids_start[0] += sizeof(u32); |
137 | } |
138 | |
139 | return ptr; |
140 | } |
141 | |
142 | static u8 *create_uuid128_list(struct hci_dev *hdev, u8 *data, ptrdiff_t len) |
143 | { |
144 | u8 *ptr = data, *uuids_start = NULL; |
145 | struct bt_uuid *uuid; |
146 | |
147 | if (len < 18) |
148 | return ptr; |
149 | |
150 | list_for_each_entry(uuid, &hdev->uuids, list) { |
151 | if (uuid->size != 128) |
152 | continue; |
153 | |
154 | if (!uuids_start) { |
155 | uuids_start = ptr; |
156 | uuids_start[0] = 1; |
157 | uuids_start[1] = EIR_UUID128_ALL; |
158 | ptr += 2; |
159 | } |
160 | |
161 | /* Stop if not enough space to put next UUID */ |
162 | if ((ptr - data) + 16 > len) { |
163 | uuids_start[1] = EIR_UUID128_SOME; |
164 | break; |
165 | } |
166 | |
167 | memcpy(ptr, uuid->uuid, 16); |
168 | ptr += 16; |
169 | uuids_start[0] += 16; |
170 | } |
171 | |
172 | return ptr; |
173 | } |
174 | |
175 | void eir_create(struct hci_dev *hdev, u8 *data) |
176 | { |
177 | u8 *ptr = data; |
178 | size_t name_len; |
179 | |
180 | name_len = strnlen(p: hdev->dev_name, maxlen: sizeof(hdev->dev_name)); |
181 | |
182 | if (name_len > 0) { |
183 | /* EIR Data type */ |
184 | if (name_len > 48) { |
185 | name_len = 48; |
186 | ptr[1] = EIR_NAME_SHORT; |
187 | } else { |
188 | ptr[1] = EIR_NAME_COMPLETE; |
189 | } |
190 | |
191 | /* EIR Data length */ |
192 | ptr[0] = name_len + 1; |
193 | |
194 | memcpy(ptr + 2, hdev->dev_name, name_len); |
195 | |
196 | ptr += (name_len + 2); |
197 | } |
198 | |
199 | if (hdev->inq_tx_power != HCI_TX_POWER_INVALID) { |
200 | ptr[0] = 2; |
201 | ptr[1] = EIR_TX_POWER; |
202 | ptr[2] = (u8)hdev->inq_tx_power; |
203 | |
204 | ptr += 3; |
205 | } |
206 | |
207 | if (hdev->devid_source > 0) { |
208 | ptr[0] = 9; |
209 | ptr[1] = EIR_DEVICE_ID; |
210 | |
211 | put_unaligned_le16(val: hdev->devid_source, p: ptr + 2); |
212 | put_unaligned_le16(val: hdev->devid_vendor, p: ptr + 4); |
213 | put_unaligned_le16(val: hdev->devid_product, p: ptr + 6); |
214 | put_unaligned_le16(val: hdev->devid_version, p: ptr + 8); |
215 | |
216 | ptr += 10; |
217 | } |
218 | |
219 | ptr = create_uuid16_list(hdev, data: ptr, HCI_MAX_EIR_LENGTH - (ptr - data)); |
220 | ptr = create_uuid32_list(hdev, data: ptr, HCI_MAX_EIR_LENGTH - (ptr - data)); |
221 | ptr = create_uuid128_list(hdev, data: ptr, HCI_MAX_EIR_LENGTH - (ptr - data)); |
222 | } |
223 | |
224 | u8 eir_create_per_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr) |
225 | { |
226 | struct adv_info *adv = NULL; |
227 | u8 ad_len = 0; |
228 | |
229 | /* Return 0 when the current instance identifier is invalid. */ |
230 | if (instance) { |
231 | adv = hci_find_adv_instance(hdev, instance); |
232 | if (!adv) |
233 | return 0; |
234 | } |
235 | |
236 | if (adv) { |
237 | memcpy(ptr, adv->per_adv_data, adv->per_adv_data_len); |
238 | ad_len += adv->per_adv_data_len; |
239 | ptr += adv->per_adv_data_len; |
240 | } |
241 | |
242 | return ad_len; |
243 | } |
244 | |
245 | u8 eir_create_adv_data(struct hci_dev *hdev, u8 instance, u8 *ptr) |
246 | { |
247 | struct adv_info *adv = NULL; |
248 | u8 ad_len = 0, flags = 0; |
249 | u32 instance_flags; |
250 | |
251 | /* Return 0 when the current instance identifier is invalid. */ |
252 | if (instance) { |
253 | adv = hci_find_adv_instance(hdev, instance); |
254 | if (!adv) |
255 | return 0; |
256 | } |
257 | |
258 | instance_flags = hci_adv_instance_flags(hdev, instance); |
259 | |
260 | /* If instance already has the flags set skip adding it once |
261 | * again. |
262 | */ |
263 | if (adv && eir_get_data(eir: adv->adv_data, eir_len: adv->adv_data_len, EIR_FLAGS, |
264 | NULL)) |
265 | goto skip_flags; |
266 | |
267 | /* The Add Advertising command allows userspace to set both the general |
268 | * and limited discoverable flags. |
269 | */ |
270 | if (instance_flags & MGMT_ADV_FLAG_DISCOV) |
271 | flags |= LE_AD_GENERAL; |
272 | |
273 | if (instance_flags & MGMT_ADV_FLAG_LIMITED_DISCOV) |
274 | flags |= LE_AD_LIMITED; |
275 | |
276 | if (!hci_dev_test_flag(hdev, HCI_BREDR_ENABLED)) |
277 | flags |= LE_AD_NO_BREDR; |
278 | |
279 | if (flags || (instance_flags & MGMT_ADV_FLAG_MANAGED_FLAGS)) { |
280 | /* If a discovery flag wasn't provided, simply use the global |
281 | * settings. |
282 | */ |
283 | if (!flags) |
284 | flags |= mgmt_get_adv_discov_flags(hdev); |
285 | |
286 | /* If flags would still be empty, then there is no need to |
287 | * include the "Flags" AD field". |
288 | */ |
289 | if (flags) { |
290 | ptr[0] = 0x02; |
291 | ptr[1] = EIR_FLAGS; |
292 | ptr[2] = flags; |
293 | |
294 | ad_len += 3; |
295 | ptr += 3; |
296 | } |
297 | } |
298 | |
299 | skip_flags: |
300 | if (adv) { |
301 | memcpy(ptr, adv->adv_data, adv->adv_data_len); |
302 | ad_len += adv->adv_data_len; |
303 | ptr += adv->adv_data_len; |
304 | } |
305 | |
306 | if (instance_flags & MGMT_ADV_FLAG_TX_POWER) { |
307 | s8 adv_tx_power; |
308 | |
309 | if (ext_adv_capable(hdev)) { |
310 | if (adv) |
311 | adv_tx_power = adv->tx_power; |
312 | else |
313 | adv_tx_power = hdev->adv_tx_power; |
314 | } else { |
315 | adv_tx_power = hdev->adv_tx_power; |
316 | } |
317 | |
318 | /* Provide Tx Power only if we can provide a valid value for it */ |
319 | if (adv_tx_power != HCI_TX_POWER_INVALID) { |
320 | ptr[0] = 0x02; |
321 | ptr[1] = EIR_TX_POWER; |
322 | ptr[2] = (u8)adv_tx_power; |
323 | |
324 | ad_len += 3; |
325 | ptr += 3; |
326 | } |
327 | } |
328 | |
329 | return ad_len; |
330 | } |
331 | |
332 | static u8 create_default_scan_rsp(struct hci_dev *hdev, u8 *ptr) |
333 | { |
334 | u8 scan_rsp_len = 0; |
335 | |
336 | if (hdev->appearance) |
337 | scan_rsp_len = eir_append_appearance(hdev, ptr, ad_len: scan_rsp_len); |
338 | |
339 | return eir_append_local_name(hdev, ptr, ad_len: scan_rsp_len); |
340 | } |
341 | |
342 | u8 eir_create_scan_rsp(struct hci_dev *hdev, u8 instance, u8 *ptr) |
343 | { |
344 | struct adv_info *adv; |
345 | u8 scan_rsp_len = 0; |
346 | |
347 | if (!instance) |
348 | return create_default_scan_rsp(hdev, ptr); |
349 | |
350 | adv = hci_find_adv_instance(hdev, instance); |
351 | if (!adv) |
352 | return 0; |
353 | |
354 | if ((adv->flags & MGMT_ADV_FLAG_APPEARANCE) && hdev->appearance) |
355 | scan_rsp_len = eir_append_appearance(hdev, ptr, ad_len: scan_rsp_len); |
356 | |
357 | memcpy(&ptr[scan_rsp_len], adv->scan_rsp_data, adv->scan_rsp_len); |
358 | |
359 | scan_rsp_len += adv->scan_rsp_len; |
360 | |
361 | if (adv->flags & MGMT_ADV_FLAG_LOCAL_NAME) |
362 | scan_rsp_len = eir_append_local_name(hdev, ptr, ad_len: scan_rsp_len); |
363 | |
364 | return scan_rsp_len; |
365 | } |
366 | |
367 | void *eir_get_service_data(u8 *eir, size_t eir_len, u16 uuid, size_t *len) |
368 | { |
369 | while ((eir = eir_get_data(eir, eir_len, EIR_SERVICE_DATA, data_len: len))) { |
370 | u16 value = get_unaligned_le16(p: eir); |
371 | |
372 | if (uuid == value) { |
373 | if (len) |
374 | *len -= 2; |
375 | return &eir[2]; |
376 | } |
377 | |
378 | eir += *len; |
379 | eir_len -= *len; |
380 | } |
381 | |
382 | return NULL; |
383 | } |
384 | |