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