← Back to branch summary

~bluetooth/bluez/vivid-phone-overlay

« back to all changes in this revision

Viewing changes to src/eir.c

  • Committer: Simon Fels
  • Date: 2015-09-11 09:01:46 UTC
  • Revision ID: morphis@gravedo.de-20150911090146-4c0ln9s7ec3xf0nx
Import package bluez_4.101-0ubuntu25.1~overlay4 from stable phone overlay PPA

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/eir.c
 
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
 
 
25
#ifdef HAVE_CONFIG_H
 
26
#include <config.h>
 
27
#endif
 
28
 
 
29
#include <errno.h>
 
30
#include <stdlib.h>
 
31
#include <stdint.h>
 
32
#include <glib.h>
 
33
 
 
34
#include <bluetooth/bluetooth.h>
 
35
#include <bluetooth/hci.h>
 
36
#include <bluetooth/sdp.h>
 
37
 
 
38
#include "glib-helper.h"
 
39
#include "eir.h"
 
40
 
 
41
void eir_data_free(struct eir_data *eir)
 
42
{
 
43
        g_slist_free_full(eir->services, g_free);
 
44
        eir->services = NULL;
 
45
        g_free(eir->name);
 
46
        eir->name = NULL;
 
47
}
 
48
 
 
49
static void eir_parse_uuid16(struct eir_data *eir, void *data, uint8_t len)
 
50
{
 
51
        uint16_t *uuid16 = data;
 
52
        uuid_t service;
 
53
        char *uuid_str;
 
54
        unsigned int i;
 
55
 
 
56
        service.type = SDP_UUID16;
 
57
        for (i = 0; i < len / 2; i++, uuid16++) {
 
58
                service.value.uuid16 = btohs(bt_get_unaligned(uuid16));
 
59
                uuid_str = bt_uuid2string(&service);
 
60
                eir->services = g_slist_append(eir->services, uuid_str);
 
61
        }
 
62
}
 
63
 
 
64
static void eir_parse_uuid32(struct eir_data *eir, void *data, uint8_t len)
 
65
{
 
66
        uint32_t *uuid32 = data;
 
67
        uuid_t service;
 
68
        char *uuid_str;
 
69
        unsigned int i;
 
70
 
 
71
        service.type = SDP_UUID32;
 
72
        for (i = 0; i < len / 4; i++, uuid32++) {
 
73
                service.value.uuid32 = btohl(bt_get_unaligned(uuid32));
 
74
                uuid_str = bt_uuid2string(&service);
 
75
                eir->services = g_slist_append(eir->services, uuid_str);
 
76
        }
 
77
}
 
78
 
 
79
static void eir_parse_uuid128(struct eir_data *eir, uint8_t *data, uint8_t len)
 
80
{
 
81
        uint8_t *uuid_ptr = data;
 
82
        uuid_t service;
 
83
        char *uuid_str;
 
84
        unsigned int i;
 
85
        int k;
 
86
 
 
87
        service.type = SDP_UUID128;
 
88
        for (i = 0; i < len / 16; i++) {
 
89
                for (k = 0; k < 16; k++)
 
90
                        service.value.uuid128.data[k] = uuid_ptr[16 - k - 1];
 
91
                uuid_str = bt_uuid2string(&service);
 
92
                eir->services = g_slist_append(eir->services, uuid_str);
 
93
                uuid_ptr += 16;
 
94
        }
 
95
}
 
96
 
 
97
int eir_parse(struct eir_data *eir, uint8_t *eir_data, uint8_t eir_len)
 
98
{
 
99
        uint16_t len = 0;
 
100
 
 
101
        eir->flags = -1;
 
102
 
 
103
        /* No EIR data to parse */
 
104
        if (eir_data == NULL)
 
105
                return 0;
 
106
 
 
107
        while (len < eir_len - 1) {
 
108
                uint8_t field_len = eir_data[0];
 
109
                uint8_t data_len, *data = &eir_data[2];
 
110
 
 
111
                /* Check for the end of EIR */
 
112
                if (field_len == 0)
 
113
                        break;
 
114
 
 
115
                len += field_len + 1;
 
116
 
 
117
                /* Do not continue EIR Data parsing if got incorrect length */
 
118
                if (len > eir_len)
 
119
                        break;
 
120
 
 
121
                data_len = field_len - 1;
 
122
 
 
123
                switch (eir_data[1]) {
 
124
                case EIR_UUID16_SOME:
 
125
                case EIR_UUID16_ALL:
 
126
                        eir_parse_uuid16(eir, data, data_len);
 
127
                        break;
 
128
 
 
129
                case EIR_UUID32_SOME:
 
130
                case EIR_UUID32_ALL:
 
131
                        eir_parse_uuid32(eir, data, data_len);
 
132
                        break;
 
133
 
 
134
                case EIR_UUID128_SOME:
 
135
                case EIR_UUID128_ALL:
 
136
                        eir_parse_uuid128(eir, data, data_len);
 
137
                        break;
 
138
 
 
139
                case EIR_FLAGS:
 
140
                        if (data_len > 0)
 
141
                                eir->flags = *data;
 
142
                        break;
 
143
 
 
144
                case EIR_NAME_SHORT:
 
145
                case EIR_NAME_COMPLETE:
 
146
                        /* Some vendors put a NUL byte terminator into
 
147
                         * the name */
 
148
                        while (data_len > 0 && data[data_len - 1] == '\0')
 
149
                                data_len--;
 
150
 
 
151
                        if (!g_utf8_validate((char *) data, data_len, NULL))
 
152
                                break;
 
153
 
 
154
                        g_free(eir->name);
 
155
 
 
156
                        eir->name = g_strndup((char *) data, data_len);
 
157
                        eir->name_complete = eir_data[1] == EIR_NAME_COMPLETE;
 
158
                        break;
 
159
 
 
160
                case EIR_CLASS_OF_DEV:
 
161
                        if (data_len < 3)
 
162
                                break;
 
163
                        memcpy(eir->dev_class, data, 3);
 
164
                        break;
 
165
 
 
166
                case EIR_GAP_APPEARANCE:
 
167
                        if (data_len < 2)
 
168
                                break;
 
169
                        eir->appearance = bt_get_le16(data);
 
170
                        break;
 
171
                }
 
172
 
 
173
                eir_data += field_len + 1;
 
174
        }
 
175
 
 
176
        return 0;
 
177
}
 
178
 
 
179
#define SIZEOF_UUID128 16
 
180
 
 
181
static void eir_generate_uuid128(GSList *list, uint8_t *ptr, uint16_t *eir_len)
 
182
{
 
183
        int i, k, uuid_count = 0;
 
184
        uint16_t len = *eir_len;
 
185
        uint8_t *uuid128;
 
186
        gboolean truncated = FALSE;
 
187
 
 
188
        /* Store UUIDs in place, skip 2 bytes to write type and length later */
 
189
        uuid128 = ptr + 2;
 
190
 
 
191
        for (; list; list = list->next) {
 
192
                struct uuid_info *uuid = list->data;
 
193
                uint8_t *uuid128_data = uuid->uuid.value.uuid128.data;
 
194
 
 
195
                if (uuid->uuid.type != SDP_UUID128)
 
196
                        continue;
 
197
 
 
198
                /* Stop if not enough space to put next UUID128 */
 
199
                if ((len + 2 + SIZEOF_UUID128) > HCI_MAX_EIR_LENGTH) {
 
200
                        truncated = TRUE;
 
201
                        break;
 
202
                }
 
203
 
 
204
                /* Check for duplicates, EIR data is Little Endian */
 
205
                for (i = 0; i < uuid_count; i++) {
 
206
                        for (k = 0; k < SIZEOF_UUID128; k++) {
 
207
                                if (uuid128[i * SIZEOF_UUID128 + k] !=
 
208
                                        uuid128_data[SIZEOF_UUID128 - 1 - k])
 
209
                                        break;
 
210
                        }
 
211
                        if (k == SIZEOF_UUID128)
 
212
                                break;
 
213
                }
 
214
 
 
215
                if (i < uuid_count)
 
216
                        continue;
 
217
 
 
218
                /* EIR data is Little Endian */
 
219
                for (k = 0; k < SIZEOF_UUID128; k++)
 
220
                        uuid128[uuid_count * SIZEOF_UUID128 + k] =
 
221
                                uuid128_data[SIZEOF_UUID128 - 1 - k];
 
222
 
 
223
                len += SIZEOF_UUID128;
 
224
                uuid_count++;
 
225
        }
 
226
 
 
227
        if (uuid_count > 0 || truncated) {
 
228
                /* EIR Data length */
 
229
                ptr[0] = (uuid_count * SIZEOF_UUID128) + 1;
 
230
                /* EIR Data type */
 
231
                ptr[1] = truncated ? EIR_UUID128_SOME : EIR_UUID128_ALL;
 
232
                len += 2;
 
233
                *eir_len = len;
 
234
        }
 
235
}
 
236
 
 
237
void eir_create(const char *name, int8_t tx_power, uint16_t did_vendor,
 
238
                        uint16_t did_product, uint16_t did_version,
 
239
                        uint16_t did_source, GSList *uuids, uint8_t *data)
 
240
{
 
241
        GSList *l;
 
242
        uint8_t *ptr = data;
 
243
        uint16_t eir_len = 0;
 
244
        uint16_t uuid16[HCI_MAX_EIR_LENGTH / 2];
 
245
        int i, uuid_count = 0;
 
246
        gboolean truncated = FALSE;
 
247
        size_t name_len;
 
248
 
 
249
        name_len = strlen(name);
 
250
 
 
251
        if (name_len > 0) {
 
252
                /* EIR Data type */
 
253
                if (name_len > 48) {
 
254
                        name_len = 48;
 
255
                        ptr[1] = EIR_NAME_SHORT;
 
256
                } else
 
257
                        ptr[1] = EIR_NAME_COMPLETE;
 
258
 
 
259
                /* EIR Data length */
 
260
                ptr[0] = name_len + 1;
 
261
 
 
262
                memcpy(ptr + 2, name, name_len);
 
263
 
 
264
                eir_len += (name_len + 2);
 
265
                ptr += (name_len + 2);
 
266
        }
 
267
 
 
268
        if (tx_power != 0) {
 
269
                *ptr++ = 2;
 
270
                *ptr++ = EIR_TX_POWER;
 
271
                *ptr++ = (uint8_t) tx_power;
 
272
                eir_len += 3;
 
273
        }
 
274
 
 
275
        if (did_vendor != 0x0000) {
 
276
                *ptr++ = 9;
 
277
                *ptr++ = EIR_DEVICE_ID;
 
278
                *ptr++ = (did_source & 0x00ff);
 
279
                *ptr++ = (did_source & 0xff00) >> 8;
 
280
                *ptr++ = (did_vendor & 0x00ff);
 
281
                *ptr++ = (did_vendor & 0xff00) >> 8;
 
282
                *ptr++ = (did_product & 0x00ff);
 
283
                *ptr++ = (did_product & 0xff00) >> 8;
 
284
                *ptr++ = (did_version & 0x00ff);
 
285
                *ptr++ = (did_version & 0xff00) >> 8;
 
286
                eir_len += 10;
 
287
        }
 
288
 
 
289
        /* Group all UUID16 types */
 
290
        for (l = uuids; l != NULL; l = g_slist_next(l)) {
 
291
                struct uuid_info *uuid = l->data;
 
292
 
 
293
                if (uuid->uuid.type != SDP_UUID16)
 
294
                        continue;
 
295
 
 
296
                if (uuid->uuid.value.uuid16 < 0x1100)
 
297
                        continue;
 
298
 
 
299
                if (uuid->uuid.value.uuid16 == PNP_INFO_SVCLASS_ID)
 
300
                        continue;
 
301
 
 
302
                /* Stop if not enough space to put next UUID16 */
 
303
                if ((eir_len + 2 + sizeof(uint16_t)) > HCI_MAX_EIR_LENGTH) {
 
304
                        truncated = TRUE;
 
305
                        break;
 
306
                }
 
307
 
 
308
                /* Check for duplicates */
 
309
                for (i = 0; i < uuid_count; i++)
 
310
                        if (uuid16[i] == uuid->uuid.value.uuid16)
 
311
                                break;
 
312
 
 
313
                if (i < uuid_count)
 
314
                        continue;
 
315
 
 
316
                uuid16[uuid_count++] = uuid->uuid.value.uuid16;
 
317
                eir_len += sizeof(uint16_t);
 
318
        }
 
319
 
 
320
        if (uuid_count > 0) {
 
321
                /* EIR Data length */
 
322
                ptr[0] = (uuid_count * sizeof(uint16_t)) + 1;
 
323
                /* EIR Data type */
 
324
                ptr[1] = truncated ? EIR_UUID16_SOME : EIR_UUID16_ALL;
 
325
 
 
326
                ptr += 2;
 
327
                eir_len += 2;
 
328
 
 
329
                for (i = 0; i < uuid_count; i++) {
 
330
                        *ptr++ = (uuid16[i] & 0x00ff);
 
331
                        *ptr++ = (uuid16[i] & 0xff00) >> 8;
 
332
                }
 
333
        }
 
334
 
 
335
        /* Group all UUID128 types */
 
336
        if (eir_len <= HCI_MAX_EIR_LENGTH - 2)
 
337
                eir_generate_uuid128(uuids, ptr, &eir_len);
 
338
}
 
339
 
 
340
gboolean eir_has_data_type(uint8_t *data, size_t len, uint8_t type)
 
341
{
 
342
        uint8_t field_len;
 
343
        size_t parsed = 0;
 
344
 
 
345
        while (parsed < len - 1) {
 
346
                field_len = data[0];
 
347
 
 
348
                if (field_len == 0)
 
349
                        break;
 
350
 
 
351
                parsed += field_len + 1;
 
352
 
 
353
                if (parsed > len)
 
354
                        break;
 
355
 
 
356
                if (data[1] == type)
 
357
                        return TRUE;
 
358
 
 
359
                data += field_len + 1;
 
360
        }
 
361
 
 
362
        return FALSE;
 
363
}
 
364
 
 
365
size_t eir_append_data(uint8_t *eir, size_t eir_len, uint8_t type,
 
366
                                                uint8_t *data, size_t data_len)
 
367
{
 
368
        eir[eir_len++] = sizeof(type) + data_len;
 
369
        eir[eir_len++] = type;
 
370
        memcpy(&eir[eir_len], data, data_len);
 
371
        eir_len += data_len;
 
372
 
 
373
        return eir_len;
 
374
}
 
375
 
 
376
size_t eir_length(uint8_t *eir, size_t maxlen)
 
377
{
 
378
        uint8_t field_len;
 
379
        size_t parsed = 0, length = 0;
 
380
 
 
381
        while (parsed < maxlen - 1) {
 
382
                field_len = eir[0];
 
383
 
 
384
                if (field_len == 0)
 
385
                        break;
 
386
 
 
387
                parsed += field_len + 1;
 
388
 
 
389
                if (parsed > maxlen)
 
390
                        break;
 
391
 
 
392
                length = parsed;
 
393
                eir += field_len + 1;
 
394
        }
 
395
 
 
396
        return length;
 
397
}