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