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