1 /* 2 * 3 * BlueZ - Bluetooth protocol stack for Linux 4 * 5 * Copyright (C) 2001-2002 Nokia Corporation 6 * Copyright (C) 2002-2003 Maxim Krasnyansky <maxk (at) qualcomm.com> 7 * Copyright (C) 2002-2010 Marcel Holtmann <marcel (at) holtmann.org> 8 * Copyright (C) 2002-2003 Stephen Crane <steve.crane (at) rococosoft.com> 9 * 10 * 11 * This program is free software; you can redistribute it and/or modify 12 * it under the terms of the GNU General Public License as published by 13 * the Free Software Foundation; either version 2 of the License, or 14 * (at your option) any later version. 15 * 16 * This program is distributed in the hope that it will be useful, 17 * but WITHOUT ANY WARRANTY; without even the implied warranty of 18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 19 * GNU General Public License for more details. 20 * 21 * You should have received a copy of the GNU General Public License 22 * along with this program; if not, write to the Free Software 23 * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA 24 * 25 */ 26 27 #ifdef HAVE_CONFIG_H 28 #include <config.h> 29 #endif 30 31 #include <stdio.h> 32 #include <errno.h> 33 #include <stdlib.h> 34 #include <string.h> 35 #include <limits.h> 36 #include <sys/socket.h> 37 38 #include <bluetooth/bluetooth.h> 39 #include <bluetooth/l2cap.h> 40 #include <bluetooth/sdp.h> 41 #include <bluetooth/sdp_lib.h> 42 43 #include <netinet/in.h> 44 45 #include "sdpd.h" 46 #include "log.h" 47 48 typedef struct { 49 uint32_t timestamp; 50 union { 51 uint16_t maxBytesSent; 52 uint16_t lastIndexSent; 53 } cStateValue; 54 } sdp_cont_state_t; 55 56 #define SDP_CONT_STATE_SIZE (sizeof(uint8_t) + sizeof(sdp_cont_state_t)) 57 58 #define MIN(x, y) ((x) < (y)) ? (x): (y) 59 60 typedef struct _sdp_cstate_list sdp_cstate_list_t; 61 62 struct _sdp_cstate_list { 63 sdp_cstate_list_t *next; 64 uint32_t timestamp; 65 sdp_buf_t buf; 66 }; 67 68 static sdp_cstate_list_t *cstates; 69 70 /* FIXME: should probably remove it when it's found */ 71 static sdp_buf_t *sdp_get_cached_rsp(sdp_cont_state_t *cstate) 72 { 73 sdp_cstate_list_t *p; 74 75 for (p = cstates; p; p = p->next) 76 if (p->timestamp == cstate->timestamp) 77 return &p->buf; 78 return 0; 79 } 80 81 static uint32_t sdp_cstate_alloc_buf(sdp_buf_t *buf) 82 { 83 sdp_cstate_list_t *cstate = malloc(sizeof(sdp_cstate_list_t)); 84 uint8_t *data = malloc(buf->data_size); 85 86 memcpy(data, buf->data, buf->data_size); 87 memset((char *)cstate, 0, sizeof(sdp_cstate_list_t)); 88 cstate->buf.data = data; 89 cstate->buf.data_size = buf->data_size; 90 cstate->buf.buf_size = buf->data_size; 91 cstate->timestamp = sdp_get_time(); 92 cstate->next = cstates; 93 cstates = cstate; 94 return cstate->timestamp; 95 } 96 97 /* Additional values for checking datatype (not in spec) */ 98 #define SDP_TYPE_UUID 0xfe 99 #define SDP_TYPE_ATTRID 0xff 100 101 struct attrid { 102 uint8_t dtd; 103 union { 104 uint16_t uint16; 105 uint32_t uint32; 106 }; 107 }; 108 109 /* 110 * Generic data element sequence extractor. Builds 111 * a list whose elements are those found in the 112 * sequence. The data type of elements found in the 113 * sequence is returned in the reference pDataType 114 */ 115 static int extract_des(uint8_t *buf, int len, sdp_list_t **svcReqSeq, uint8_t *pDataType, uint8_t expectedType) 116 { 117 uint8_t seqType; 118 int scanned, data_size = 0; 119 short numberOfElements = 0; 120 int seqlen = 0; 121 sdp_list_t *pSeq = NULL; 122 uint8_t dataType; 123 int status = 0; 124 const uint8_t *p; 125 size_t bufsize; 126 127 scanned = sdp_extract_seqtype(buf, len, &seqType, &data_size); 128 129 SDPDBG("Seq type : %d", seqType); 130 if (!scanned || (seqType != SDP_SEQ8 && seqType != SDP_SEQ16)) { 131 error("Unknown seq type"); 132 return -1; 133 } 134 p = buf + scanned; 135 bufsize = len - scanned; 136 137 SDPDBG("Data size : %d", data_size); 138 139 for (;;) { 140 char *pElem = NULL; 141 int localSeqLength = 0; 142 143 if (bufsize < sizeof(uint8_t)) { 144 SDPDBG("->Unexpected end of buffer"); 145 goto failed; 146 } 147 148 dataType = *p; 149 150 SDPDBG("Data type: 0x%02x", dataType); 151 152 if (expectedType == SDP_TYPE_UUID) { 153 if (dataType != SDP_UUID16 && dataType != SDP_UUID32 && dataType != SDP_UUID128) { 154 SDPDBG("->Unexpected Data type (expected UUID_ANY)"); 155 goto failed; 156 } 157 } else if (expectedType == SDP_TYPE_ATTRID && 158 (dataType != SDP_UINT16 && dataType != SDP_UINT32)) { 159 SDPDBG("->Unexpected Data type (expected 0x%02x or 0x%02x)", 160 SDP_UINT16, SDP_UINT32); 161 goto failed; 162 } else if (expectedType != SDP_TYPE_ATTRID && dataType != expectedType) { 163 SDPDBG("->Unexpected Data type (expected 0x%02x)", expectedType); 164 goto failed; 165 } 166 167 switch (dataType) { 168 case SDP_UINT16: 169 p += sizeof(uint8_t); 170 seqlen += sizeof(uint8_t); 171 bufsize -= sizeof(uint8_t); 172 if (bufsize < sizeof(uint16_t)) { 173 SDPDBG("->Unexpected end of buffer"); 174 goto failed; 175 } 176 177 if (expectedType == SDP_TYPE_ATTRID) { 178 struct attrid *aid; 179 aid = malloc(sizeof(struct attrid)); 180 aid->dtd = dataType; 181 bt_put_unaligned(ntohs(bt_get_unaligned((uint16_t *)p)), (uint16_t *)&aid->uint16); 182 pElem = (char *) aid; 183 } else { 184 pElem = malloc(sizeof(uint16_t)); 185 bt_put_unaligned(ntohs(bt_get_unaligned((uint16_t *)p)), (uint16_t *)pElem); 186 } 187 p += sizeof(uint16_t); 188 seqlen += sizeof(uint16_t); 189 bufsize -= sizeof(uint16_t); 190 break; 191 case SDP_UINT32: 192 p += sizeof(uint8_t); 193 seqlen += sizeof(uint8_t); 194 bufsize -= sizeof(uint8_t); 195 if (bufsize < (int)sizeof(uint32_t)) { 196 SDPDBG("->Unexpected end of buffer"); 197 goto failed; 198 } 199 200 if (expectedType == SDP_TYPE_ATTRID) { 201 struct attrid *aid; 202 aid = malloc(sizeof(struct attrid)); 203 aid->dtd = dataType; 204 bt_put_unaligned(ntohl(bt_get_unaligned((uint32_t *)p)), (uint32_t *)&aid->uint32); 205 pElem = (char *) aid; 206 } else { 207 pElem = malloc(sizeof(uint32_t)); 208 bt_put_unaligned(ntohl(bt_get_unaligned((uint32_t *)p)), (uint32_t *)pElem); 209 } 210 p += sizeof(uint32_t); 211 seqlen += sizeof(uint32_t); 212 bufsize -= sizeof(uint32_t); 213 break; 214 case SDP_UUID16: 215 case SDP_UUID32: 216 case SDP_UUID128: 217 pElem = malloc(sizeof(uuid_t)); 218 status = sdp_uuid_extract(p, bufsize, (uuid_t *) pElem, &localSeqLength); 219 if (status < 0) { 220 free(pElem); 221 goto failed; 222 } 223 seqlen += localSeqLength; 224 p += localSeqLength; 225 bufsize -= localSeqLength; 226 break; 227 default: 228 return -1; 229 } 230 if (status == 0) { 231 pSeq = sdp_list_append(pSeq, pElem); 232 numberOfElements++; 233 SDPDBG("No of elements : %d", numberOfElements); 234 235 if (seqlen == data_size) 236 break; 237 else if (seqlen > data_size || seqlen > len) 238 goto failed; 239 } else 240 free(pElem); 241 } 242 *svcReqSeq = pSeq; 243 scanned += seqlen; 244 *pDataType = dataType; 245 return scanned; 246 247 failed: 248 sdp_list_free(pSeq, free); 249 return -1; 250 } 251 252 static int sdp_set_cstate_pdu(sdp_buf_t *buf, sdp_cont_state_t *cstate) 253 { 254 uint8_t *pdata = buf->data + buf->data_size; 255 int length = 0; 256 257 if (cstate) { 258 SDPDBG("Non null sdp_cstate_t id : 0x%x", cstate->timestamp); 259 *pdata = sizeof(sdp_cont_state_t); 260 pdata += sizeof(uint8_t); 261 length += sizeof(uint8_t); 262 memcpy(pdata, cstate, sizeof(sdp_cont_state_t)); 263 length += sizeof(sdp_cont_state_t); 264 } else { 265 /* set "null" continuation state */ 266 *pdata = 0; 267 length += sizeof(uint8_t); 268 } 269 buf->data_size += length; 270 return length; 271 } 272 273 static int sdp_cstate_get(uint8_t *buffer, size_t len, 274 sdp_cont_state_t **cstate) 275 { 276 uint8_t cStateSize = *buffer; 277 278 SDPDBG("Continuation State size : %d", cStateSize); 279 280 if (cStateSize == 0) { 281 *cstate = NULL; 282 return 0; 283 } 284 285 buffer++; 286 len--; 287 288 if (len < sizeof(sdp_cont_state_t)) 289 return -EINVAL; 290 291 /* 292 * Check if continuation state exists, if yes attempt 293 * to get response remainder from cache, else send error 294 */ 295 296 *cstate = malloc(sizeof(sdp_cont_state_t)); 297 if (!(*cstate)) 298 return -ENOMEM; 299 300 memcpy(*cstate, buffer, sizeof(sdp_cont_state_t)); 301 302 SDPDBG("Cstate TS : 0x%x", (*cstate)->timestamp); 303 SDPDBG("Bytes sent : %d", (*cstate)->cStateValue.maxBytesSent); 304 305 return 0; 306 } 307 308 /* 309 * The matching process is defined as "each and every UUID 310 * specified in the "search pattern" must be present in the 311 * "target pattern". Here "search pattern" is the set of UUIDs 312 * specified by the service discovery client and "target pattern" 313 * is the set of UUIDs present in a service record. 314 * 315 * Return 1 if each and every UUID in the search 316 * pattern exists in the target pattern, 0 if the 317 * match succeeds and -1 on error. 318 */ 319 static int sdp_match_uuid(sdp_list_t *search, sdp_list_t *pattern) 320 { 321 /* 322 * The target is a sorted list, so we need not look 323 * at all elements to confirm existence of an element 324 * from the search pattern 325 */ 326 int patlen = sdp_list_len(pattern); 327 328 if (patlen < sdp_list_len(search)) 329 return -1; 330 for (; search; search = search->next) { 331 uuid_t *uuid128; 332 void *data = search->data; 333 sdp_list_t *list; 334 if (data == NULL) 335 return -1; 336 337 /* create 128-bit form of the search UUID */ 338 uuid128 = sdp_uuid_to_uuid128((uuid_t *)data); 339 list = sdp_list_find(pattern, uuid128, sdp_uuid128_cmp); 340 bt_free(uuid128); 341 if (!list) 342 return 0; 343 } 344 return 1; 345 } 346 347 /* 348 * Service search request PDU. This method extracts the search pattern 349 * (a sequence of UUIDs) and calls the matching function 350 * to find matching services 351 */ 352 static int service_search_req(sdp_req_t *req, sdp_buf_t *buf) 353 { 354 int status = 0, i, plen, mlen, mtu, scanned; 355 sdp_list_t *pattern = NULL; 356 uint16_t expected, actual, rsp_count = 0; 357 uint8_t dtd; 358 sdp_cont_state_t *cstate = NULL; 359 uint8_t *pCacheBuffer = NULL; 360 int handleSize = 0; 361 uint32_t cStateId = 0; 362 short *pTotalRecordCount, *pCurrentRecordCount; 363 uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t); 364 size_t data_left = req->len - sizeof(sdp_pdu_hdr_t); 365 366 scanned = extract_des(pdata, data_left, &pattern, &dtd, SDP_TYPE_UUID); 367 368 if (scanned == -1) { 369 status = SDP_INVALID_SYNTAX; 370 goto done; 371 } 372 pdata += scanned; 373 data_left -= scanned; 374 375 plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen); 376 mlen = scanned + sizeof(uint16_t) + 1; 377 /* ensure we don't read past buffer */ 378 if (plen < mlen || plen != mlen + *(uint8_t *)(pdata+sizeof(uint16_t))) { 379 status = SDP_INVALID_SYNTAX; 380 goto done; 381 } 382 383 if (data_left < sizeof(uint16_t)) { 384 status = SDP_INVALID_SYNTAX; 385 goto done; 386 } 387 388 expected = ntohs(bt_get_unaligned((uint16_t *)pdata)); 389 390 SDPDBG("Expected count: %d", expected); 391 SDPDBG("Bytes scanned : %d", scanned); 392 393 pdata += sizeof(uint16_t); 394 data_left -= sizeof(uint16_t); 395 396 /* 397 * Check if continuation state exists, if yes attempt 398 * to get rsp remainder from cache, else send error 399 */ 400 if (sdp_cstate_get(pdata, data_left, &cstate) < 0) { 401 status = SDP_INVALID_SYNTAX; 402 goto done; 403 } 404 405 mtu = req->mtu - sizeof(sdp_pdu_hdr_t) - sizeof(uint16_t) - sizeof(uint16_t) - SDP_CONT_STATE_SIZE; 406 actual = MIN(expected, mtu >> 2); 407 408 /* make space in the rsp buffer for total and current record counts */ 409 pdata = buf->data; 410 411 /* total service record count = 0 */ 412 pTotalRecordCount = (short *)pdata; 413 bt_put_unaligned(0, (uint16_t *)pdata); 414 pdata += sizeof(uint16_t); 415 buf->data_size += sizeof(uint16_t); 416 417 /* current service record count = 0 */ 418 pCurrentRecordCount = (short *)pdata; 419 bt_put_unaligned(0, (uint16_t *)pdata); 420 pdata += sizeof(uint16_t); 421 buf->data_size += sizeof(uint16_t); 422 423 if (cstate == NULL) { 424 /* for every record in the DB, do a pattern search */ 425 sdp_list_t *list = sdp_get_record_list(); 426 427 handleSize = 0; 428 for (; list && rsp_count < expected; list = list->next) { 429 sdp_record_t *rec = list->data; 430 431 SDPDBG("Checking svcRec : 0x%x", rec->handle); 432 433 if (sdp_match_uuid(pattern, rec->pattern) > 0 && 434 sdp_check_access(rec->handle, &req->device)) { 435 rsp_count++; 436 bt_put_unaligned(htonl(rec->handle), (uint32_t *)pdata); 437 pdata += sizeof(uint32_t); 438 handleSize += sizeof(uint32_t); 439 } 440 } 441 442 SDPDBG("Match count: %d", rsp_count); 443 444 buf->data_size += handleSize; 445 bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount); 446 bt_put_unaligned(htons(rsp_count), (uint16_t *)pCurrentRecordCount); 447 448 if (rsp_count > actual) { 449 /* cache the rsp and generate a continuation state */ 450 cStateId = sdp_cstate_alloc_buf(buf); 451 /* 452 * subtract handleSize since we now send only 453 * a subset of handles 454 */ 455 buf->data_size -= handleSize; 456 } else { 457 /* NULL continuation state */ 458 sdp_set_cstate_pdu(buf, NULL); 459 } 460 } 461 462 /* under both the conditions below, the rsp buffer is not built yet */ 463 if (cstate || cStateId > 0) { 464 short lastIndex = 0; 465 466 if (cstate) { 467 /* 468 * Get the previous sdp_cont_state_t and obtain 469 * the cached rsp 470 */ 471 sdp_buf_t *pCache = sdp_get_cached_rsp(cstate); 472 if (pCache) { 473 pCacheBuffer = pCache->data; 474 /* get the rsp_count from the cached buffer */ 475 rsp_count = ntohs(bt_get_unaligned((uint16_t *)pCacheBuffer)); 476 477 /* get index of the last sdp_record_t sent */ 478 lastIndex = cstate->cStateValue.lastIndexSent; 479 } else { 480 status = SDP_INVALID_CSTATE; 481 goto done; 482 } 483 } else { 484 pCacheBuffer = buf->data; 485 lastIndex = 0; 486 } 487 488 /* 489 * Set the local buffer pointer to after the 490 * current record count and increment the cached 491 * buffer pointer to beyond the counters 492 */ 493 pdata = (uint8_t *) pCurrentRecordCount + sizeof(uint16_t); 494 495 /* increment beyond the totalCount and the currentCount */ 496 pCacheBuffer += 2 * sizeof(uint16_t); 497 498 if (cstate) { 499 handleSize = 0; 500 for (i = lastIndex; (i - lastIndex) < actual && i < rsp_count; i++) { 501 bt_put_unaligned(bt_get_unaligned((uint32_t *)(pCacheBuffer + i * sizeof(uint32_t))), (uint32_t *)pdata); 502 pdata += sizeof(uint32_t); 503 handleSize += sizeof(uint32_t); 504 } 505 } else { 506 handleSize = actual << 2; 507 i = actual; 508 } 509 510 buf->data_size += handleSize; 511 bt_put_unaligned(htons(rsp_count), (uint16_t *)pTotalRecordCount); 512 bt_put_unaligned(htons(i - lastIndex), (uint16_t *)pCurrentRecordCount); 513 514 if (i == rsp_count) { 515 /* set "null" continuationState */ 516 sdp_set_cstate_pdu(buf, NULL); 517 } else { 518 /* 519 * there's more: set lastIndexSent to 520 * the new value and move on 521 */ 522 sdp_cont_state_t newState; 523 524 SDPDBG("Setting non-NULL sdp_cstate_t"); 525 526 if (cstate) 527 memcpy(&newState, cstate, sizeof(sdp_cont_state_t)); 528 else { 529 memset(&newState, 0, sizeof(sdp_cont_state_t)); 530 newState.timestamp = cStateId; 531 } 532 newState.cStateValue.lastIndexSent = i; 533 sdp_set_cstate_pdu(buf, &newState); 534 } 535 } 536 537 done: 538 free(cstate); 539 if (pattern) 540 sdp_list_free(pattern, free); 541 542 return status; 543 } 544 545 /* 546 * Extract attribute identifiers from the request PDU. 547 * Clients could request a subset of attributes (by id) 548 * from a service record, instead of the whole set. The 549 * requested identifiers are present in the PDU form of 550 * the request 551 */ 552 static int extract_attrs(sdp_record_t *rec, sdp_list_t *seq, sdp_buf_t *buf) 553 { 554 sdp_buf_t pdu; 555 556 if (!rec) 557 return SDP_INVALID_RECORD_HANDLE; 558 559 if (seq == NULL) { 560 SDPDBG("Attribute sequence is NULL"); 561 return 0; 562 } 563 564 SDPDBG("Entries in attr seq : %d", sdp_list_len(seq)); 565 566 sdp_gen_record_pdu(rec, &pdu); 567 568 for (; seq; seq = seq->next) { 569 struct attrid *aid = seq->data; 570 571 SDPDBG("AttrDataType : %d", aid->dtd); 572 573 if (aid->dtd == SDP_UINT16) { 574 uint16_t attr = bt_get_unaligned((uint16_t *)&aid->uint16); 575 sdp_data_t *a = sdp_data_get(rec, attr); 576 if (a) 577 sdp_append_to_pdu(buf, a); 578 } else if (aid->dtd == SDP_UINT32) { 579 uint32_t range = bt_get_unaligned((uint32_t *)&aid->uint32); 580 uint16_t attr; 581 uint16_t low = (0xffff0000 & range) >> 16; 582 uint16_t high = 0x0000ffff & range; 583 sdp_data_t *data; 584 585 SDPDBG("attr range : 0x%x", range); 586 SDPDBG("Low id : 0x%x", low); 587 SDPDBG("High id : 0x%x", high); 588 589 if (low == 0x0000 && high == 0xffff && pdu.data_size <= buf->buf_size) { 590 /* copy it */ 591 memcpy(buf->data, pdu.data, pdu.data_size); 592 buf->data_size = pdu.data_size; 593 break; 594 } 595 /* (else) sub-range of attributes */ 596 for (attr = low; attr < high; attr++) { 597 data = sdp_data_get(rec, attr); 598 if (data) 599 sdp_append_to_pdu(buf, data); 600 } 601 data = sdp_data_get(rec, high); 602 if (data) 603 sdp_append_to_pdu(buf, data); 604 } else { 605 error("Unexpected data type : 0x%x", aid->dtd); 606 error("Expect uint16_t or uint32_t"); 607 free(pdu.data); 608 return SDP_INVALID_SYNTAX; 609 } 610 } 611 612 free(pdu.data); 613 614 return 0; 615 } 616 617 /* 618 * A request for the attributes of a service record. 619 * First check if the service record (specified by 620 * service record handle) exists, then call the attribute 621 * streaming function 622 */ 623 static int service_attr_req(sdp_req_t *req, sdp_buf_t *buf) 624 { 625 sdp_cont_state_t *cstate = NULL; 626 uint8_t *pResponse = NULL; 627 short cstate_size = 0; 628 sdp_list_t *seq = NULL; 629 uint8_t dtd = 0; 630 int scanned = 0; 631 unsigned int max_rsp_size; 632 int status = 0, plen, mlen; 633 uint8_t *pdata = req->buf + sizeof(sdp_pdu_hdr_t); 634 size_t data_left = req->len - sizeof(sdp_pdu_hdr_t); 635 uint32_t handle; 636 637 if (data_left < sizeof(uint32_t)) { 638 status = SDP_INVALID_SYNTAX; 639 goto done; 640 } 641 642 handle = ntohl(bt_get_unaligned((uint32_t *)pdata)); 643 644 pdata += sizeof(uint32_t); 645 data_left -= sizeof(uint32_t); 646 647 if (data_left < sizeof(uint16_t)) { 648 status = SDP_INVALID_SYNTAX; 649 goto done; 650 } 651 652 max_rsp_size = ntohs(bt_get_unaligned((uint16_t *)pdata)); 653 654 pdata += sizeof(uint16_t); 655 data_left -= sizeof(uint16_t); 656 657 if (data_left < sizeof(sdp_pdu_hdr_t)) { 658 status = SDP_INVALID_SYNTAX; 659 goto done; 660 } 661 662 /* extract the attribute list */ 663 scanned = extract_des(pdata, data_left, &seq, &dtd, SDP_TYPE_ATTRID); 664 if (scanned == -1) { 665 status = SDP_INVALID_SYNTAX; 666 goto done; 667 } 668 pdata += scanned; 669 data_left -= scanned; 670 671 plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen); 672 mlen = scanned + sizeof(uint32_t) + sizeof(uint16_t) + 1; 673 /* ensure we don't read past buffer */ 674 if (plen < mlen || plen != mlen + *(uint8_t *)pdata) { 675 status = SDP_INVALID_PDU_SIZE; 676 goto done; 677 } 678 679 /* 680 * if continuation state exists, attempt 681 * to get rsp remainder from cache, else send error 682 */ 683 if (sdp_cstate_get(pdata, data_left, &cstate) < 0) { 684 status = SDP_INVALID_SYNTAX; 685 goto done; 686 } 687 688 SDPDBG("SvcRecHandle : 0x%x", handle); 689 SDPDBG("max_rsp_size : %d", max_rsp_size); 690 691 /* 692 * Check that max_rsp_size is within valid range 693 * a minimum size of 0x0007 has to be used for data field 694 */ 695 if (max_rsp_size < 0x0007) { 696 status = SDP_INVALID_SYNTAX; 697 goto done; 698 } 699 700 /* 701 * Calculate Attribute size acording to MTU 702 * We can send only (MTU - sizeof(sdp_pdu_hdr_t) - sizeof(sdp_cont_state_t)) 703 */ 704 max_rsp_size = MIN(max_rsp_size, req->mtu - sizeof(sdp_pdu_hdr_t) - 705 sizeof(uint32_t) - SDP_CONT_STATE_SIZE - sizeof(uint16_t)); 706 707 /* pull header for AttributeList byte count */ 708 buf->data += sizeof(uint16_t); 709 buf->buf_size -= sizeof(uint16_t); 710 711 if (cstate) { 712 sdp_buf_t *pCache = sdp_get_cached_rsp(cstate); 713 714 SDPDBG("Obtained cached rsp : %p", pCache); 715 716 if (pCache) { 717 short sent = MIN(max_rsp_size, pCache->data_size - cstate->cStateValue.maxBytesSent); 718 pResponse = pCache->data; 719 memcpy(buf->data, pResponse + cstate->cStateValue.maxBytesSent, sent); 720 buf->data_size += sent; 721 cstate->cStateValue.maxBytesSent += sent; 722 723 SDPDBG("Response size : %d sending now : %d bytes sent so far : %d", 724 pCache->data_size, sent, cstate->cStateValue.maxBytesSent); 725 if (cstate->cStateValue.maxBytesSent == pCache->data_size) 726 cstate_size = sdp_set_cstate_pdu(buf, NULL); 727 else 728 cstate_size = sdp_set_cstate_pdu(buf, cstate); 729 } else { 730 status = SDP_INVALID_CSTATE; 731 error("NULL cache buffer and non-NULL continuation state"); 732 } 733 } else { 734 sdp_record_t *rec = sdp_record_find(handle); 735 status = extract_attrs(rec, seq, buf); 736 if (buf->data_size > max_rsp_size) { 737 sdp_cont_state_t newState; 738 739 memset((char *)&newState, 0, sizeof(sdp_cont_state_t)); 740 newState.timestamp = sdp_cstate_alloc_buf(buf); 741 /* 742 * Reset the buffer size to the maximum expected and 743 * set the sdp_cont_state_t 744 */ 745 SDPDBG("Creating continuation state of size : %d", buf->data_size); 746 buf->data_size = max_rsp_size; 747 newState.cStateValue.maxBytesSent = max_rsp_size; 748 cstate_size = sdp_set_cstate_pdu(buf, &newState); 749 } else { 750 if (buf->data_size == 0) 751 sdp_append_to_buf(buf, 0, 0); 752 cstate_size = sdp_set_cstate_pdu(buf, NULL); 753 } 754 } 755 756 /* push header */ 757 buf->data -= sizeof(uint16_t); 758 buf->buf_size += sizeof(uint16_t); 759 760 done: 761 free(cstate); 762 if (seq) 763 sdp_list_free(seq, free); 764 if (status) 765 return status; 766 767 /* set attribute list byte count */ 768 bt_put_unaligned(htons(buf->data_size - cstate_size), (uint16_t *)buf->data); 769 buf->data_size += sizeof(uint16_t); 770 return 0; 771 } 772 773 /* 774 * combined service search and attribute extraction 775 */ 776 static int service_search_attr_req(sdp_req_t *req, sdp_buf_t *buf) 777 { 778 int status = 0, plen, totscanned; 779 uint8_t *pdata, *pResponse = NULL; 780 unsigned int max; 781 int scanned, rsp_count = 0; 782 sdp_list_t *pattern = NULL, *seq = NULL, *svcList; 783 sdp_cont_state_t *cstate = NULL; 784 short cstate_size = 0; 785 uint8_t dtd = 0; 786 sdp_buf_t tmpbuf; 787 size_t data_left; 788 789 tmpbuf.data = NULL; 790 pdata = req->buf + sizeof(sdp_pdu_hdr_t); 791 data_left = req->len - sizeof(sdp_pdu_hdr_t); 792 scanned = extract_des(pdata, data_left, &pattern, &dtd, SDP_TYPE_UUID); 793 if (scanned == -1) { 794 status = SDP_INVALID_SYNTAX; 795 goto done; 796 } 797 totscanned = scanned; 798 799 SDPDBG("Bytes scanned: %d", scanned); 800 801 pdata += scanned; 802 data_left -= scanned; 803 804 if (data_left < sizeof(uint16_t)) { 805 status = SDP_INVALID_SYNTAX; 806 goto done; 807 } 808 809 max = ntohs(bt_get_unaligned((uint16_t *)pdata)); 810 811 pdata += sizeof(uint16_t); 812 data_left -= sizeof(uint16_t); 813 814 SDPDBG("Max Attr expected: %d", max); 815 816 if (data_left < sizeof(sdp_pdu_hdr_t)) { 817 status = SDP_INVALID_SYNTAX; 818 goto done; 819 } 820 821 /* extract the attribute list */ 822 scanned = extract_des(pdata, data_left, &seq, &dtd, SDP_TYPE_ATTRID); 823 if (scanned == -1) { 824 status = SDP_INVALID_SYNTAX; 825 goto done; 826 } 827 828 pdata += scanned; 829 data_left -= scanned; 830 831 totscanned += scanned + sizeof(uint16_t) + 1; 832 833 plen = ntohs(((sdp_pdu_hdr_t *)(req->buf))->plen); 834 if (plen < totscanned || plen != totscanned + *(uint8_t *)pdata) { 835 status = SDP_INVALID_PDU_SIZE; 836 goto done; 837 } 838 839 /* 840 * if continuation state exists attempt 841 * to get rsp remainder from cache, else send error 842 */ 843 if (sdp_cstate_get(pdata, data_left, &cstate) < 0) { 844 status = SDP_INVALID_SYNTAX; 845 goto done; 846 } 847 848 svcList = sdp_get_record_list(); 849 850 tmpbuf.data = malloc(USHRT_MAX); 851 tmpbuf.data_size = 0; 852 tmpbuf.buf_size = USHRT_MAX; 853 memset(tmpbuf.data, 0, USHRT_MAX); 854 855 /* 856 * Calculate Attribute size acording to MTU 857 * We can send only (MTU - sizeof(sdp_pdu_hdr_t) - sizeof(sdp_cont_state_t)) 858 */ 859 max = MIN(max, req->mtu - sizeof(sdp_pdu_hdr_t) - SDP_CONT_STATE_SIZE - sizeof(uint16_t)); 860 861 /* pull header for AttributeList byte count */ 862 buf->data += sizeof(uint16_t); 863 buf->buf_size -= sizeof(uint16_t); 864 865 if (cstate == NULL) { 866 /* no continuation state -> create new response */ 867 sdp_list_t *p; 868 for (p = svcList; p; p = p->next) { 869 sdp_record_t *rec = p->data; 870 if (sdp_match_uuid(pattern, rec->pattern) > 0 && 871 sdp_check_access(rec->handle, &req->device)) { 872 rsp_count++; 873 status = extract_attrs(rec, seq, &tmpbuf); 874 875 SDPDBG("Response count : %d", rsp_count); 876 SDPDBG("Local PDU size : %d", tmpbuf.data_size); 877 if (status) { 878 SDPDBG("Extract attr from record returns err"); 879 break; 880 } 881 if (buf->data_size + tmpbuf.data_size < buf->buf_size) { 882 /* to be sure no relocations */ 883 sdp_append_to_buf(buf, tmpbuf.data, tmpbuf.data_size); 884 tmpbuf.data_size = 0; 885 memset(tmpbuf.data, 0, USHRT_MAX); 886 } else { 887 error("Relocation needed"); 888 break; 889 } 890 SDPDBG("Net PDU size : %d", buf->data_size); 891 } 892 } 893 if (buf->data_size > max) { 894 sdp_cont_state_t newState; 895 896 memset((char *)&newState, 0, sizeof(sdp_cont_state_t)); 897 newState.timestamp = sdp_cstate_alloc_buf(buf); 898 /* 899 * Reset the buffer size to the maximum expected and 900 * set the sdp_cont_state_t 901 */ 902 buf->data_size = max; 903 newState.cStateValue.maxBytesSent = max; 904 cstate_size = sdp_set_cstate_pdu(buf, &newState); 905 } else 906 cstate_size = sdp_set_cstate_pdu(buf, NULL); 907 } else { 908 /* continuation State exists -> get from cache */ 909 sdp_buf_t *pCache = sdp_get_cached_rsp(cstate); 910 if (pCache) { 911 uint16_t sent = MIN(max, pCache->data_size - cstate->cStateValue.maxBytesSent); 912 pResponse = pCache->data; 913 memcpy(buf->data, pResponse + cstate->cStateValue.maxBytesSent, sent); 914 buf->data_size += sent; 915 cstate->cStateValue.maxBytesSent += sent; 916 if (cstate->cStateValue.maxBytesSent == pCache->data_size) 917 cstate_size = sdp_set_cstate_pdu(buf, NULL); 918 else 919 cstate_size = sdp_set_cstate_pdu(buf, cstate); 920 } else { 921 status = SDP_INVALID_CSTATE; 922 SDPDBG("Non-null continuation state, but null cache buffer"); 923 } 924 } 925 926 if (!rsp_count && !cstate) { 927 /* found nothing */ 928 buf->data_size = 0; 929 sdp_append_to_buf(buf, tmpbuf.data, tmpbuf.data_size); 930 sdp_set_cstate_pdu(buf, NULL); 931 } 932 933 /* push header */ 934 buf->data -= sizeof(uint16_t); 935 buf->buf_size += sizeof(uint16_t); 936 937 if (!status) { 938 /* set attribute list byte count */ 939 bt_put_unaligned(htons(buf->data_size - cstate_size), (uint16_t *)buf->data); 940 buf->data_size += sizeof(uint16_t); 941 } 942 943 done: 944 free(cstate); 945 free(tmpbuf.data); 946 if (pattern) 947 sdp_list_free(pattern, free); 948 if (seq) 949 sdp_list_free(seq, free); 950 return status; 951 } 952 953 /* 954 * Top level request processor. Calls the appropriate processing 955 * function based on request type. Handles service registration 956 * client requests also. 957 */ 958 static void process_request(sdp_req_t *req) 959 { 960 sdp_pdu_hdr_t *reqhdr = (sdp_pdu_hdr_t *)req->buf; 961 sdp_pdu_hdr_t *rsphdr; 962 sdp_buf_t rsp; 963 uint8_t *buf = malloc(USHRT_MAX); 964 int status = SDP_INVALID_SYNTAX; 965 966 memset(buf, 0, USHRT_MAX); 967 rsp.data = buf + sizeof(sdp_pdu_hdr_t); 968 rsp.data_size = 0; 969 rsp.buf_size = USHRT_MAX - sizeof(sdp_pdu_hdr_t); 970 rsphdr = (sdp_pdu_hdr_t *)buf; 971 972 if (ntohs(reqhdr->plen) != req->len - sizeof(sdp_pdu_hdr_t)) { 973 status = SDP_INVALID_PDU_SIZE; 974 goto send_rsp; 975 } 976 switch (reqhdr->pdu_id) { 977 case SDP_SVC_SEARCH_REQ: 978 SDPDBG("Got a svc srch req"); 979 status = service_search_req(req, &rsp); 980 rsphdr->pdu_id = SDP_SVC_SEARCH_RSP; 981 break; 982 case SDP_SVC_ATTR_REQ: 983 SDPDBG("Got a svc attr req"); 984 status = service_attr_req(req, &rsp); 985 rsphdr->pdu_id = SDP_SVC_ATTR_RSP; 986 break; 987 case SDP_SVC_SEARCH_ATTR_REQ: 988 SDPDBG("Got a svc srch attr req"); 989 status = service_search_attr_req(req, &rsp); 990 rsphdr->pdu_id = SDP_SVC_SEARCH_ATTR_RSP; 991 break; 992 /* Following requests are allowed only for local connections */ 993 case SDP_SVC_REGISTER_REQ: 994 SDPDBG("Service register request"); 995 if (req->local) { 996 status = service_register_req(req, &rsp); 997 rsphdr->pdu_id = SDP_SVC_REGISTER_RSP; 998 } 999 break; 1000 case SDP_SVC_UPDATE_REQ: 1001 SDPDBG("Service update request"); 1002 if (req->local) { 1003 status = service_update_req(req, &rsp); 1004 rsphdr->pdu_id = SDP_SVC_UPDATE_RSP; 1005 } 1006 break; 1007 case SDP_SVC_REMOVE_REQ: 1008 SDPDBG("Service removal request"); 1009 if (req->local) { 1010 status = service_remove_req(req, &rsp); 1011 rsphdr->pdu_id = SDP_SVC_REMOVE_RSP; 1012 } 1013 break; 1014 default: 1015 error("Unknown PDU ID : 0x%x received", reqhdr->pdu_id); 1016 status = SDP_INVALID_SYNTAX; 1017 break; 1018 } 1019 1020 send_rsp: 1021 if (status) { 1022 rsphdr->pdu_id = SDP_ERROR_RSP; 1023 bt_put_unaligned(htons(status), (uint16_t *)rsp.data); 1024 rsp.data_size = sizeof(uint16_t); 1025 } 1026 1027 SDPDBG("Sending rsp. status %d", status); 1028 1029 rsphdr->tid = reqhdr->tid; 1030 rsphdr->plen = htons(rsp.data_size); 1031 1032 /* point back to the real buffer start and set the real rsp length */ 1033 rsp.data_size += sizeof(sdp_pdu_hdr_t); 1034 rsp.data = buf; 1035 1036 /* stream the rsp PDU */ 1037 if (send(req->sock, rsp.data, rsp.data_size, 0) < 0) 1038 error("send: %s (%d)", strerror(errno), errno); 1039 1040 SDPDBG("Bytes Sent : %d", sent); 1041 1042 free(rsp.data); 1043 free(req->buf); 1044 } 1045 1046 void handle_request(int sk, uint8_t *data, int len) 1047 { 1048 struct sockaddr_l2 sa; 1049 socklen_t size; 1050 sdp_req_t req; 1051 1052 size = sizeof(sa); 1053 if (getpeername(sk, (struct sockaddr *) &sa, &size) < 0) { 1054 error("getpeername: %s", strerror(errno)); 1055 return; 1056 } 1057 1058 if (sa.l2_family == AF_BLUETOOTH) { 1059 struct l2cap_options lo; 1060 1061 memset(&lo, 0, sizeof(lo)); 1062 size = sizeof(lo); 1063 1064 if (getsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &lo, &size) < 0) { 1065 error("getsockopt: %s", strerror(errno)); 1066 return; 1067 } 1068 1069 bacpy(&req.bdaddr, &sa.l2_bdaddr); 1070 req.mtu = lo.omtu; 1071 req.local = 0; 1072 memset(&sa, 0, sizeof(sa)); 1073 size = sizeof(sa); 1074 1075 if (getsockname(sk, (struct sockaddr *) &sa, &size) < 0) { 1076 error("getsockname: %s", strerror(errno)); 1077 return; 1078 } 1079 1080 bacpy(&req.device, &sa.l2_bdaddr); 1081 } else { 1082 bacpy(&req.device, BDADDR_ANY); 1083 bacpy(&req.bdaddr, BDADDR_LOCAL); 1084 req.mtu = 2048; 1085 req.local = 1; 1086 } 1087 1088 req.sock = sk; 1089 req.buf = data; 1090 req.len = len; 1091 1092 process_request(&req); 1093 } 1094
