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