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
sdp_datalen(const uint8_t ** element,ssize_t * left)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
sdp_uuid_match(struct sdp_service_record_s * record,const uint8_t * uuid,ssize_t datalen)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
sdp_svc_match(struct bt_l2cap_sdp_state_s * sdp,const uint8_t ** req,ssize_t * len)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
sdp_svc_search(struct bt_l2cap_sdp_state_s * sdp,uint8_t * rsp,const uint8_t * req,ssize_t len)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
sdp_attr_match(struct sdp_service_record_s * record,const uint8_t ** req,ssize_t * len)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
sdp_attr_get(struct bt_l2cap_sdp_state_s * sdp,uint8_t * rsp,const uint8_t * req,ssize_t len)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
sdp_svc_attr_match(struct bt_l2cap_sdp_state_s * sdp,const uint8_t ** req,ssize_t * len)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
sdp_svc_search_attr_get(struct bt_l2cap_sdp_state_s * sdp,uint8_t * rsp,const uint8_t * req,ssize_t len)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
bt_l2cap_sdp_sdu_in(void * opaque,const uint8_t * data,int len)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
bt_l2cap_sdp_close_ch(void * opaque)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. */
sdp_attr_max_size(struct sdp_def_data_element_s * element,int * uuids)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
sdp_attr_write(uint8_t * data,struct sdp_def_data_element_s * element,int ** uuid)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
sdp_attributeid_compare(const struct sdp_service_attribute_s * a,const struct sdp_service_attribute_s * b)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
sdp_uuid_compare(const int * a,const int * b)692 static int sdp_uuid_compare(const int *a, const int *b)
693 {
694 return *a - *b;
695 }
696
sdp_service_record_build(struct sdp_service_record_s * record,struct sdp_def_service_s * def,int handle)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
sdp_service_db_build(struct bt_l2cap_sdp_state_s * sdp,struct sdp_def_service_s ** service)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 TRUE { \
790 .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \
791 .value.uint = 1, \
792 },
793 #define 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, TRUE)
846 ATTRIBUTE(RECONNECT_INITIATE, 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, FALSE)
887 ATTRIBUTE(BATTERY_POWER, TRUE)
888 ATTRIBUTE(REMOTE_WAKEUP, TRUE)
889 ATTRIBUTE(BOOT_DEVICE, TRUE) /* XXX: untested */
890 ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80))
891 ATTRIBUTE(NORMALLY_CONNECTABLE, 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, TRUE)
940 )
941
bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s * dev,struct bt_l2cap_conn_params_s * params)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
bt_l2cap_sdp_init(struct bt_l2cap_device_s * dev)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 }
968