1 /*
2 *
3 * BlueZ - Bluetooth protocol stack for Linux
4 *
5 * Copyright (C) 2011 Nokia Corporation
6 * Copyright (C) 2011 Marcel Holtmann <marcel@holtmann.org>
7 *
8 *
9 * This program is free software; you can redistribute it and/or modify
10 * it under the terms of the GNU General Public License as published by
11 * the Free Software Foundation; either version 2 of the License, or
12 * (at your option) any later version.
13 *
14 * This program is distributed in the hope that it will be useful,
15 * but WITHOUT ANY WARRANTY; without even the implied warranty of
16 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
17 * GNU General Public License for more details.
18 *
19 * You should have received a copy of the GNU General Public License
20 * along with this program; if not, write to the Free Software
21 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
22 *
23 */
24 #include <errno.h>
25 #include <stdlib.h>
26 #include <stdint.h>
27 #include <glib.h>
28
29 #include <bluetooth/bluetooth.h>
30 #include <bluetooth/hci.h>
31 #include <bluetooth/sdp.h>
32
33 #include "glib-helper.h"
34 #include "eir.h"
35
36 #define EIR_FLAGS 0x01 /* flags */
37 #define EIR_UUID16_SOME 0x02 /* 16-bit UUID, more available */
38 #define EIR_UUID16_ALL 0x03 /* 16-bit UUID, all listed */
39 #define EIR_UUID32_SOME 0x04 /* 32-bit UUID, more available */
40 #define EIR_UUID32_ALL 0x05 /* 32-bit UUID, all listed */
41 #define EIR_UUID128_SOME 0x06 /* 128-bit UUID, more available */
42 #define EIR_UUID128_ALL 0x07 /* 128-bit UUID, all listed */
43 #define EIR_NAME_SHORT 0x08 /* shortened local name */
44 #define EIR_NAME_COMPLETE 0x09 /* complete local name */
45 #define EIR_TX_POWER 0x0A /* transmit power level */
46 #define EIR_DEVICE_ID 0x10 /* device ID */
47
eir_data_free(struct eir_data * eir)48 void eir_data_free(struct eir_data *eir)
49 {
50 g_slist_foreach(eir->services, (GFunc) g_free, NULL);
51 g_slist_free(eir->services);
52 g_free(eir->name);
53 }
54
eir_parse(struct eir_data * eir,uint8_t * eir_data)55 int eir_parse(struct eir_data *eir, uint8_t *eir_data)
56 {
57 uint16_t len = 0;
58 size_t total;
59 size_t uuid16_count = 0;
60 size_t uuid32_count = 0;
61 size_t uuid128_count = 0;
62 uint8_t *uuid16 = NULL;
63 uint8_t *uuid32 = NULL;
64 uint8_t *uuid128 = NULL;
65 uuid_t service;
66 char *uuid_str;
67 unsigned int i;
68
69 eir->flags = -1;
70
71 /* No EIR data to parse */
72 if (eir_data == NULL)
73 return 0;
74
75 while (len < HCI_MAX_EIR_LENGTH - 1) {
76 uint8_t field_len = eir_data[0];
77
78 /* Check for the end of EIR */
79 if (field_len == 0)
80 break;
81
82 switch (eir_data[1]) {
83 case EIR_UUID16_SOME:
84 case EIR_UUID16_ALL:
85 uuid16_count = field_len / 2;
86 uuid16 = &eir_data[2];
87 break;
88 case EIR_UUID32_SOME:
89 case EIR_UUID32_ALL:
90 uuid32_count = field_len / 4;
91 uuid32 = &eir_data[2];
92 break;
93 case EIR_UUID128_SOME:
94 case EIR_UUID128_ALL:
95 uuid128_count = field_len / 16;
96 uuid128 = &eir_data[2];
97 break;
98 case EIR_FLAGS:
99 eir->flags = eir_data[2];
100 break;
101 case EIR_NAME_SHORT:
102 case EIR_NAME_COMPLETE:
103 if (g_utf8_validate((char *) &eir_data[2],
104 field_len - 1, NULL))
105 eir->name = g_strndup((char *) &eir_data[2],
106 field_len - 1);
107 else
108 eir->name = g_strdup("");
109 eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE;
110 break;
111 }
112
113 len += field_len + 1;
114 eir_data += field_len + 1;
115 }
116
117 /* Bail out if got incorrect length */
118 if (len > HCI_MAX_EIR_LENGTH)
119 return -EINVAL;
120
121 total = uuid16_count + uuid32_count + uuid128_count;
122
123 /* No UUIDs were parsed, so skip code below */
124 if (!total)
125 return 0;
126
127 /* Generate uuids in SDP format (EIR data is Little Endian) */
128 service.type = SDP_UUID16;
129 for (i = 0; i < uuid16_count; i++) {
130 uint16_t val16 = uuid16[1];
131
132 val16 = (val16 << 8) + uuid16[0];
133 service.value.uuid16 = val16;
134 uuid_str = bt_uuid2string(&service);
135 eir->services = g_slist_append(eir->services, uuid_str);
136 uuid16 += 2;
137 }
138
139 service.type = SDP_UUID32;
140 for (i = uuid16_count; i < uuid32_count + uuid16_count; i++) {
141 uint32_t val32 = uuid32[3];
142 int k;
143
144 for (k = 2; k >= 0; k--)
145 val32 = (val32 << 8) + uuid32[k];
146
147 service.value.uuid32 = val32;
148 uuid_str = bt_uuid2string(&service);
149 eir->services = g_slist_append(eir->services, uuid_str);
150 uuid32 += 4;
151 }
152
153 service.type = SDP_UUID128;
154 for (i = uuid32_count + uuid16_count; i < total; i++) {
155 int k;
156
157 for (k = 0; k < 16; k++)
158 service.value.uuid128.data[k] = uuid128[16 - k - 1];
159
160 uuid_str = bt_uuid2string(&service);
161 eir->services = g_slist_append(eir->services, uuid_str);
162 uuid128 += 16;
163 }
164
165 return 0;
166 }
167
168 #define SIZEOF_UUID128 16
169
eir_generate_uuid128(GSList * list,uint8_t * ptr,uint16_t * eir_len)170 static void eir_generate_uuid128(GSList *list, uint8_t *ptr, uint16_t *eir_len)
171 {
172 int i, k, uuid_count = 0;
173 uint16_t len = *eir_len;
174 uint8_t *uuid128;
175 gboolean truncated = FALSE;
176
177 /* Store UUIDs in place, skip 2 bytes to write type and length later */
178 uuid128 = ptr + 2;
179
180 for (; list; list = list->next) {
181 struct uuid_info *uuid = list->data;
182 uint8_t *uuid128_data = uuid->uuid.value.uuid128.data;
183
184 if (uuid->uuid.type != SDP_UUID128)
185 continue;
186
187 /* Stop if not enough space to put next UUID128 */
188 if ((len + 2 + SIZEOF_UUID128) > HCI_MAX_EIR_LENGTH) {
189 truncated = TRUE;
190 break;
191 }
192
193 /* Check for duplicates, EIR data is Little Endian */
194 for (i = 0; i < uuid_count; i++) {
195 for (k = 0; k < SIZEOF_UUID128; k++) {
196 if (uuid128[i * SIZEOF_UUID128 + k] !=
197 uuid128_data[SIZEOF_UUID128 - 1 - k])
198 break;
199 }
200 if (k == SIZEOF_UUID128)
201 break;
202 }
203
204 if (i < uuid_count)
205 continue;
206
207 /* EIR data is Little Endian */
208 for (k = 0; k < SIZEOF_UUID128; k++)
209 uuid128[uuid_count * SIZEOF_UUID128 + k] =
210 uuid128_data[SIZEOF_UUID128 - 1 - k];
211
212 len += SIZEOF_UUID128;
213 uuid_count++;
214 }
215
216 if (uuid_count > 0 || truncated) {
217 /* EIR Data length */
218 ptr[0] = (uuid_count * SIZEOF_UUID128) + 1;
219 /* EIR Data type */
220 ptr[1] = truncated ? EIR_UUID128_SOME : EIR_UUID128_ALL;
221 len += 2;
222 *eir_len = len;
223 }
224 }
225
eir_create(const char * name,int8_t tx_power,uint16_t did_vendor,uint16_t did_product,uint16_t did_version,GSList * uuids,uint8_t * data)226 void eir_create(const char *name, int8_t tx_power, uint16_t did_vendor,
227 uint16_t did_product, uint16_t did_version,
228 GSList *uuids, uint8_t *data)
229 {
230 GSList *l;
231 uint8_t *ptr = data;
232 uint16_t eir_len = 0;
233 uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2];
234 int i, uuid_count = 0;
235 gboolean truncated = FALSE;
236 size_t name_len;
237
238 name_len = strlen(name);
239
240 if (name_len > 0) {
241 /* EIR Data type */
242 if (name_len > 48) {
243 name_len = 48;
244 ptr[1] = EIR_NAME_SHORT;
245 } else
246 ptr[1] = EIR_NAME_COMPLETE;
247
248 /* EIR Data length */
249 ptr[0] = name_len + 1;
250
251 memcpy(ptr + 2, name, name_len);
252
253 eir_len += (name_len + 2);
254 ptr += (name_len + 2);
255 }
256
257 if (tx_power != 0) {
258 *ptr++ = 2;
259 *ptr++ = EIR_TX_POWER;
260 *ptr++ = (uint8_t) tx_power;
261 eir_len += 3;
262 }
263
264 if (did_vendor != 0x0000) {
265 uint16_t source = 0x0002;
266 *ptr++ = 9;
267 *ptr++ = EIR_DEVICE_ID;
268 *ptr++ = (source & 0x00ff);
269 *ptr++ = (source & 0xff00) >> 8;
270 *ptr++ = (did_vendor & 0x00ff);
271 *ptr++ = (did_vendor & 0xff00) >> 8;
272 *ptr++ = (did_product & 0x00ff);
273 *ptr++ = (did_product & 0xff00) >> 8;
274 *ptr++ = (did_version & 0x00ff);
275 *ptr++ = (did_version & 0xff00) >> 8;
276 eir_len += 10;
277 }
278
279 /* Group all UUID16 types */
280 for (l = uuids; l != NULL; l = g_slist_next(l)) {
281 struct uuid_info *uuid = l->data;
282
283 if (uuid->uuid.type != SDP_UUID16)
284 continue;
285
286 if (uuid->uuid.value.uuid16 < 0x1100)
287 continue;
288
289 if (uuid->uuid.value.uuid16 == PNP_INFO_SVCLASS_ID)
290 continue;
291
292 /* Stop if not enough space to put next UUID16 */
293 if ((eir_len + 2 + sizeof(uint16_t)) > HCI_MAX_EIR_LENGTH) {
294 truncated = TRUE;
295 break;
296 }
297
298 /* Check for duplicates */
299 for (i = 0; i < uuid_count; i++)
300 if (uuid16[i] == uuid->uuid.value.uuid16)
301 break;
302
303 if (i < uuid_count)
304 continue;
305
306 uuid16[uuid_count++] = uuid->uuid.value.uuid16;
307 eir_len += sizeof(uint16_t);
308 }
309
310 if (uuid_count > 0) {
311 /* EIR Data length */
312 ptr[0] = (uuid_count * sizeof(uint16_t)) + 1;
313 /* EIR Data type */
314 ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL;
315
316 ptr += 2;
317 eir_len += 2;
318
319 for (i = 0; i < uuid_count; i++) {
320 *ptr++ = (uuid16[i] & 0x00ff);
321 *ptr++ = (uuid16[i] & 0xff00) >> 8;
322 }
323 }
324
325 /* Group all UUID128 types */
326 if (eir_len <= HCI_MAX_EIR_LENGTH - 2)
327 eir_generate_uuid128(uuids, ptr, &eir_len);
328 }
329