← Back to branch summary

~vcs-imports/qemu/git

« back to all changes in this revision

Viewing changes to hw/bt-sdp.c

  • Committer: pbrook
  • Date: 2007-03-04 00:52:16 UTC
  • Revision ID: git-v1:7a2d6d9650ed16b2cdb0b4876fe9efce7ef8ea6d
64bit->win32 cross build fix.


git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@2467 c046a42c-6fe2-441c-8c8c-71466251a162

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