1 /* 2 * Copyright (C) 2014 BlueKitchen GmbH 3 * 4 * Redistribution and use in source and binary forms, with or without 5 * modification, are permitted provided that the following conditions 6 * are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright 9 * notice, this list of conditions and the following disclaimer. 10 * 2. Redistributions in binary form must reproduce the above copyright 11 * notice, this list of conditions and the following disclaimer in the 12 * documentation and/or other materials provided with the distribution. 13 * 3. Neither the name of the copyright holders nor the names of 14 * contributors may be used to endorse or promote products derived 15 * from this software without specific prior written permission. 16 * 4. Any redistribution, use, or modification is done solely for 17 * personal benefit and not for any commercial purpose or for 18 * monetary gain. 19 * 20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL MATTHIAS 24 * RINGWALD OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31 * SUCH DAMAGE. 32 * 33 * Please inquire about commercial licensing options at 34 * [email protected] 35 * 36 */ 37 38 /* 39 * sdp_client.c 40 */ 41 42 #include "btstack_config.h" 43 #include "classic/sdp_client.h" 44 45 #include "hci_cmd.h" 46 47 #include "l2cap.h" 48 #include "classic/sdp_server.h" 49 #include "classic/sdp_util.h" 50 #include "btstack_debug.h" 51 52 // Types SDP Parser - Data Element stream helper 53 typedef enum { 54 GET_LIST_LENGTH = 1, 55 GET_RECORD_LENGTH, 56 GET_ATTRIBUTE_ID_HEADER_LENGTH, 57 GET_ATTRIBUTE_ID, 58 GET_ATTRIBUTE_VALUE_LENGTH, 59 GET_ATTRIBUTE_VALUE 60 } sdp_parser_state_t; 61 62 // Types SDP Client 63 typedef enum { 64 INIT, W4_CONNECT, W2_SEND, W4_RESPONSE, QUERY_COMPLETE 65 } sdp_client_state_t; 66 67 68 // Prototypes SDP Parser 69 void sdp_parser_init(btstack_packet_handler_t callback); 70 void sdp_parser_handle_chunk(uint8_t * data, uint16_t size); 71 void sdp_parser_handle_done(uint8_t status); 72 void sdp_parser_init_service_attribute_search(void); 73 void sdp_parser_init_service_search(void); 74 void sdp_parser_handle_service_search(uint8_t * data, uint16_t total_count, uint16_t record_handle_count); 75 76 // Prototypes SDP Client 77 void sdp_client_reset(void); 78 void sdp_client_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size); 79 static uint16_t sdp_client_setup_service_search_attribute_request(uint8_t * data); 80 #ifdef ENABLE_SDP_EXTRA_QUERIES 81 static uint16_t sdp_client_setup_service_search_request(uint8_t * data); 82 static uint16_t sdp_client_setup_service_attribute_request(uint8_t * data); 83 static void sdp_client_parse_service_search_response(uint8_t* packet); 84 static void sdp_client_parse_service_attribute_response(uint8_t* packet); 85 static uint32_t serviceRecordHandle; 86 static uint32_t record_handle; 87 #endif 88 89 // State DES Parser 90 static de_state_t de_header_state; 91 92 // State SDP Parser 93 static sdp_parser_state_t state = GET_LIST_LENGTH; 94 static uint16_t attribute_id = 0; 95 static uint16_t attribute_bytes_received = 0; 96 static uint16_t attribute_bytes_delivered = 0; 97 static uint16_t list_offset = 0; 98 static uint16_t list_size; 99 static uint16_t record_offset = 0; 100 static uint16_t record_size; 101 static uint16_t attribute_value_size; 102 static int record_counter = 0; 103 static btstack_packet_handler_t sdp_parser_callback; 104 105 // State SDP Client 106 static uint16_t mtu; 107 static uint16_t sdp_cid = 0x40; 108 static const uint8_t * service_search_pattern; 109 static const uint8_t * attribute_id_list; 110 static uint16_t transactionID = 0; 111 static uint8_t continuationState[16]; 112 static uint8_t continuationStateLen; 113 static sdp_client_state_t sdp_client_state = INIT; 114 static SDP_PDU_ID_t PDU_ID = SDP_Invalid; 115 116 // DES Parser 117 void de_state_init(de_state_t * de_state){ 118 de_state->in_state_GET_DE_HEADER_LENGTH = 1; 119 de_state->addon_header_bytes = 0; 120 de_state->de_size = 0; 121 de_state->de_offset = 0; 122 } 123 124 int de_state_size(uint8_t eventByte, de_state_t *de_state){ 125 if (de_state->in_state_GET_DE_HEADER_LENGTH){ 126 de_state->addon_header_bytes = de_get_header_size(&eventByte) - 1; 127 de_state->de_size = 0; 128 de_state->de_offset = 0; 129 130 if (de_state->addon_header_bytes == 0){ 131 de_state->de_size = de_get_data_size(&eventByte); 132 if (de_state->de_size == 0) { 133 log_error(" ERROR: ID size is zero"); 134 } 135 // log_info("Data element payload is %d bytes.", de_state->de_size); 136 return 1; 137 } 138 de_state->in_state_GET_DE_HEADER_LENGTH = 0; 139 return 0; 140 } 141 142 if (de_state->addon_header_bytes > 0){ 143 de_state->de_size = (de_state->de_size << 8) | eventByte; 144 de_state->addon_header_bytes--; 145 } 146 if (de_state->addon_header_bytes > 0) return 0; 147 // log_info("Data element payload is %d bytes.", de_state->de_size); 148 de_state->in_state_GET_DE_HEADER_LENGTH = 1; 149 return 1; 150 } 151 152 // SDP Parser 153 static void sdp_parser_emit_value_byte(uint8_t event_byte){ 154 uint8_t event[11]; 155 event[0] = SDP_EVENT_QUERY_ATTRIBUTE_VALUE; 156 event[1] = 9; 157 little_endian_store_16(event, 2, record_counter); 158 little_endian_store_16(event, 4, attribute_id); 159 little_endian_store_16(event, 6, attribute_value_size); 160 little_endian_store_16(event, 8, attribute_bytes_delivered); 161 event[10] = event_byte; 162 (*sdp_parser_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 163 } 164 165 static void sdp_parser_process_byte(uint8_t eventByte){ 166 // count all bytes 167 list_offset++; 168 record_offset++; 169 170 // log_info(" parse BYTE_RECEIVED %02x", eventByte); 171 switch(state){ 172 case GET_LIST_LENGTH: 173 if (!de_state_size(eventByte, &de_header_state)) break; 174 list_offset = de_header_state.de_offset; 175 list_size = de_header_state.de_size; 176 // log_info("parser: List offset %u, list size %u", list_offset, list_size); 177 178 record_counter = 0; 179 state = GET_RECORD_LENGTH; 180 break; 181 182 case GET_RECORD_LENGTH: 183 // check size 184 if (!de_state_size(eventByte, &de_header_state)) break; 185 // log_info("parser: Record payload is %d bytes.", de_header_state.de_size); 186 record_offset = de_header_state.de_offset; 187 record_size = de_header_state.de_size; 188 state = GET_ATTRIBUTE_ID_HEADER_LENGTH; 189 break; 190 191 case GET_ATTRIBUTE_ID_HEADER_LENGTH: 192 if (!de_state_size(eventByte, &de_header_state)) break; 193 attribute_id = 0; 194 log_info("ID data is stored in %d bytes.", (int) de_header_state.de_size); 195 state = GET_ATTRIBUTE_ID; 196 break; 197 198 case GET_ATTRIBUTE_ID: 199 attribute_id = (attribute_id << 8) | eventByte; 200 de_header_state.de_size--; 201 if (de_header_state.de_size > 0) break; 202 log_info("parser: Attribute ID: %04x.", attribute_id); 203 204 state = GET_ATTRIBUTE_VALUE_LENGTH; 205 attribute_bytes_received = 0; 206 attribute_bytes_delivered = 0; 207 attribute_value_size = 0; 208 de_state_init(&de_header_state); 209 break; 210 211 case GET_ATTRIBUTE_VALUE_LENGTH: 212 attribute_bytes_received++; 213 sdp_parser_emit_value_byte(eventByte); 214 attribute_bytes_delivered++; 215 if (!de_state_size(eventByte, &de_header_state)) break; 216 217 attribute_value_size = de_header_state.de_size + attribute_bytes_received; 218 219 state = GET_ATTRIBUTE_VALUE; 220 break; 221 222 case GET_ATTRIBUTE_VALUE: 223 attribute_bytes_received++; 224 sdp_parser_emit_value_byte(eventByte); 225 attribute_bytes_delivered++; 226 // log_info("paser: attribute_bytes_received %u, attribute_value_size %u", attribute_bytes_received, attribute_value_size); 227 228 if (attribute_bytes_received < attribute_value_size) break; 229 // log_info("parser: Record offset %u, record size %u", record_offset, record_size); 230 if (record_offset != record_size){ 231 state = GET_ATTRIBUTE_ID_HEADER_LENGTH; 232 // log_info("Get next attribute"); 233 break; 234 } 235 record_offset = 0; 236 // log_info("parser: List offset %u, list size %u", list_offset, list_size); 237 238 if (list_size > 0 && list_offset != list_size){ 239 record_counter++; 240 state = GET_RECORD_LENGTH; 241 log_info("parser: END_OF_RECORD"); 242 break; 243 } 244 list_offset = 0; 245 de_state_init(&de_header_state); 246 state = GET_LIST_LENGTH; 247 record_counter = 0; 248 log_info("parser: END_OF_RECORD & DONE"); 249 break; 250 default: 251 break; 252 } 253 } 254 255 void sdp_parser_init(btstack_packet_handler_t callback){ 256 // init 257 sdp_parser_callback = callback; 258 de_state_init(&de_header_state); 259 state = GET_LIST_LENGTH; 260 list_offset = 0; 261 record_offset = 0; 262 record_counter = 0; 263 } 264 265 void sdp_parser_handle_chunk(uint8_t * data, uint16_t size){ 266 int i; 267 for (i=0;i<size;i++){ 268 sdp_parser_process_byte(data[i]); 269 } 270 } 271 272 #ifdef ENABLE_SDP_EXTRA_QUERIES 273 void sdp_parser_init_service_attribute_search(void){ 274 // init 275 de_state_init(&de_header_state); 276 state = GET_RECORD_LENGTH; 277 list_offset = 0; 278 record_offset = 0; 279 record_counter = 0; 280 } 281 282 void sdp_parser_init_service_search(void){ 283 record_offset = 0; 284 } 285 286 void sdp_parser_handle_service_search(uint8_t * data, uint16_t total_count, uint16_t record_handle_count){ 287 int i; 288 for (i=0;i<record_handle_count;i++){ 289 record_handle = big_endian_read_32(data, i*4); 290 record_counter++; 291 uint8_t event[10]; 292 event[0] = SDP_EVENT_QUERY_SERVICE_RECORD_HANDLE; 293 event[1] = 8; 294 little_endian_store_16(event, 2, total_count); 295 little_endian_store_16(event, 4, record_counter); 296 little_endian_store_32(event, 6, record_handle); 297 (*sdp_parser_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 298 } 299 } 300 #endif 301 302 void sdp_parser_handle_done(uint8_t status){ 303 uint8_t event[3]; 304 event[0] = SDP_EVENT_QUERY_COMPLETE; 305 event[1] = 1; 306 event[2] = status; 307 (*sdp_parser_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 308 } 309 310 static void sdp_client_emit_busy(btstack_packet_handler_t callback){ 311 log_error("sdp_client query initiated when not ready"); 312 uint8_t event[] = { SDP_EVENT_QUERY_COMPLETE, 1, SDP_QUERY_BUSY}; 313 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 314 } 315 316 // SDP Client 317 318 // TODO: inline if not needed (des(des)) 319 320 static int sdp_client_can_send_now(uint16_t channel){ 321 if (sdp_client_state != W2_SEND) return 0; 322 if (!l2cap_can_send_packet_now(channel)) return 0; 323 return 1; 324 } 325 326 static void sdp_client_parse_attribute_lists(uint8_t* packet, uint16_t length){ 327 sdp_parser_handle_chunk(packet, length); 328 } 329 330 331 static void sdp_client_send_request(uint16_t channel){ 332 l2cap_reserve_packet_buffer(); 333 uint8_t * data = l2cap_get_outgoing_buffer(); 334 uint16_t request_len = 0; 335 336 switch (PDU_ID){ 337 #ifdef ENABLE_SDP_EXTRA_QUERIES 338 case SDP_ServiceSearchResponse: 339 request_len = sdp_client_setup_service_search_request(data); 340 break; 341 case SDP_ServiceAttributeResponse: 342 request_len = sdp_client_setup_service_attribute_request(data); 343 break; 344 #endif 345 case SDP_ServiceSearchAttributeResponse: 346 request_len = sdp_client_setup_service_search_attribute_request(data); 347 break; 348 default: 349 log_error("SDP Client sdp_client_send_request :: PDU ID invalid. %u", PDU_ID); 350 return; 351 } 352 353 // prevent re-entrance 354 sdp_client_state = W4_RESPONSE; 355 int err = l2cap_send_prepared(channel, request_len); 356 // l2cap_send_prepared shouldn't have failed as l2ap_can_send_packet_now() was true 357 switch (err){ 358 case 0: 359 log_debug("l2cap_send() -> OK"); 360 PDU_ID = SDP_Invalid; 361 break; 362 case BTSTACK_ACL_BUFFERS_FULL: 363 sdp_client_state = W2_SEND; 364 log_info("l2cap_send() ->BTSTACK_ACL_BUFFERS_FULL"); 365 break; 366 default: 367 sdp_client_state = W2_SEND; 368 log_error("l2cap_send() -> err %d", err); 369 break; 370 } 371 } 372 373 374 static void sdp_client_parse_service_search_attribute_response(uint8_t* packet){ 375 uint16_t offset = 3; 376 uint16_t parameterLength = big_endian_read_16(packet,offset); 377 offset+=2; 378 // AttributeListByteCount <= mtu 379 uint16_t attributeListByteCount = big_endian_read_16(packet,offset); 380 offset+=2; 381 382 if (attributeListByteCount > mtu){ 383 log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount."); 384 return; 385 } 386 387 // AttributeLists 388 sdp_client_parse_attribute_lists(packet+offset, attributeListByteCount); 389 offset+=attributeListByteCount; 390 391 continuationStateLen = packet[offset]; 392 offset++; 393 394 if (continuationStateLen > 16){ 395 log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in continuation state exceedes 16."); 396 return; 397 } 398 memcpy(continuationState, packet+offset, continuationStateLen); 399 offset+=continuationStateLen; 400 401 if (parameterLength != offset - 5){ 402 log_error("Error parsing ServiceSearchAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset); 403 } 404 } 405 406 void sdp_client_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 407 // uint16_t handle; 408 if (packet_type == L2CAP_DATA_PACKET){ 409 uint16_t responseTransactionID = big_endian_read_16(packet,1); 410 if ( responseTransactionID != transactionID){ 411 log_error("Missmatching transaction ID, expected %u, found %u.", transactionID, responseTransactionID); 412 return; 413 } 414 415 if (packet[0] != SDP_ServiceSearchAttributeResponse 416 && packet[0] != SDP_ServiceSearchResponse 417 && packet[0] != SDP_ServiceAttributeResponse){ 418 log_error("Not a valid PDU ID, expected %u, %u or %u, found %u.", SDP_ServiceSearchResponse, 419 SDP_ServiceAttributeResponse, SDP_ServiceSearchAttributeResponse, packet[0]); 420 return; 421 } 422 423 PDU_ID = (SDP_PDU_ID_t)packet[0]; 424 log_info("SDP Client :: PDU ID. %u ,%u", PDU_ID, packet[0]); 425 switch (PDU_ID){ 426 #ifdef ENABLE_SDP_EXTRA_QUERIES 427 case SDP_ServiceSearchResponse: 428 sdp_client_parse_service_search_response(packet); 429 break; 430 case SDP_ServiceAttributeResponse: 431 sdp_client_parse_service_attribute_response(packet); 432 break; 433 #endif 434 case SDP_ServiceSearchAttributeResponse: 435 sdp_client_parse_service_search_attribute_response(packet); 436 break; 437 default: 438 log_error("SDP Client :: PDU ID invalid. %u ,%u", PDU_ID, packet[0]); 439 return; 440 } 441 442 // continuation set or DONE? 443 if (continuationStateLen == 0){ 444 log_info("SDP Client Query DONE! "); 445 sdp_client_state = QUERY_COMPLETE; 446 l2cap_disconnect(sdp_cid, 0); 447 // sdp_parser_handle_done(0); 448 return; 449 } 450 // prepare next request and send 451 sdp_client_state = W2_SEND; 452 if (sdp_client_can_send_now(sdp_cid)) sdp_client_send_request(sdp_cid); 453 return; 454 } 455 456 if (packet_type != HCI_EVENT_PACKET) return; 457 458 switch(packet[0]){ 459 case L2CAP_EVENT_TIMEOUT_CHECK: 460 log_info("sdp client: L2CAP_EVENT_TIMEOUT_CHECK"); 461 break; 462 case L2CAP_EVENT_CHANNEL_OPENED: 463 if (sdp_client_state != W4_CONNECT) break; 464 // data: event (8), len(8), status (8), address(48), handle (16), psm (16), local_cid(16), remote_cid (16), local_mtu(16), remote_mtu(16) 465 if (packet[2]) { 466 log_error("SDP Client Connection failed."); 467 sdp_parser_handle_done(packet[2]); 468 break; 469 } 470 sdp_cid = channel; 471 mtu = little_endian_read_16(packet, 17); 472 // handle = little_endian_read_16(packet, 9); 473 log_info("SDP Client Connected, cid %x, mtu %u.", sdp_cid, mtu); 474 475 sdp_client_state = W2_SEND; 476 if (sdp_client_can_send_now(sdp_cid)) sdp_client_send_request(sdp_cid); 477 478 break; 479 case L2CAP_EVENT_CAN_SEND_NOW: 480 if (sdp_client_can_send_now(sdp_cid)) sdp_client_send_request(sdp_cid); 481 break; 482 case L2CAP_EVENT_CHANNEL_CLOSED: { 483 if (sdp_cid != little_endian_read_16(packet, 2)) { 484 // log_info("Received L2CAP_EVENT_CHANNEL_CLOSED for cid %x, current cid %x\n", little_endian_read_16(packet, 2),sdp_cid); 485 break; 486 } 487 log_info("SDP Client disconnected."); 488 uint8_t status = sdp_client_state == QUERY_COMPLETE ? 0 : SDP_QUERY_INCOMPLETE; 489 sdp_client_state = INIT; 490 sdp_parser_handle_done(status); 491 break; 492 } 493 default: 494 break; 495 } 496 } 497 498 499 static uint16_t sdp_client_setup_service_search_attribute_request(uint8_t * data){ 500 501 uint16_t offset = 0; 502 transactionID++; 503 // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest; 504 data[offset++] = SDP_ServiceSearchAttributeRequest; 505 // uint16_t transactionID 506 big_endian_store_16(data, offset, transactionID); 507 offset += 2; 508 509 // param legnth 510 offset += 2; 511 512 // parameters: 513 // Service_search_pattern - DES (min 1 UUID, max 12) 514 uint16_t service_search_pattern_len = de_get_len(service_search_pattern); 515 memcpy(data + offset, service_search_pattern, service_search_pattern_len); 516 offset += service_search_pattern_len; 517 518 // MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu 519 big_endian_store_16(data, offset, mtu); 520 offset += 2; 521 522 // AttibuteIDList 523 uint16_t attribute_id_list_len = de_get_len(attribute_id_list); 524 memcpy(data + offset, attribute_id_list, attribute_id_list_len); 525 offset += attribute_id_list_len; 526 527 // ContinuationState - uint8_t number of cont. bytes N<=16 528 data[offset++] = continuationStateLen; 529 // - N-bytes previous response from server 530 memcpy(data + offset, continuationState, continuationStateLen); 531 offset += continuationStateLen; 532 533 // uint16_t paramLength 534 big_endian_store_16(data, 3, offset - 5); 535 536 return offset; 537 } 538 539 #ifdef ENABLE_SDP_EXTRA_QUERIES 540 void sdp_client_parse_service_record_handle_list(uint8_t* packet, uint16_t total_count, uint16_t current_count){ 541 sdp_parser_handle_service_search(packet, total_count, current_count); 542 } 543 544 static uint16_t sdp_client_setup_service_search_request(uint8_t * data){ 545 uint16_t offset = 0; 546 transactionID++; 547 // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest; 548 data[offset++] = SDP_ServiceSearchRequest; 549 // uint16_t transactionID 550 big_endian_store_16(data, offset, transactionID); 551 offset += 2; 552 553 // param legnth 554 offset += 2; 555 556 // parameters: 557 // Service_search_pattern - DES (min 1 UUID, max 12) 558 uint16_t service_search_pattern_len = de_get_len(service_search_pattern); 559 memcpy(data + offset, service_search_pattern, service_search_pattern_len); 560 offset += service_search_pattern_len; 561 562 // MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu 563 big_endian_store_16(data, offset, mtu); 564 offset += 2; 565 566 // ContinuationState - uint8_t number of cont. bytes N<=16 567 data[offset++] = continuationStateLen; 568 // - N-bytes previous response from server 569 memcpy(data + offset, continuationState, continuationStateLen); 570 offset += continuationStateLen; 571 572 // uint16_t paramLength 573 big_endian_store_16(data, 3, offset - 5); 574 575 return offset; 576 } 577 578 579 static uint16_t sdp_client_setup_service_attribute_request(uint8_t * data){ 580 581 uint16_t offset = 0; 582 transactionID++; 583 // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest; 584 data[offset++] = SDP_ServiceAttributeRequest; 585 // uint16_t transactionID 586 big_endian_store_16(data, offset, transactionID); 587 offset += 2; 588 589 // param legnth 590 offset += 2; 591 592 // parameters: 593 // ServiceRecordHandle 594 big_endian_store_32(data, offset, serviceRecordHandle); 595 offset += 4; 596 597 // MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu 598 big_endian_store_16(data, offset, mtu); 599 offset += 2; 600 601 // AttibuteIDList 602 uint16_t attribute_id_list_len = de_get_len(attribute_id_list); 603 memcpy(data + offset, attribute_id_list, attribute_id_list_len); 604 offset += attribute_id_list_len; 605 606 // ContinuationState - uint8_t number of cont. bytes N<=16 607 data[offset++] = continuationStateLen; 608 // - N-bytes previous response from server 609 memcpy(data + offset, continuationState, continuationStateLen); 610 offset += continuationStateLen; 611 612 // uint16_t paramLength 613 big_endian_store_16(data, 3, offset - 5); 614 615 return offset; 616 } 617 618 static void sdp_client_parse_service_search_response(uint8_t* packet){ 619 uint16_t offset = 3; 620 uint16_t parameterLength = big_endian_read_16(packet,offset); 621 offset+=2; 622 623 uint16_t totalServiceRecordCount = big_endian_read_16(packet,offset); 624 offset+=2; 625 626 uint16_t currentServiceRecordCount = big_endian_read_16(packet,offset); 627 offset+=2; 628 if (currentServiceRecordCount > totalServiceRecordCount){ 629 log_error("CurrentServiceRecordCount is larger then TotalServiceRecordCount."); 630 return; 631 } 632 633 sdp_client_parse_service_record_handle_list(packet+offset, totalServiceRecordCount, currentServiceRecordCount); 634 offset+=(currentServiceRecordCount * 4); 635 636 continuationStateLen = packet[offset]; 637 offset++; 638 if (continuationStateLen > 16){ 639 log_error("Error parsing ServiceSearchResponse: Number of bytes in continuation state exceedes 16."); 640 return; 641 } 642 memcpy(continuationState, packet+offset, continuationStateLen); 643 offset+=continuationStateLen; 644 645 if (parameterLength != offset - 5){ 646 log_error("Error parsing ServiceSearchResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset); 647 } 648 } 649 650 static void sdp_client_parse_service_attribute_response(uint8_t* packet){ 651 uint16_t offset = 3; 652 uint16_t parameterLength = big_endian_read_16(packet,offset); 653 offset+=2; 654 655 // AttributeListByteCount <= mtu 656 uint16_t attributeListByteCount = big_endian_read_16(packet,offset); 657 offset+=2; 658 659 if (attributeListByteCount > mtu){ 660 log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount."); 661 return; 662 } 663 664 // AttributeLists 665 sdp_client_parse_attribute_lists(packet+offset, attributeListByteCount); 666 offset+=attributeListByteCount; 667 668 continuationStateLen = packet[offset]; 669 offset++; 670 671 if (continuationStateLen > 16){ 672 log_error("Error parsing ServiceAttributeResponse: Number of bytes in continuation state exceedes 16."); 673 return; 674 } 675 memcpy(continuationState, packet+offset, continuationStateLen); 676 offset+=continuationStateLen; 677 678 if (parameterLength != offset - 5){ 679 log_error("Error parsing ServiceAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset); 680 } 681 } 682 #endif 683 684 // for testing only 685 void sdp_client_reset(void){ 686 sdp_client_state = INIT; 687 } 688 689 // Public API 690 691 int sdp_client_ready(void){ 692 return sdp_client_state == INIT; 693 } 694 695 void sdp_client_query(btstack_packet_handler_t callback, bd_addr_t remote, const uint8_t * des_service_search_pattern, const uint8_t * des_attribute_id_list){ 696 if (!sdp_client_ready()) { 697 sdp_client_emit_busy(callback); 698 return; 699 } 700 sdp_parser_init(callback); 701 service_search_pattern = des_service_search_pattern; 702 attribute_id_list = des_attribute_id_list; 703 continuationStateLen = 0; 704 PDU_ID = SDP_ServiceSearchAttributeResponse; 705 706 sdp_client_state = W4_CONNECT; 707 l2cap_create_channel(sdp_client_packet_handler, remote, PSM_SDP, l2cap_max_mtu(), NULL); 708 } 709 710 #ifdef ENABLE_SDP_EXTRA_QUERIES 711 void sdp_client_service_attribute_search(btstack_packet_handler_t callback, bd_addr_t remote, uint32_t search_service_record_handle, uint8_t * des_attribute_id_list){ 712 if (!sdp_client_ready()) { 713 sdp_client_emit_busy(callback); 714 return; 715 } 716 sdp_parser_init(callback); 717 serviceRecordHandle = search_service_record_handle; 718 attribute_id_list = des_attribute_id_list; 719 continuationStateLen = 0; 720 PDU_ID = SDP_ServiceAttributeResponse; 721 722 sdp_client_state = W4_CONNECT; 723 l2cap_create_channel(sdp_client_packet_handler, remote, PSM_SDP, l2cap_max_mtu(), NULL); 724 } 725 726 void sdp_client_service_search(btstack_packet_handler_t callback, bd_addr_t remote, uint8_t * des_service_search_pattern){ 727 if (!sdp_client_ready()) { 728 sdp_client_emit_busy(callback); 729 return; 730 } 731 sdp_parser_init(callback); 732 service_search_pattern = des_service_search_pattern; 733 continuationStateLen = 0; 734 PDU_ID = SDP_ServiceSearchResponse; 735 736 sdp_client_state = W4_CONNECT; 737 l2cap_create_channel(sdp_client_packet_handler, remote, PSM_SDP, l2cap_max_mtu(), NULL); 738 } 739 #endif 740 741