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/error-report.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 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 | error_report("%s: short SDP PDU (%iB)." , __func__, 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 | error_report("%s: wrong SDP PDU length (%iB != %iB)." , |
521 | __func__, 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 | error_report("%s: unexpected SDP PDU ID %02x." , |
548 | __func__, 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[0].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 | |
724 | assert(len > 0); |
725 | record->uuids = pow2ceil(record->uuids); |
726 | record->attribute_list = |
727 | g_malloc0(record->attributes * sizeof(*record->attribute_list)); |
728 | record->uuid = |
729 | g_malloc0(record->uuids * sizeof(*record->uuid)); |
730 | data = g_malloc(len); |
731 | |
732 | record->attributes = 0; |
733 | uuid = record->uuid; |
734 | while (def->attributes[record->attributes].data.type) { |
735 | int attribute_id = def->attributes[record->attributes].id; |
736 | record->attribute_list[record->attributes].pair = data; |
737 | record->attribute_list[record->attributes].attribute_id = attribute_id; |
738 | |
739 | len = 0; |
740 | data[len ++] = SDP_DTYPE_UINT | SDP_DSIZE_2; |
741 | data[len ++] = attribute_id >> 8; |
742 | data[len ++] = attribute_id & 0xff; |
743 | len += sdp_attr_write(data + len, |
744 | &def->attributes[record->attributes].data, &uuid); |
745 | |
746 | /* Special case: assign a ServiceRecordHandle in sequence */ |
747 | if (def->attributes[record->attributes].id == SDP_ATTR_RECORD_HANDLE) |
748 | def->attributes[record->attributes].data.value.uint = handle; |
749 | /* Note: we could also assign a ServiceDescription based on |
750 | * sdp->device.device->lmp_name. */ |
751 | |
752 | record->attribute_list[record->attributes ++].len = len; |
753 | data += len; |
754 | } |
755 | |
756 | /* Sort the attribute list by the AttributeID. The first must be |
757 | * SDP_ATTR_RECORD_HANDLE so that bt_l2cap_sdp_close_ch can free |
758 | * the buffer. |
759 | */ |
760 | qsort(record->attribute_list, record->attributes, |
761 | sizeof(*record->attribute_list), |
762 | (void *) sdp_attributeid_compare); |
763 | assert(record->attribute_list[0].pair == data); |
764 | |
765 | /* Sort the searchable UUIDs list for bisection */ |
766 | qsort(record->uuid, record->uuids, |
767 | sizeof(*record->uuid), |
768 | (void *) sdp_uuid_compare); |
769 | } |
770 | |
771 | static void sdp_service_db_build(struct bt_l2cap_sdp_state_s *sdp, |
772 | struct sdp_def_service_s **service) |
773 | { |
774 | sdp->services = 0; |
775 | while (service[sdp->services]) |
776 | sdp->services ++; |
777 | sdp->service_list = |
778 | g_malloc0(sdp->services * sizeof(*sdp->service_list)); |
779 | |
780 | sdp->services = 0; |
781 | while (*service) { |
782 | sdp_service_record_build(&sdp->service_list[sdp->services], |
783 | *service, sdp->services); |
784 | service ++; |
785 | sdp->services ++; |
786 | } |
787 | } |
788 | |
789 | #define LAST { .type = 0 } |
790 | #define SERVICE(name, attrs) \ |
791 | static struct sdp_def_service_s glue(glue(sdp_service_, name), _s) = { \ |
792 | .attributes = { attrs { .data = LAST } }, \ |
793 | }; |
794 | #define ATTRIBUTE(attrid, val) { .id = glue(SDP_ATTR_, attrid), .data = val }, |
795 | #define UINT8(val) { \ |
796 | .type = SDP_DTYPE_UINT | SDP_DSIZE_1, \ |
797 | .value.uint = val, \ |
798 | }, |
799 | #define UINT16(val) { \ |
800 | .type = SDP_DTYPE_UINT | SDP_DSIZE_2, \ |
801 | .value.uint = val, \ |
802 | }, |
803 | #define UINT32(val) { \ |
804 | .type = SDP_DTYPE_UINT | SDP_DSIZE_4, \ |
805 | .value.uint = val, \ |
806 | }, |
807 | #define UUID128(val) { \ |
808 | .type = SDP_DTYPE_UUID | SDP_DSIZE_16, \ |
809 | .value.uint = val, \ |
810 | }, |
811 | #define SDP_TRUE { \ |
812 | .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ |
813 | .value.uint = 1, \ |
814 | }, |
815 | #define SDP_FALSE { \ |
816 | .type = SDP_DTYPE_BOOL | SDP_DSIZE_1, \ |
817 | .value.uint = 0, \ |
818 | }, |
819 | #define STRING(val) { \ |
820 | .type = SDP_DTYPE_STRING, \ |
821 | .value.str = val, \ |
822 | }, |
823 | #define ARRAY(...) { \ |
824 | .type = SDP_DTYPE_STRING | SDP_DSIZE_2, \ |
825 | .value.str = (char []) { __VA_ARGS__, 0, 0 }, \ |
826 | }, |
827 | #define URL(val) { \ |
828 | .type = SDP_DTYPE_URL, \ |
829 | .value.str = val, \ |
830 | }, |
831 | #if 1 |
832 | #define LIST(val) { \ |
833 | .type = SDP_DTYPE_SEQ, \ |
834 | .value.list = (struct sdp_def_data_element_s []) { val LAST }, \ |
835 | }, |
836 | #endif |
837 | |
838 | /* Try to keep each single attribute below MAX_PDU_OUT_SIZE bytes |
839 | * in resulting SDP data representation size. */ |
840 | |
841 | SERVICE(hid, |
842 | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
843 | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(HID_SVCLASS_ID))) |
844 | ATTRIBUTE(RECORD_STATE, UINT32(1)) |
845 | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
846 | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_HID_CTRL)) |
847 | LIST(UUID128(HIDP_UUID)) |
848 | )) |
849 | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) |
850 | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
851 | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
852 | )) |
853 | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
854 | LIST(UUID128(HID_PROFILE_ID) UINT16(0x0100)) |
855 | )) |
856 | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html" )) |
857 | ATTRIBUTE(SVCNAME_PRIMARY, STRING("QEMU Bluetooth HID" )) |
858 | ATTRIBUTE(SVCDESC_PRIMARY, STRING("QEMU Keyboard/Mouse" )) |
859 | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU" )) |
860 | |
861 | /* Profile specific */ |
862 | ATTRIBUTE(DEVICE_RELEASE_NUMBER, UINT16(0x0091)) /* Deprecated, remove */ |
863 | ATTRIBUTE(PARSER_VERSION, UINT16(0x0111)) |
864 | /* TODO: extract from l2cap_device->device.class[0] */ |
865 | ATTRIBUTE(DEVICE_SUBCLASS, UINT8(0x40)) |
866 | ATTRIBUTE(COUNTRY_CODE, UINT8(0x15)) |
867 | ATTRIBUTE(VIRTUAL_CABLE, SDP_TRUE) |
868 | ATTRIBUTE(RECONNECT_INITIATE, SDP_FALSE) |
869 | /* TODO: extract from hid->usbdev->report_desc */ |
870 | ATTRIBUTE(DESCRIPTOR_LIST, LIST( |
871 | LIST(UINT8(0x22) ARRAY( |
872 | 0x05, 0x01, /* Usage Page (Generic Desktop) */ |
873 | 0x09, 0x06, /* Usage (Keyboard) */ |
874 | 0xa1, 0x01, /* Collection (Application) */ |
875 | 0x75, 0x01, /* Report Size (1) */ |
876 | 0x95, 0x08, /* Report Count (8) */ |
877 | 0x05, 0x07, /* Usage Page (Key Codes) */ |
878 | 0x19, 0xe0, /* Usage Minimum (224) */ |
879 | 0x29, 0xe7, /* Usage Maximum (231) */ |
880 | 0x15, 0x00, /* Logical Minimum (0) */ |
881 | 0x25, 0x01, /* Logical Maximum (1) */ |
882 | 0x81, 0x02, /* Input (Data, Variable, Absolute) */ |
883 | 0x95, 0x01, /* Report Count (1) */ |
884 | 0x75, 0x08, /* Report Size (8) */ |
885 | 0x81, 0x01, /* Input (Constant) */ |
886 | 0x95, 0x05, /* Report Count (5) */ |
887 | 0x75, 0x01, /* Report Size (1) */ |
888 | 0x05, 0x08, /* Usage Page (LEDs) */ |
889 | 0x19, 0x01, /* Usage Minimum (1) */ |
890 | 0x29, 0x05, /* Usage Maximum (5) */ |
891 | 0x91, 0x02, /* Output (Data, Variable, Absolute) */ |
892 | 0x95, 0x01, /* Report Count (1) */ |
893 | 0x75, 0x03, /* Report Size (3) */ |
894 | 0x91, 0x01, /* Output (Constant) */ |
895 | 0x95, 0x06, /* Report Count (6) */ |
896 | 0x75, 0x08, /* Report Size (8) */ |
897 | 0x15, 0x00, /* Logical Minimum (0) */ |
898 | 0x25, 0xff, /* Logical Maximum (255) */ |
899 | 0x05, 0x07, /* Usage Page (Key Codes) */ |
900 | 0x19, 0x00, /* Usage Minimum (0) */ |
901 | 0x29, 0xff, /* Usage Maximum (255) */ |
902 | 0x81, 0x00, /* Input (Data, Array) */ |
903 | 0xc0 /* End Collection */ |
904 | )))) |
905 | ATTRIBUTE(LANG_ID_BASE_LIST, LIST( |
906 | LIST(UINT16(0x0409) UINT16(0x0100)) |
907 | )) |
908 | ATTRIBUTE(SDP_DISABLE, SDP_FALSE) |
909 | ATTRIBUTE(BATTERY_POWER, SDP_TRUE) |
910 | ATTRIBUTE(REMOTE_WAKEUP, SDP_TRUE) |
911 | ATTRIBUTE(BOOT_DEVICE, SDP_TRUE) /* XXX: untested */ |
912 | ATTRIBUTE(SUPERVISION_TIMEOUT, UINT16(0x0c80)) |
913 | ATTRIBUTE(NORMALLY_CONNECTABLE, SDP_TRUE) |
914 | ATTRIBUTE(PROFILE_VERSION, UINT16(0x0100)) |
915 | ) |
916 | |
917 | SERVICE(sdp, |
918 | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
919 | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(SDP_SERVER_SVCLASS_ID))) |
920 | ATTRIBUTE(RECORD_STATE, UINT32(1)) |
921 | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
922 | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) |
923 | LIST(UUID128(SDP_UUID)) |
924 | )) |
925 | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) |
926 | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
927 | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
928 | )) |
929 | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
930 | LIST(UUID128(SDP_SERVER_PROFILE_ID) UINT16(0x0100)) |
931 | )) |
932 | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html" )) |
933 | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU" )) |
934 | |
935 | /* Profile specific */ |
936 | ATTRIBUTE(VERSION_NUM_LIST, LIST(UINT16(0x0100))) |
937 | ATTRIBUTE(SVCDB_STATE , UINT32(1)) |
938 | ) |
939 | |
940 | SERVICE(pnp, |
941 | ATTRIBUTE(RECORD_HANDLE, UINT32(0)) /* Filled in later */ |
942 | ATTRIBUTE(SVCLASS_ID_LIST, LIST(UUID128(PNP_INFO_SVCLASS_ID))) |
943 | ATTRIBUTE(RECORD_STATE, UINT32(1)) |
944 | ATTRIBUTE(PROTO_DESC_LIST, LIST( |
945 | LIST(UUID128(L2CAP_UUID) UINT16(BT_PSM_SDP)) |
946 | LIST(UUID128(SDP_UUID)) |
947 | )) |
948 | ATTRIBUTE(BROWSE_GRP_LIST, LIST(UUID128(0x1002))) |
949 | ATTRIBUTE(LANG_BASE_ATTR_ID_LIST, LIST( |
950 | UINT16(0x656e) UINT16(0x006a) UINT16(0x0100) |
951 | )) |
952 | ATTRIBUTE(PFILE_DESC_LIST, LIST( |
953 | LIST(UUID128(PNP_INFO_PROFILE_ID) UINT16(0x0100)) |
954 | )) |
955 | ATTRIBUTE(DOC_URL, URL("http://bellard.org/qemu/user-doc.html" )) |
956 | ATTRIBUTE(SVCPROV_PRIMARY, STRING("QEMU" )) |
957 | |
958 | /* Profile specific */ |
959 | ATTRIBUTE(SPECIFICATION_ID, UINT16(0x0100)) |
960 | ATTRIBUTE(VERSION, UINT16(0x0100)) |
961 | ATTRIBUTE(PRIMARY_RECORD, SDP_TRUE) |
962 | ) |
963 | |
964 | static int bt_l2cap_sdp_new_ch(struct bt_l2cap_device_s *dev, |
965 | struct bt_l2cap_conn_params_s *params) |
966 | { |
967 | struct bt_l2cap_sdp_state_s *sdp = g_malloc0(sizeof(*sdp)); |
968 | struct sdp_def_service_s *services[] = { |
969 | &sdp_service_sdp_s, |
970 | &sdp_service_hid_s, |
971 | &sdp_service_pnp_s, |
972 | NULL, |
973 | }; |
974 | |
975 | sdp->channel = params; |
976 | sdp->channel->opaque = sdp; |
977 | sdp->channel->close = bt_l2cap_sdp_close_ch; |
978 | sdp->channel->sdu_in = bt_l2cap_sdp_sdu_in; |
979 | |
980 | sdp_service_db_build(sdp, services); |
981 | |
982 | return 0; |
983 | } |
984 | |
985 | void bt_l2cap_sdp_init(struct bt_l2cap_device_s *dev) |
986 | { |
987 | bt_l2cap_psm_register(dev, BT_PSM_SDP, |
988 | MAX_PDU_OUT_SIZE, bt_l2cap_sdp_new_ch); |
989 | } |
990 | |