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