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