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