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