![[BACK]](/cvsweb/icons/back.gif){kind=icon}
Return to bt-sdp.c CVS log ![[TXT]](/cvsweb/icons/text.gif){kind=icon}
![[DIR]](/cvsweb/icons/dir.gif){kind=icon}
Up to [Qemu by Fabrice Bellard] / qemu / hw|
Return to bt-sdp.c CVS log | Up to [Qemu by Fabrice Bellard] / qemu / hw |
1.1 ! root 1: /* ! 2: * Service Discover Protocol server for QEMU L2CAP devices ! 3: * ! 4: * Copyright (C) 2008 Andrzej Zaborowski <[email protected]> ! 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: 0, ! 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: }
This archive runs on limited infrastructure. Preserving old code on modern bandwidth. Automated agents are requested to crawl responsibly.