Statistics
| Branch: | Revision:

root / hw / bt-sdp.c @ ff753bb9

History | View | Annotate | Download (28.9 kB)

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
}