← Back to branch summary

~ubuntu-branches/ubuntu/wily/qemu-kvm-spice/wily

« back to all changes in this revision

Viewing changes to hw/bt-sdp.c

  • Committer: Bazaar Package Importer
  • Author(s): Serge Hallyn
  • Date: 2011-10-19 10:44:56 UTC
  • Revision ID: james.westby@ubuntu.com-20111019104456-xgvskumk3sxi97f4
Tags: upstream-0.15.0+noroms
Import upstream version 0.15.0+noroms

Show diffs side-by-side

added added

removed removed

Lines of Context:
hw/bt-sdp.c
 
1
/*
 
2
 * Service Discover Protocol server for QEMU L2CAP devices
 
3
 *
 
4
 * Copyright (C) 2008 Andrzej Zaborowski  <balrog@zabor.org>
 
5
 *
 
6
 * This program is free software; you can redistribute it and/or
 
7
 * modify it under the terms of the GNU General Public License as
 
8
 * published by the Free Software Foundation; either version 2 of
 
9
 * the License, or (at your option) any later version.
 
10
 *
 
11
 * This program is distributed in the hope that it will be useful,
 
12
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 
13
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 
14
 * GNU General Public License for more details.
 
15
 *
 
16
 * You should have received a copy of the GNU General Public License along
 
17
 * with this program; if not, see <http://www.gnu.org/licenses/>.
 
18
 */
 
19
 
 
20
#include "qemu-common.h"
 
21
#include "bt.h"
 
22
 
 
23
struct bt_l2cap_sdp_state_s {
 
24
    struct bt_l2cap_conn_params_s *channel;
 
25
 
 
26
    struct sdp_service_record_s {
 
27
        int match;
 
28
 
 
29
        int *uuid;
 
30
        int uuids;
 
31
        struct sdp_service_attribute_s {
 
32
            int match;
 
33
 
 
34
            int attribute_id;
 
35
            int len;
 
36
            void *pair;
 
37
        } *attribute_list;
 
38
        int attributes;
 
39
    } *service_list;
 
40
    int services;
 
41
};
 
42
 
 
43
static ssize_t sdp_datalen(const uint8_t **element, ssize_t *left)
 
44
{
 
45
    size_t len = *(*element) ++ & SDP_DSIZE_MASK;
 
46
 
 
47
    if (!*left)
 
48
        return -1;
 
49
    (*left) --;
 
50
 
 
51
    if (len < SDP_DSIZE_NEXT1)
 
52
        return 1 << len;
 
53
    else if (len == SDP_DSIZE_NEXT1) {
 
54
        if (*left < 1)
 
55
            return -1;
 
56
        (*left) --;
 
57
 
 
58
        return *(*element) ++;
 
59
    } else if (len == SDP_DSIZE_NEXT2) {
 
60
        if (*left < 2)
 
61
            return -1;
 
62
        (*left) -= 2;
 
63
 
 
64
        len = (*(*element) ++) << 8;
 
65
        return len | (*(*element) ++);
 
66
    } else {
 
67
        if (*left < 4)
 
68
            return -1;
 
69
        (*left) -= 4;
 
70
 
 
71
        len = (*(*element) ++) << 24;
 
72
        len |= (*(*element) ++) << 16;
 
73
        len |= (*(*element) ++) << 8;
 
74
        return len | (*(*element) ++);
 
75
    }
 
76
}
 
77
 
 
78
static const uint8_t bt_base_uuid[12] = {
 
79
    0x00, 0x00, 0x10, 0x00, 0x80, 0x00, 0x00, 0x80, 0x5f, 0x9b, 0x34, 0xfb,
 
80
};
 
81
 
 
82
static int sdp_uuid_match(struct sdp_service_record_s *record,
 
83
                const uint8_t *uuid, ssize_t datalen)
 
84
{
 
85
    int *lo, hi, val;
 
86
 
 
87
    if (datalen == 16 || datalen == 4) {
 
88
        if (datalen == 16 && memcmp(uuid + 4, bt_base_uuid, 12))
 
89
            return 0;
 
90
 
 
91
        if (uuid[0] | uuid[1])
 
92
            return 0;
 
93
        uuid += 2;
 
94
    }
 
95
 
 
96
    val = (uuid[0] << 8) | uuid[1];
 
97
    lo = record->uuid;
 
98
    hi = record->uuids;
 
99
    while (hi >>= 1)
 
100
        if (lo[hi] <= val)
 
101
            lo += hi;
 
102
 
 
103
    return *lo == val;
 
104
}
 
105
 
 
106
#define CONTINUATION_PARAM_SIZE (1 + sizeof(int))
 
107
#define MAX_PDU_OUT_SIZE        96      /* Arbitrary */
 
108
#define PDU_HEADER_SIZE         5
 
109
#define MAX_RSP_PARAM_SIZE      (MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE - \
 
110
                CONTINUATION_PARAM_SIZE)
 
111
 
 
112
static int sdp_svc_match(struct bt_l2cap_sdp_state_s *sdp,
 
113
                const uint8_t **req, ssize_t *len)
 
114
{
 
115
    size_t datalen;
 
116
    int i;
 
117
 
 
118
    if ((**req & ~SDP_DSIZE_MASK) != SDP_DTYPE_UUID)
 
119
        return 1;
 
120
 
 
121
    datalen = sdp_datalen(req, len);
 
122
    if (datalen != 2 && datalen != 4 && datalen != 16)
 
123
        return 1;
 
124
 
 
125
    for (i = 0; i < sdp->services; i ++)
 
126
        if (sdp_uuid_match(&sdp->service_list[i], *req, datalen))
 
127
            sdp->service_list[i].match = 1;
 
128
 
 
129
    (*req) += datalen;
 
130
    (*len) -= datalen;
 
131
 
 
132
    return 0;
 
133
}
 
134
 
 
135
static ssize_t sdp_svc_search(struct bt_l2cap_sdp_state_s *sdp,
 
136
                uint8_t *rsp, const uint8_t *req, ssize_t len)
 
137
{
 
138
    ssize_t seqlen;
 
139
    int i, count, start, end, max;
 
140
    int32_t handle;
 
141
 
 
142
    /* Perform the search */
 
143
    for (i = 0; i < sdp->services; i ++)
 
144
        sdp->service_list[i].match = 0;
 
145
 
 
146
    if (len < 1)
 
147
        return -SDP_INVALID_SYNTAX;
 
148
    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 
149
        seqlen = sdp_datalen(&req, &len);
 
150
        if (seqlen < 3 || len < seqlen)
 
151
            return -SDP_INVALID_SYNTAX;
 
152
        len -= seqlen;
 
153
 
 
154
        while (seqlen)
 
155
            if (sdp_svc_match(sdp, &req, &seqlen))
 
156
                return -SDP_INVALID_SYNTAX;
 
157
    } else if (sdp_svc_match(sdp, &req, &seqlen))
 
158
        return -SDP_INVALID_SYNTAX;
 
159
 
 
160
    if (len < 3)
 
161
        return -SDP_INVALID_SYNTAX;
 
162
    max = (req[0] << 8) | req[1];
 
163
    req += 2;
 
164
    len -= 2;
 
165
 
 
166
    if (*req) {
 
167
        if (len <= sizeof(int))
 
168
            return -SDP_INVALID_SYNTAX;
 
169
        len -= sizeof(int);
 
170
        memcpy(&start, req + 1, sizeof(int));
 
171
    } else
 
172
        start = 0;
 
173
 
 
174
    if (len > 1)
 
175
        return -SDP_INVALID_SYNTAX;
 
176
 
 
177
    /* Output the results */
 
178
    len = 4;
 
179
    count = 0;
 
180
    end = start;
 
181
    for (i = 0; i < sdp->services; i ++)
 
182
        if (sdp->service_list[i].match) {
 
183
            if (count >= start && count < max && len + 4 < MAX_RSP_PARAM_SIZE) {
 
184
                handle = i;
 
185
                memcpy(rsp + len, &handle, 4);
 
186
                len += 4;
 
187
                end = count + 1;
 
188
            }
 
189
 
 
190
            count ++;
 
191
        }
 
192
 
 
193
    rsp[0] = count >> 8;
 
194
    rsp[1] = count & 0xff;
 
195
    rsp[2] = (end - start) >> 8;
 
196
    rsp[3] = (end - start) & 0xff;
 
197
 
 
198
    if (end < count) {
 
199
        rsp[len ++] = sizeof(int);
 
200
        memcpy(rsp + len, &end, sizeof(int));
 
201
        len += 4;
 
202
    } else
 
203
        rsp[len ++] = 0;
 
204
 
 
205
    return len;
 
206
}
 
207
 
 
208
static int sdp_attr_match(struct sdp_service_record_s *record,
 
209
                const uint8_t **req, ssize_t *len)
 
210
{
 
211
    int i, start, end;
 
212
 
 
213
    if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
 
214
        (*req) ++;
 
215
        if (*len < 3)
 
216
            return 1;
 
217
 
 
218
        start = (*(*req) ++) << 8;
 
219
        start |= *(*req) ++;
 
220
        end = start;
 
221
        *len -= 3;
 
222
    } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
 
223
        (*req) ++;
 
224
        if (*len < 5)
 
225
            return 1;
 
226
 
 
227
        start = (*(*req) ++) << 8;
 
228
        start |= *(*req) ++;
 
229
        end = (*(*req) ++) << 8;
 
230
        end |= *(*req) ++;
 
231
        *len -= 5;
 
232
    } else
 
233
        return 1;
 
234
 
 
235
    for (i = 0; i < record->attributes; i ++)
 
236
        if (record->attribute_list[i].attribute_id >= start &&
 
237
                        record->attribute_list[i].attribute_id <= end)
 
238
            record->attribute_list[i].match = 1;
 
239
 
 
240
    return 0;
 
241
}
 
242
 
 
243
static ssize_t sdp_attr_get(struct bt_l2cap_sdp_state_s *sdp,
 
244
                uint8_t *rsp, const uint8_t *req, ssize_t len)
 
245
{
 
246
    ssize_t seqlen;
 
247
    int i, start, end, max;
 
248
    int32_t handle;
 
249
    struct sdp_service_record_s *record;
 
250
    uint8_t *lst;
 
251
 
 
252
    /* Perform the search */
 
253
    if (len < 7)
 
254
        return -SDP_INVALID_SYNTAX;
 
255
    memcpy(&handle, req, 4);
 
256
    req += 4;
 
257
    len -= 4;
 
258
 
 
259
    if (handle < 0 || handle > sdp->services)
 
260
        return -SDP_INVALID_RECORD_HANDLE;
 
261
    record = &sdp->service_list[handle];
 
262
 
 
263
    for (i = 0; i < record->attributes; i ++)
 
264
        record->attribute_list[i].match = 0;
 
265
 
 
266
    max = (req[0] << 8) | req[1];
 
267
    req += 2;
 
268
    len -= 2;
 
269
    if (max < 0x0007)
 
270
        return -SDP_INVALID_SYNTAX;
 
271
 
 
272
    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 
273
        seqlen = sdp_datalen(&req, &len);
 
274
        if (seqlen < 3 || len < seqlen)
 
275
            return -SDP_INVALID_SYNTAX;
 
276
        len -= seqlen;
 
277
 
 
278
        while (seqlen)
 
279
            if (sdp_attr_match(record, &req, &seqlen))
 
280
                return -SDP_INVALID_SYNTAX;
 
281
    } else if (sdp_attr_match(record, &req, &seqlen))
 
282
        return -SDP_INVALID_SYNTAX;
 
283
 
 
284
    if (len < 1)
 
285
        return -SDP_INVALID_SYNTAX;
 
286
 
 
287
    if (*req) {
 
288
        if (len <= sizeof(int))
 
289
            return -SDP_INVALID_SYNTAX;
 
290
        len -= sizeof(int);
 
291
        memcpy(&start, req + 1, sizeof(int));
 
292
    } else
 
293
        start = 0;
 
294
 
 
295
    if (len > 1)
 
296
        return -SDP_INVALID_SYNTAX;
 
297
 
 
298
    /* Output the results */
 
299
    lst = rsp + 2;
 
300
    max = MIN(max, MAX_RSP_PARAM_SIZE);
 
301
    len = 3 - start;
 
302
    end = 0;
 
303
    for (i = 0; i < record->attributes; i ++)
 
304
        if (record->attribute_list[i].match) {
 
305
            if (len >= 0 && len + record->attribute_list[i].len < max) {
 
306
                memcpy(lst + len, record->attribute_list[i].pair,
 
307
                                record->attribute_list[i].len);
 
308
                end = len + record->attribute_list[i].len;
 
309
            }
 
310
            len += record->attribute_list[i].len;
 
311
        }
 
312
    if (0 >= start) {
 
313
       lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
 
314
       lst[1] = (len + start - 3) >> 8;
 
315
       lst[2] = (len + start - 3) & 0xff;
 
316
    }
 
317
 
 
318
    rsp[0] = end >> 8;
 
319
    rsp[1] = end & 0xff;
 
320
 
 
321
    if (end < len) {
 
322
        len = end + start;
 
323
        lst[end ++] = sizeof(int);
 
324
        memcpy(lst + end, &len, sizeof(int));
 
325
        end += sizeof(int);
 
326
    } else
 
327
        lst[end ++] = 0;
 
328
 
 
329
    return end + 2;
 
330
}
 
331
 
 
332
static int sdp_svc_attr_match(struct bt_l2cap_sdp_state_s *sdp,
 
333
                const uint8_t **req, ssize_t *len)
 
334
{
 
335
    int i, j, start, end;
 
336
    struct sdp_service_record_s *record;
 
337
 
 
338
    if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_2)) {
 
339
        (*req) ++;
 
340
        if (*len < 3)
 
341
            return 1;
 
342
 
 
343
        start = (*(*req) ++) << 8;
 
344
        start |= *(*req) ++;
 
345
        end = start;
 
346
        *len -= 3;
 
347
    } else if (**req == (SDP_DTYPE_UINT | SDP_DSIZE_4)) {
 
348
        (*req) ++;
 
349
        if (*len < 5)
 
350
            return 1;
 
351
 
 
352
        start = (*(*req) ++) << 8;
 
353
        start |= *(*req) ++;
 
354
        end = (*(*req) ++) << 8;
 
355
        end |= *(*req) ++;
 
356
        *len -= 5;
 
357
    } else
 
358
        return 1;
 
359
 
 
360
    for (i = 0; i < sdp->services; i ++)
 
361
        if ((record = &sdp->service_list[i])->match)
 
362
            for (j = 0; j < record->attributes; j ++)
 
363
                if (record->attribute_list[j].attribute_id >= start &&
 
364
                                record->attribute_list[j].attribute_id <= end)
 
365
                    record->attribute_list[j].match = 1;
 
366
 
 
367
    return 0;
 
368
}
 
369
 
 
370
static ssize_t sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s *sdp,
 
371
                uint8_t *rsp, const uint8_t *req, ssize_t len)
 
372
{
 
373
    ssize_t seqlen;
 
374
    int i, j, start, end, max;
 
375
    struct sdp_service_record_s *record;
 
376
    uint8_t *lst;
 
377
 
 
378
    /* Perform the search */
 
379
    for (i = 0; i < sdp->services; i ++) {
 
380
        sdp->service_list[i].match = 0;
 
381
            for (j = 0; j < sdp->service_list[i].attributes; j ++)
 
382
                sdp->service_list[i].attribute_list[j].match = 0;
 
383
    }
 
384
 
 
385
    if (len < 1)
 
386
        return -SDP_INVALID_SYNTAX;
 
387
    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 
388
        seqlen = sdp_datalen(&req, &len);
 
389
        if (seqlen < 3 || len < seqlen)
 
390
            return -SDP_INVALID_SYNTAX;
 
391
        len -= seqlen;
 
392
 
 
393
        while (seqlen)
 
394
            if (sdp_svc_match(sdp, &req, &seqlen))
 
395
                return -SDP_INVALID_SYNTAX;
 
396
    } else if (sdp_svc_match(sdp, &req, &seqlen))
 
397
        return -SDP_INVALID_SYNTAX;
 
398
 
 
399
    if (len < 3)
 
400
        return -SDP_INVALID_SYNTAX;
 
401
    max = (req[0] << 8) | req[1];
 
402
    req += 2;
 
403
    len -= 2;
 
404
    if (max < 0x0007)
 
405
        return -SDP_INVALID_SYNTAX;
 
406
 
 
407
    if ((*req & ~SDP_DSIZE_MASK) == SDP_DTYPE_SEQ) {
 
408
        seqlen = sdp_datalen(&req, &len);
 
409
        if (seqlen < 3 || len < seqlen)
 
410
            return -SDP_INVALID_SYNTAX;
 
411
        len -= seqlen;
 
412
 
 
413
        while (seqlen)
 
414
            if (sdp_svc_attr_match(sdp, &req, &seqlen))
 
415
                return -SDP_INVALID_SYNTAX;
 
416
    } else if (sdp_svc_attr_match(sdp, &req, &seqlen))
 
417
        return -SDP_INVALID_SYNTAX;
 
418
 
 
419
    if (len < 1)
 
420
        return -SDP_INVALID_SYNTAX;
 
421
 
 
422
    if (*req) {
 
423
        if (len <= sizeof(int))
 
424
            return -SDP_INVALID_SYNTAX;
 
425
        len -= sizeof(int);
 
426
        memcpy(&start, req + 1, sizeof(int));
 
427
    } else
 
428
        start = 0;
 
429
 
 
430
    if (len > 1)
 
431
        return -SDP_INVALID_SYNTAX;
 
432
 
 
433
    /* Output the results */
 
434
    /* This assumes empty attribute lists are never to be returned even
 
435
     * for matching Service Records.  In practice this shouldn't happen
 
436
     * as the requestor will usually include the always present
 
437
     * ServiceRecordHandle AttributeID in AttributeIDList.  */
 
438
    lst = rsp + 2;
 
439
    max = MIN(max, MAX_RSP_PARAM_SIZE);
 
440
    len = 3 - start;
 
441
    end = 0;
 
442
    for (i = 0; i < sdp->services; i ++)
 
443
        if ((record = &sdp->service_list[i])->match) {
 
444
            len += 3;
 
445
            seqlen = len;
 
446
            for (j = 0; j < record->attributes; j ++)
 
447
                if (record->attribute_list[j].match) {
 
448
                    if (len >= 0)
 
449
                        if (len + record->attribute_list[j].len < max) {
 
450
                            memcpy(lst + len, record->attribute_list[j].pair,
 
451
                                            record->attribute_list[j].len);
 
452
                            end = len + record->attribute_list[j].len;
 
453
                        }
 
454
                    len += record->attribute_list[j].len;
 
455
                }
 
456
            if (seqlen == len)
 
457
                len -= 3;
 
458
            else if (seqlen >= 3 && seqlen < max) {
 
459
                lst[seqlen - 3] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
 
460
                lst[seqlen - 2] = (len - seqlen) >> 8;
 
461
                lst[seqlen - 1] = (len - seqlen) & 0xff;
 
462
            }
 
463
        }
 
464
    if (len == 3 - start)
 
465
        len -= 3;
 
466
    else if (0 >= start) {
 
467
       lst[0] = SDP_DTYPE_SEQ | SDP_DSIZE_NEXT2;
 
468
       lst[1] = (len + start - 3) >> 8;
 
469
       lst[2] = (len + start - 3) & 0xff;
 
470
    }
 
471
 
 
472
    rsp[0] = end >> 8;
 
473
    rsp[1] = end & 0xff;
 
474
 
 
475
    if (end < len) {
 
476
        len = end + start;
 
477
        lst[end ++] = sizeof(int);
 
478
        memcpy(lst + end, &len, sizeof(int));
 
479
        end += sizeof(int);
 
480
    } else
 
481
        lst[end ++] = 0;
 
482
 
 
483
    return end + 2;
 
484
}
 
485
 
 
486
static void bt_l2cap_sdp_sdu_in(void *opaque, const uint8_t *data, int len)
 
487
{
 
488
    struct bt_l2cap_sdp_state_s *sdp = opaque;
 
489
    enum bt_sdp_cmd pdu_id;
 
490
    uint8_t rsp[MAX_PDU_OUT_SIZE - PDU_HEADER_SIZE], *sdu_out;
 
491
    int transaction_id, plen;
 
492
    int err = 0;
 
493
    int rsp_len = 0;
 
494
 
 
495
    if (len < 5) {
 
496
        fprintf(stderr, "%s: short SDP PDU (%iB).\n", __FUNCTION__, len);
 
497
        return;
 
498
    }
 
499
 
 
500
    pdu_id = *data ++;
 
501
    transaction_id = (data[0] << 8) | data[1];
 
502
    plen = (data[2] << 8) | data[3];
 
503
    data += 4;
 
504
    len -= 5;
 
505
 
 
506
    if (len != plen) {
 
507
        fprintf(stderr, "%s: wrong SDP PDU length (%iB != %iB).\n",
 
508
                        __FUNCTION__, plen, len);
 
509
        err = SDP_INVALID_PDU_SIZE;
 
510
        goto respond;
 
511
    }
 
512
 
 
513
    switch (pdu_id) {
 
514
    case SDP_SVC_SEARCH_REQ:
 
515
        rsp_len = sdp_svc_search(sdp, rsp, data, len);
 
516
        pdu_id = SDP_SVC_SEARCH_RSP;
 
517
        break;
 
518
 
 
519
    case SDP_SVC_ATTR_REQ:
 
520
        rsp_len = sdp_attr_get(sdp, rsp, data, len);
 
521
        pdu_id = SDP_SVC_ATTR_RSP;
 
522
        break;
 
523
 
 
524
    case SDP_SVC_SEARCH_ATTR_REQ:
 
525
        rsp_len = sdp_svc_search_attr_get(sdp, rsp, data, len);
 
526
        pdu_id = SDP_SVC_SEARCH_ATTR_RSP;
 
527
        break;
 
528
 
 
529
    case SDP_ERROR_RSP:
 
530
    case SDP_SVC_ATTR_RSP:
 
531
    case SDP_SVC_SEARCH_RSP:
 
532
    case SDP_SVC_SEARCH_ATTR_RSP:
 
533
    default:
 
534
        fprintf(stderr, "%s: unexpected SDP PDU ID %02x.\n",
 
535
                        __FUNCTION__, pdu_id);
 
536
        err = SDP_INVALID_SYNTAX;
 
537
        break;
 
538
    }
 
539
 
 
540
    if (rsp_len < 0) {
 
541
        err = -rsp_len;
 
542
        rsp_len = 0;
 
543
    }
 
544
 
 
545
respond:
 
546
    if (err) {
 
547
        pdu_id = SDP_ERROR_RSP;
 
548
        rsp[rsp_len ++] = err >> 8;
 
549
        rsp[rsp_len ++] = err & 0xff;
 
550
    }
 
551
 
 
552
    sdu_out = sdp->channel->sdu_out(sdp->channel, rsp_len + PDU_HEADER_SIZE);
 
553
 
 
554
    sdu_out[0] = pdu_id;
 
555
    sdu_out[1] = transaction_id >> 8;
 
556
    sdu_out[2] = transaction_id & 0xff;
 
557
    sdu_out[3] = rsp_len >> 8;
 
558
    sdu_out[4] = rsp_len & 0xff;
 
559
    memcpy(sdu_out + PDU_HEADER_SIZE, rsp, rsp_len);
 
560
 
 
561
    sdp->channel->sdu_submit(sdp->channel);
 
562
}
 
563
 
 
564
static void bt_l2cap_sdp_close_ch(void *opaque)
 
565
{
 
566
    struct bt_l2cap_sdp_state_s *sdp = opaque;
 
567
    int i;
 
568
 
 
569
    for (i = 0; i < sdp->services; i ++) {
 
570
        qemu_free(sdp->service_list[i].attribute_list->pair);
 
571
        qemu_free(sdp->service_list[i].attribute_list);
 
572
        qemu_free(sdp->service_list[i].uuid);
 
573
    }
 
574
    qemu_free(sdp->service_list);
 
575
    qemu_free(sdp);
 
576
}
 
577
 
 
578
struct sdp_def_service_s {
 
579
    uint16_t class_uuid;
 
580
    struct sdp_def_attribute_s {
 
581
        uint16_t id;
 
582
        struct sdp_def_data_element_s {
 
583
            uint8_t type;
 
584
            union {
 
585
                uint32_t uint;
 
586
                const char *str;
 
587
                struct sdp_def_data_element_s *list;
 
588
            } value;
 
589
        } data;
 
590
    } attributes[];
 
591
};
 
592
 
 
593
/* Calculate a safe byte count to allocate that will store the given
 
594
 * element, at the same time count elements of a UUID type.  */
 
595
static int sdp_attr_max_size(struct sdp_def_data_element_s *element,
 
596
                int *uuids)
 
597
{
 
598
    int type = element->type & ~SDP_DSIZE_MASK;
 
599
    int len;
 
600
 
 
601
    if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_UUID ||
 
602
                    type == SDP_DTYPE_BOOL) {
 
603
        if (type == SDP_DTYPE_UUID)
 
604
            (*uuids) ++;
 
605
        return 1 + (1 << (element->type & SDP_DSIZE_MASK));
 
606
    }
 
607
 
 
608
    if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
 
609
        if (element->type & SDP_DSIZE_MASK) {
 
610
            for (len = 0; element->value.str[len] |
 
611
                            element->value.str[len + 1]; len ++);
 
612
            return len;
 
613
        } else
 
614
            return 2 + strlen(element->value.str);
 
615
    }
 
616
 
 
617
    if (type != SDP_DTYPE_SEQ)
 
618
        exit(-1);
 
619
    len = 2;
 
620
    element = element->value.list;
 
621
    while (element->type)
 
622
        len += sdp_attr_max_size(element ++, uuids);
 
623
    if (len > 255)
 
624
        exit (-1);
 
625
 
 
626
    return len;
 
627
}
 
628
 
 
629
static int sdp_attr_write(uint8_t *data,
 
630
                struct sdp_def_data_element_s *element, int **uuid)
 
631
{
 
632
    int type = element->type & ~SDP_DSIZE_MASK;
 
633
    int len = 0;
 
634
 
 
635
    if (type == SDP_DTYPE_UINT || type == SDP_DTYPE_BOOL) {
 
636
        data[len ++] = element->type;
 
637
        if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_1)
 
638
            data[len ++] = (element->value.uint >>  0) & 0xff;
 
639
        else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_2) {
 
640
            data[len ++] = (element->value.uint >>  8) & 0xff;
 
641
            data[len ++] = (element->value.uint >>  0) & 0xff;
 
642
        } else if ((element->type & SDP_DSIZE_MASK) == SDP_DSIZE_4) {
 
643
            data[len ++] = (element->value.uint >>  24) & 0xff;
 
644
            data[len ++] = (element->value.uint >>  16) & 0xff;
 
645
            data[len ++] = (element->value.uint >>  8) & 0xff;
 
646
            data[len ++] = (element->value.uint >>  0) & 0xff;
 
647
        }
 
648
 
 
649
        return len;
 
650
    }
 
651
 
 
652
    if (type == SDP_DTYPE_UUID) {
 
653
        *(*uuid) ++ = element->value.uint;
 
654
 
 
655
        data[len ++] = element->type;
 
656
        data[len ++] = (element->value.uint >>  24) & 0xff;
 
657
        data[len ++] = (element->value.uint >>  16) & 0xff;
 
658
        data[len ++] = (element->value.uint >>  8) & 0xff;
 
659
        data[len ++] = (element->value.uint >>  0) & 0xff;
 
660
        memcpy(data + len, bt_base_uuid, 12);
 
661
 
 
662
        return len + 12;
 
663
    }
 
664
 
 
665
    data[0] = type | SDP_DSIZE_NEXT1;
 
666
    if (type == SDP_DTYPE_STRING || type == SDP_DTYPE_URL) {
 
667
        if (element->type & SDP_DSIZE_MASK)
 
668
            for (len = 0; element->value.str[len] |
 
669
                            element->value.str[len + 1]; len ++);
 
670
        else
 
671
            len = strlen(element->value.str);
 
672
        memcpy(data + 2, element->value.str, data[1] = len);
 
673
 
 
674
        return len + 2;
 
675
    }
 
676
 
 
677
    len = 2;
 
678
    element = element->value.list;
 
679
    while (element->type)
 
680
        len += sdp_attr_write(data + len, element ++, uuid);
 
681
    data[1] = len - 2;
 
682
 
 
683
    return len;
 
684
}
 
685
 
 
686
static int sdp_attributeid_compare(const struct sdp_service_attribute_s *a,
 
687
                const struct sdp_service_attribute_s *b)
 
688
{
 
689
    return (int) b->attribute_id - a->attribute_id;
 
690
}
 
691
 
 
692
static int sdp_uuid_compare(const int *a, const int *b)
 
693
{
 
694
    return *a - *b;
 
695
}
 
696
 
 
697
static void sdp_service_record_build(struct sdp_service_record_s *record,
 
698
                struct sdp_def_service_s *def, int handle)
 
699
{
 
700
    int len = 0;
 
701
    uint8_t *data;
 
702
    int *uuid;
 
703
 
 
704
    record->uuids = 0;
 
705
    while (def->attributes[record->attributes].data.type) {
 
706
        len += 3;
 
707
        len += sdp_attr_max_size(&def->attributes[record->attributes ++].data,
 
708
                        &record->uuids);
 
709
    }
 
710
    record->uuids = 1 << ffs(record->uuids - 1);
 
711
    record->attribute_list =
 
712
            qemu_mallocz(record->attributes * sizeof(*record->attribute_list));
 
713
    record->uuid =
 
714
            qemu_mallocz(record->uuids * sizeof(*record->uuid));
 
715
    data = qemu_malloc(len);
 
716
 
 
717
    record->attributes = 0;
 
718
    uuid = record->uuid;
 
719
    while (def->attributes[record->attributes].data.type) {
 
720
        record->attribute_list[record->attributes].pair = data;
 
721
 
 
722
        len = 0;
 
723
        data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2;
 
724
        data[len ++] = def->attributes[record->attributes].id >> 8;
 
725
        data[len ++] = def->attributes[record->attributes].id & 0xff;
 
726
        len += sdp_attr_write(data + len,
 
727
                        &def->attributes[record->attributes].data, &uuid);
 
728
 
 
729
        /* Special case: assign a ServiceRecordHandle in sequence */
 
730
        if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE)
 
731
            def->attributes[record->attributes].data.value.uint = handle;
 
732
        /* Note: we could also assign a ServiceDescription based on
 
733
         * sdp->device.device->lmp_name.  */
 
734
 
 
735
        record->attribute_list[record->attributes ++].len = len;
 
736
        data += len;
 
737
    }
 
738
 
 
739
    /* Sort the attribute list by the AttributeID */
 
740
    qsort(record->attribute_list, record->attributes,
 
741
                    sizeof(*record->attribute_list),
 
742
                    (void *) sdp_attributeid_compare);
 
743
    /* Sort the searchable UUIDs list for bisection */
 
744
    qsort(record->uuid, record->uuids,
 
745
                    sizeof(*record->uuid),
 
746
                    (void *) sdp_uuid_compare);
 
747
}
 
748
 
 
749
static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp,
 
750
                struct sdp_def_service_s **service)
 
751
{
 
752
    sdp->services = 0;
 
753
    while (service[sdp->services])
 
754
        sdp->services ++;
 
755
    sdp->service_list =
 
756
            qemu_mallocz(sdp->services * sizeof(*sdp->service_list));
 
757
 
 
758
    sdp->services = 0;
 
759
    while (*service) {
 
760
        sdp_service_record_build(&sdp->service_list[sdp->services],
 
761
                        *service, sdp->services);
 
762
        service ++;
 
763
        sdp->services ++;
 
764
    }
 
765
}
 
766
 
 
767
#define LAST { .type = 0 }
 
768
#define SERVICE(name, attrs)                            \
 
769
    static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \
 
770
        .attributes = { attrs { .data = LAST } },       \
 
771
    };
 
772
#define ATTRIBUTE(attrid, val)  { .id = glue(SDP_ATTR_, attrid), .data = val },
 
773
#define UINT8(val)      {                               \
 
774
        .type       = SDP_DTYPE_UINT | SDP_DSIZE_1,     \
 
775
        .value.uint = val,                              \
 
776
    },
 
777
#define UINT16(val)     {                               \
 
778
        .type       = SDP_DTYPE_UINT | SDP_DSIZE_2,     \
 
779
        .value.uint = val,                              \
 
780
    },
 
781
#define UINT32(val)     {                               \
 
782
        .type       = SDP_DTYPE_UINT | SDP_DSIZE_4,     \
 
783
        .value.uint = val,                              \
 
784
    },
 
785
#define UUID128(val)    {                               \
 
786
        .type       = SDP_DTYPE_UUID | SDP_DSIZE_16,    \
 
787
        .value.uint = val,                              \
 
788
    },
 
789
#define SDP_TRUE        {                               \
 
790
        .type       = SDP_DTYPE_BOOL | SDP_DSIZE_1,     \
 
791
        .value.uint = 1,                                \
 
792
    },
 
793
#define SDP_FALSE       {                               \
 
794
        .type       = SDP_DTYPE_BOOL | SDP_DSIZE_1,     \
 
795
        .value.uint = 0,                                \
 
796
    },
 
797
#define STRING(val)     {                               \
 
798
        .type       = SDP_DTYPE_STRING,                 \
 
799
        .value.str  = val,                              \
 
800
    },
 
801
#define ARRAY(...)      {                               \
 
802
        .type       = SDP_DTYPE_STRING | SDP_DSIZE_2,   \
 
803
        .value.str  = (char []) { __VA_ARGS__, 0, 0 },  \
 
804
    },
 
805
#define URL(val)        {                               \
 
806
        .type       = SDP_DTYPE_URL,                    \
 
807
        .value.str  = val,                              \
 
808
    },
 
809
#if 1
 
810
#define LIST(val)       {                               \
 
811
        .type       = SDP_DTYPE_SEQ,                    \
 
812
        .value.list = (struct sdp_def_data_element_s []) { val LAST }, \
 
813
    },
 
814
#endif
 
815
 
 
816
/* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes
 
817
 * in resulting SDP data representation size.  */
 
818
 
 
819
SERVICE(hid,
 
820
    ATTRIBUTE(RECORD_HANDLE,   UINT32(0))       /* Filled in later */
 
821
    ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID)))
 
822
    ATTRIBUTE(RECORD_STATE,    UINT32(1))
 
823
    ATTRIBUTE(PROTO_DESC_LIST, LIST(
 
824
        LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL))
 
825
        LIST(UUID128(HIDP_UUID))
 
826
    ))
 
827
    ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
 
828
    ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
 
829
        UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
 
830
    ))
 
831
    ATTRIBUTE(PFILE_DESC_LIST, LIST(
 
832
        LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100))
 
833
    ))
 
834
    ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
 
835
    ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID"))
 
836
    ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse"))
 
837
    ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
 
838
 
 
839
    /* Profile specific */
 
840
    ATTRIBUTE(DEVICE_RELEASE_NUMBER,    UINT16(0x0091)) /* Deprecated, remove */
 
841
    ATTRIBUTE(PARSER_VERSION,           UINT16(0x0111))
 
842
    /* TODO: extract from l2cap_device->device.class[0] */
 
843
    ATTRIBUTE(DEVICE_SUBCLASS,          UINT8(0x40))
 
844
    ATTRIBUTE(COUNTRY_CODE,             UINT8(0x15))
 
845
    ATTRIBUTE(VIRTUAL_CABLE,            SDP_TRUE)
 
846
    ATTRIBUTE(RECONNECT_INITIATE,       SDP_FALSE)
 
847
    /* TODO: extract from hid->usbdev->report_desc */
 
848
    ATTRIBUTE(DESCRIPTOR_LIST,          LIST(
 
849
        LIST(UINT8(0x22) ARRAY(
 
850
            0x05, 0x01, /* Usage Page (Generic Desktop) */
 
851
            0x09, 0x06, /* Usage (Keyboard) */
 
852
            0xa1, 0x01, /* Collection (Application) */
 
853
            0x75, 0x01, /*   Report Size (1) */
 
854
            0x95, 0x08, /*   Report Count (8) */
 
855
            0x05, 0x07, /*   Usage Page (Key Codes) */
 
856
            0x19, 0xe0, /*   Usage Minimum (224) */
 
857
            0x29, 0xe7, /*   Usage Maximum (231) */
 
858
            0x15, 0x00, /*   Logical Minimum (0) */
 
859
            0x25, 0x01, /*   Logical Maximum (1) */
 
860
            0x81, 0x02, /*   Input (Data, Variable, Absolute) */
 
861
            0x95, 0x01, /*   Report Count (1) */
 
862
            0x75, 0x08, /*   Report Size (8) */
 
863
            0x81, 0x01, /*   Input (Constant) */
 
864
            0x95, 0x05, /*   Report Count (5) */
 
865
            0x75, 0x01, /*   Report Size (1) */
 
866
            0x05, 0x08, /*   Usage Page (LEDs) */
 
867
            0x19, 0x01, /*   Usage Minimum (1) */
 
868
            0x29, 0x05, /*   Usage Maximum (5) */
 
869
            0x91, 0x02, /*   Output (Data, Variable, Absolute) */
 
870
            0x95, 0x01, /*   Report Count (1) */
 
871
            0x75, 0x03, /*   Report Size (3) */
 
872
            0x91, 0x01, /*   Output (Constant) */
 
873
            0x95, 0x06, /*   Report Count (6) */
 
874
            0x75, 0x08, /*   Report Size (8) */
 
875
            0x15, 0x00, /*   Logical Minimum (0) */
 
876
            0x25, 0xff, /*   Logical Maximum (255) */
 
877
            0x05, 0x07, /*   Usage Page (Key Codes) */
 
878
            0x19, 0x00, /*   Usage Minimum (0) */
 
879
            0x29, 0xff, /*   Usage Maximum (255) */
 
880
            0x81, 0x00, /*   Input (Data, Array) */
 
881
            0xc0        /* End Collection */
 
882
    ))))
 
883
    ATTRIBUTE(LANG_ID_BASE_LIST,        LIST(
 
884
        LIST(UINT16(0x0409) UINT16(0x0100))
 
885
    ))
 
886
    ATTRIBUTE(SDP_DISABLE,              SDP_FALSE)
 
887
    ATTRIBUTE(BATTERY_POWER,            SDP_TRUE)
 
888
    ATTRIBUTE(REMOTE_WAKEUP,            SDP_TRUE)
 
889
    ATTRIBUTE(BOOT_DEVICE,              SDP_TRUE)       /* XXX: untested */
 
890
    ATTRIBUTE(SUPERVISION_TIMEOUT,      UINT16(0x0c80))
 
891
    ATTRIBUTE(NORMALLY_CONNECTABLE,     SDP_TRUE)
 
892
    ATTRIBUTE(PROFILE_VERSION,          UINT16(0x0100))
 
893
)
 
894
 
 
895
SERVICE(sdp,
 
896
    ATTRIBUTE(RECORD_HANDLE,   UINT32(0))       /* Filled in later */
 
897
    ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID)))
 
898
    ATTRIBUTE(RECORD_STATE,    UINT32(1))
 
899
    ATTRIBUTE(PROTO_DESC_LIST, LIST(
 
900
        LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
 
901
        LIST(UUID128(SDP_UUID))
 
902
    ))
 
903
    ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
 
904
    ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
 
905
        UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
 
906
    ))
 
907
    ATTRIBUTE(PFILE_DESC_LIST, LIST(
 
908
        LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100))
 
909
    ))
 
910
    ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
 
911
    ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
 
912
 
 
913
    /* Profile specific */
 
914
    ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100)))
 
915
    ATTRIBUTE(SVCDB_STATE    , UINT32(1))
 
916
)
 
917
 
 
918
SERVICE(pnp,
 
919
    ATTRIBUTE(RECORD_HANDLE,   UINT32(0))       /* Filled in later */
 
920
    ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID)))
 
921
    ATTRIBUTE(RECORD_STATE,    UINT32(1))
 
922
    ATTRIBUTE(PROTO_DESC_LIST, LIST(
 
923
        LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP))
 
924
        LIST(UUID128(SDP_UUID))
 
925
    ))
 
926
    ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002)))
 
927
    ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST(
 
928
        UINT16(0x656e) UINT16(0x006a) UINT16(0x0100)
 
929
    ))
 
930
    ATTRIBUTE(PFILE_DESC_LIST, LIST(
 
931
        LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100))
 
932
    ))
 
933
    ATTRIBUTE(DOC_URL,         URL("http://bellard.org/qemu/user-doc.html"))
 
934
    ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU " QEMU_VERSION))
 
935
 
 
936
    /* Profile specific */
 
937
    ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100))
 
938
    ATTRIBUTE(VERSION,         UINT16(0x0100))
 
939
    ATTRIBUTE(PRIMARY_RECORD,  SDP_TRUE)
 
940
)
 
941
 
 
942
static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev,
 
943
                struct bt_l2cap_conn_params_s *params)
 
944
{
 
945
    struct bt_l2cap_sdp_state_s *sdp = qemu_mallocz(sizeof(*sdp));
 
946
    struct sdp_def_service_s *services[] = {
 
947
        &sdp_service_sdp_s,
 
948
        &sdp_service_hid_s,
 
949
        &sdp_service_pnp_s,
 
950
        NULL,
 
951
    };
 
952
 
 
953
    sdp->channel = params;
 
954
    sdp->channel->opaque = sdp;
 
955
    sdp->channel->close = bt_l2cap_sdp_close_ch;
 
956
    sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in;
 
957
 
 
958
    sdp_service_db_build(sdp, services);
 
959
 
 
960
    return 0;
 
961
}
 
962
 
 
963
void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev)
 
964
{
 
965
    bt_l2cap_psm_register(dev, BT_PSM_SDP,
 
966
                    MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch);
 
967
}