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 #endif 86 87 static uint8_t des_attributeIDList[] = { 0x35, 0x05, 0x0A, 0x00, 0x01, 0xff, 0xff}; // Attribute: 0x0001 - 0x0100 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 #ifdef ENABLE_SDP_EXTRA_QUERIES 116 static uint32_t serviceRecordHandle; 117 static uint32_t record_handle; 118 #endif 119 120 // DES Parser 121 void de_state_init(de_state_t * de_state){ 122 de_state->in_state_GET_DE_HEADER_LENGTH = 1; 123 de_state->addon_header_bytes = 0; 124 de_state->de_size = 0; 125 de_state->de_offset = 0; 126 } 127 128 int de_state_size(uint8_t eventByte, de_state_t *de_state){ 129 if (de_state->in_state_GET_DE_HEADER_LENGTH){ 130 de_state->addon_header_bytes = de_get_header_size(&eventByte) - 1; 131 de_state->de_size = 0; 132 de_state->de_offset = 0; 133 134 if (de_state->addon_header_bytes == 0){ 135 de_state->de_size = de_get_data_size(&eventByte); 136 if (de_state->de_size == 0) { 137 log_error(" ERROR: ID size is zero"); 138 } 139 // log_info("Data element payload is %d bytes.", de_state->de_size); 140 return 1; 141 } 142 de_state->in_state_GET_DE_HEADER_LENGTH = 0; 143 return 0; 144 } 145 146 if (de_state->addon_header_bytes > 0){ 147 de_state->de_size = (de_state->de_size << 8) | eventByte; 148 de_state->addon_header_bytes--; 149 } 150 if (de_state->addon_header_bytes > 0) return 0; 151 // log_info("Data element payload is %d bytes.", de_state->de_size); 152 de_state->in_state_GET_DE_HEADER_LENGTH = 1; 153 return 1; 154 } 155 156 // SDP Parser 157 static void sdp_parser_emit_value_byte(uint8_t event_byte){ 158 uint8_t event[11]; 159 event[0] = SDP_EVENT_QUERY_ATTRIBUTE_VALUE; 160 event[1] = 9; 161 little_endian_store_16(event, 2, record_counter); 162 little_endian_store_16(event, 4, attribute_id); 163 little_endian_store_16(event, 6, attribute_value_size); 164 little_endian_store_16(event, 8, attribute_bytes_delivered); 165 event[10] = event_byte; 166 (*sdp_parser_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 167 } 168 169 static void sdp_parser_process_byte(uint8_t eventByte){ 170 // count all bytes 171 list_offset++; 172 record_offset++; 173 174 // log_info(" parse BYTE_RECEIVED %02x", eventByte); 175 switch(state){ 176 case GET_LIST_LENGTH: 177 if (!de_state_size(eventByte, &de_header_state)) break; 178 list_offset = de_header_state.de_offset; 179 list_size = de_header_state.de_size; 180 // log_info("parser: List offset %u, list size %u", list_offset, list_size); 181 182 record_counter = 0; 183 state = GET_RECORD_LENGTH; 184 break; 185 186 case GET_RECORD_LENGTH: 187 // check size 188 if (!de_state_size(eventByte, &de_header_state)) break; 189 // log_info("parser: Record payload is %d bytes.", de_header_state.de_size); 190 record_offset = de_header_state.de_offset; 191 record_size = de_header_state.de_size; 192 state = GET_ATTRIBUTE_ID_HEADER_LENGTH; 193 break; 194 195 case GET_ATTRIBUTE_ID_HEADER_LENGTH: 196 if (!de_state_size(eventByte, &de_header_state)) break; 197 attribute_id = 0; 198 log_info("ID data is stored in %d bytes.", (int) de_header_state.de_size); 199 state = GET_ATTRIBUTE_ID; 200 break; 201 202 case GET_ATTRIBUTE_ID: 203 attribute_id = (attribute_id << 8) | eventByte; 204 de_header_state.de_size--; 205 if (de_header_state.de_size > 0) break; 206 log_info("parser: Attribute ID: %04x.", attribute_id); 207 208 state = GET_ATTRIBUTE_VALUE_LENGTH; 209 attribute_bytes_received = 0; 210 attribute_bytes_delivered = 0; 211 attribute_value_size = 0; 212 de_state_init(&de_header_state); 213 break; 214 215 case GET_ATTRIBUTE_VALUE_LENGTH: 216 attribute_bytes_received++; 217 sdp_parser_emit_value_byte(eventByte); 218 attribute_bytes_delivered++; 219 if (!de_state_size(eventByte, &de_header_state)) break; 220 221 attribute_value_size = de_header_state.de_size + attribute_bytes_received; 222 223 state = GET_ATTRIBUTE_VALUE; 224 break; 225 226 case GET_ATTRIBUTE_VALUE: 227 attribute_bytes_received++; 228 sdp_parser_emit_value_byte(eventByte); 229 attribute_bytes_delivered++; 230 // log_info("paser: attribute_bytes_received %u, attribute_value_size %u", attribute_bytes_received, attribute_value_size); 231 232 if (attribute_bytes_received < attribute_value_size) break; 233 // log_info("parser: Record offset %u, record size %u", record_offset, record_size); 234 if (record_offset != record_size){ 235 state = GET_ATTRIBUTE_ID_HEADER_LENGTH; 236 // log_info("Get next attribute"); 237 break; 238 } 239 record_offset = 0; 240 // log_info("parser: List offset %u, list size %u", list_offset, list_size); 241 242 if (list_size > 0 && list_offset != list_size){ 243 record_counter++; 244 state = GET_RECORD_LENGTH; 245 log_info("parser: END_OF_RECORD"); 246 break; 247 } 248 list_offset = 0; 249 de_state_init(&de_header_state); 250 state = GET_LIST_LENGTH; 251 record_counter = 0; 252 log_info("parser: END_OF_RECORD & DONE"); 253 break; 254 default: 255 break; 256 } 257 } 258 259 void sdp_parser_init(btstack_packet_handler_t callback){ 260 // init 261 sdp_parser_callback = callback; 262 de_state_init(&de_header_state); 263 state = GET_LIST_LENGTH; 264 list_offset = 0; 265 record_offset = 0; 266 record_counter = 0; 267 } 268 269 void sdp_parser_handle_chunk(uint8_t * data, uint16_t size){ 270 int i; 271 for (i=0;i<size;i++){ 272 sdp_parser_process_byte(data[i]); 273 } 274 } 275 276 #ifdef ENABLE_SDP_EXTRA_QUERIES 277 void sdp_parser_init_service_attribute_search(void){ 278 // init 279 de_state_init(&de_header_state); 280 state = GET_RECORD_LENGTH; 281 list_offset = 0; 282 record_offset = 0; 283 record_counter = 0; 284 } 285 286 void sdp_parser_init_service_search(void){ 287 record_offset = 0; 288 } 289 290 void sdp_parser_handle_service_search(uint8_t * data, uint16_t total_count, uint16_t record_handle_count){ 291 int i; 292 for (i=0;i<record_handle_count;i++){ 293 record_handle = big_endian_read_32(data, i*4); 294 record_counter++; 295 uint8_t event[10]; 296 event[0] = SDP_EVENT_QUERY_SERVICE_RECORD_HANDLE; 297 event[1] = 8; 298 little_endian_store_16(event, 2, total_count); 299 little_endian_store_16(event, 4, record_counter); 300 little_endian_store_32(event, 6, record_handle); 301 (*sdp_parser_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 302 } 303 } 304 #endif 305 306 void sdp_parser_handle_done(uint8_t status){ 307 uint8_t event[3]; 308 event[0] = SDP_EVENT_QUERY_COMPLETE; 309 event[1] = 1; 310 event[2] = status; 311 (*sdp_parser_callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 312 } 313 314 static void sdp_client_emit_busy(btstack_packet_handler_t callback){ 315 log_error("sdp_client query initiated when not ready"); 316 uint8_t event[] = { SDP_EVENT_QUERY_COMPLETE, 1, SDP_QUERY_BUSY}; 317 (*callback)(HCI_EVENT_PACKET, 0, event, sizeof(event)); 318 } 319 320 // SDP Client 321 322 // TODO: inline if not needed (des(des)) 323 324 static int sdp_client_can_send_now(uint16_t channel){ 325 if (sdp_client_state != W2_SEND) return 0; 326 if (!l2cap_can_send_packet_now(channel)) return 0; 327 return 1; 328 } 329 330 static void sdp_client_parse_attribute_lists(uint8_t* packet, uint16_t length){ 331 sdp_parser_handle_chunk(packet, length); 332 } 333 334 335 static void sdp_client_send_request(uint16_t channel){ 336 l2cap_reserve_packet_buffer(); 337 uint8_t * data = l2cap_get_outgoing_buffer(); 338 uint16_t request_len = 0; 339 340 switch (PDU_ID){ 341 #ifdef ENABLE_SDP_EXTRA_QUERIES 342 case SDP_ServiceSearchResponse: 343 request_len = sdp_client_setup_service_search_request(data); 344 break; 345 case SDP_ServiceAttributeResponse: 346 request_len = sdp_client_setup_service_attribute_request(data); 347 break; 348 #endif 349 case SDP_ServiceSearchAttributeResponse: 350 request_len = sdp_client_setup_service_search_attribute_request(data); 351 break; 352 default: 353 log_error("SDP Client sdp_client_send_request :: PDU ID invalid. %u", PDU_ID); 354 return; 355 } 356 357 // prevent re-entrance 358 sdp_client_state = W4_RESPONSE; 359 int err = l2cap_send_prepared(channel, request_len); 360 // l2cap_send_prepared shouldn't have failed as l2ap_can_send_packet_now() was true 361 switch (err){ 362 case 0: 363 log_debug("l2cap_send() -> OK"); 364 PDU_ID = SDP_Invalid; 365 break; 366 case BTSTACK_ACL_BUFFERS_FULL: 367 sdp_client_state = W2_SEND; 368 log_info("l2cap_send() ->BTSTACK_ACL_BUFFERS_FULL"); 369 break; 370 default: 371 sdp_client_state = W2_SEND; 372 log_error("l2cap_send() -> err %d", err); 373 break; 374 } 375 } 376 377 378 static void sdp_client_parse_service_search_attribute_response(uint8_t* packet){ 379 uint16_t offset = 3; 380 uint16_t parameterLength = big_endian_read_16(packet,offset); 381 offset+=2; 382 // AttributeListByteCount <= mtu 383 uint16_t attributeListByteCount = big_endian_read_16(packet,offset); 384 offset+=2; 385 386 if (attributeListByteCount > mtu){ 387 log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount."); 388 return; 389 } 390 391 // AttributeLists 392 sdp_client_parse_attribute_lists(packet+offset, attributeListByteCount); 393 offset+=attributeListByteCount; 394 395 continuationStateLen = packet[offset]; 396 offset++; 397 398 if (continuationStateLen > 16){ 399 log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in continuation state exceedes 16."); 400 return; 401 } 402 memcpy(continuationState, packet+offset, continuationStateLen); 403 offset+=continuationStateLen; 404 405 if (parameterLength != offset - 5){ 406 log_error("Error parsing ServiceSearchAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset); 407 } 408 } 409 410 void sdp_client_packet_handler(uint8_t packet_type, uint16_t channel, uint8_t *packet, uint16_t size){ 411 // uint16_t handle; 412 if (packet_type == L2CAP_DATA_PACKET){ 413 uint16_t responseTransactionID = big_endian_read_16(packet,1); 414 if ( responseTransactionID != transactionID){ 415 log_error("Missmatching transaction ID, expected %u, found %u.", transactionID, responseTransactionID); 416 return; 417 } 418 419 if (packet[0] != SDP_ServiceSearchAttributeResponse 420 && packet[0] != SDP_ServiceSearchResponse 421 && packet[0] != SDP_ServiceAttributeResponse){ 422 log_error("Not a valid PDU ID, expected %u, %u or %u, found %u.", SDP_ServiceSearchResponse, 423 SDP_ServiceAttributeResponse, SDP_ServiceSearchAttributeResponse, packet[0]); 424 return; 425 } 426 427 PDU_ID = (SDP_PDU_ID_t)packet[0]; 428 log_info("SDP Client :: PDU ID. %u ,%u", PDU_ID, packet[0]); 429 switch (PDU_ID){ 430 #ifdef ENABLE_SDP_EXTRA_QUERIES 431 case SDP_ServiceSearchResponse: 432 sdp_client_parse_service_search_response(packet); 433 break; 434 case SDP_ServiceAttributeResponse: 435 sdp_client_parse_service_attribute_response(packet); 436 break; 437 #endif 438 case SDP_ServiceSearchAttributeResponse: 439 sdp_client_parse_service_search_attribute_response(packet); 440 break; 441 default: 442 log_error("SDP Client :: PDU ID invalid. %u ,%u", PDU_ID, packet[0]); 443 return; 444 } 445 446 // continuation set or DONE? 447 if (continuationStateLen == 0){ 448 log_info("SDP Client Query DONE! "); 449 sdp_client_state = QUERY_COMPLETE; 450 l2cap_disconnect(sdp_cid, 0); 451 // sdp_parser_handle_done(0); 452 return; 453 } 454 // prepare next request and send 455 sdp_client_state = W2_SEND; 456 if (sdp_client_can_send_now(sdp_cid)) sdp_client_send_request(sdp_cid); 457 return; 458 } 459 460 if (packet_type != HCI_EVENT_PACKET) return; 461 462 switch(packet[0]){ 463 case L2CAP_EVENT_TIMEOUT_CHECK: 464 log_info("sdp client: L2CAP_EVENT_TIMEOUT_CHECK"); 465 break; 466 case L2CAP_EVENT_CHANNEL_OPENED: 467 if (sdp_client_state != W4_CONNECT) break; 468 // 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) 469 if (packet[2]) { 470 log_error("SDP Client Connection failed."); 471 sdp_parser_handle_done(packet[2]); 472 break; 473 } 474 sdp_cid = channel; 475 mtu = little_endian_read_16(packet, 17); 476 // handle = little_endian_read_16(packet, 9); 477 log_info("SDP Client Connected, cid %x, mtu %u.", sdp_cid, mtu); 478 479 sdp_client_state = W2_SEND; 480 if (sdp_client_can_send_now(sdp_cid)) sdp_client_send_request(sdp_cid); 481 482 break; 483 case L2CAP_EVENT_CAN_SEND_NOW: 484 if (sdp_client_can_send_now(sdp_cid)) sdp_client_send_request(sdp_cid); 485 break; 486 case L2CAP_EVENT_CHANNEL_CLOSED: { 487 if (sdp_cid != little_endian_read_16(packet, 2)) { 488 // log_info("Received L2CAP_EVENT_CHANNEL_CLOSED for cid %x, current cid %x\n", little_endian_read_16(packet, 2),sdp_cid); 489 break; 490 } 491 log_info("SDP Client disconnected."); 492 uint8_t status = sdp_client_state == QUERY_COMPLETE ? 0 : SDP_QUERY_INCOMPLETE; 493 sdp_client_state = INIT; 494 sdp_parser_handle_done(status); 495 break; 496 } 497 default: 498 break; 499 } 500 } 501 502 503 static uint16_t sdp_client_setup_service_search_attribute_request(uint8_t * data){ 504 505 uint16_t offset = 0; 506 transactionID++; 507 // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest; 508 data[offset++] = SDP_ServiceSearchAttributeRequest; 509 // uint16_t transactionID 510 big_endian_store_16(data, offset, transactionID); 511 offset += 2; 512 513 // param legnth 514 offset += 2; 515 516 // parameters: 517 // Service_search_pattern - DES (min 1 UUID, max 12) 518 uint16_t service_search_pattern_len = de_get_len(service_search_pattern); 519 memcpy(data + offset, service_search_pattern, service_search_pattern_len); 520 offset += service_search_pattern_len; 521 522 // MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu 523 big_endian_store_16(data, offset, mtu); 524 offset += 2; 525 526 // AttibuteIDList 527 uint16_t attribute_id_list_len = de_get_len(attribute_id_list); 528 memcpy(data + offset, attribute_id_list, attribute_id_list_len); 529 offset += attribute_id_list_len; 530 531 // ContinuationState - uint8_t number of cont. bytes N<=16 532 data[offset++] = continuationStateLen; 533 // - N-bytes previous response from server 534 memcpy(data + offset, continuationState, continuationStateLen); 535 offset += continuationStateLen; 536 537 // uint16_t paramLength 538 big_endian_store_16(data, 3, offset - 5); 539 540 return offset; 541 } 542 543 #ifdef ENABLE_SDP_EXTRA_QUERIES 544 void sdp_client_parse_service_record_handle_list(uint8_t* packet, uint16_t total_count, uint16_t current_count){ 545 sdp_parser_handle_service_search(packet, total_count, current_count); 546 } 547 548 static uint16_t sdp_client_setup_service_search_request(uint8_t * data){ 549 uint16_t offset = 0; 550 transactionID++; 551 // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest; 552 data[offset++] = SDP_ServiceSearchRequest; 553 // uint16_t transactionID 554 big_endian_store_16(data, offset, transactionID); 555 offset += 2; 556 557 // param legnth 558 offset += 2; 559 560 // parameters: 561 // Service_search_pattern - DES (min 1 UUID, max 12) 562 uint16_t service_search_pattern_len = de_get_len(service_search_pattern); 563 memcpy(data + offset, service_search_pattern, service_search_pattern_len); 564 offset += service_search_pattern_len; 565 566 // MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu 567 big_endian_store_16(data, offset, mtu); 568 offset += 2; 569 570 // ContinuationState - uint8_t number of cont. bytes N<=16 571 data[offset++] = continuationStateLen; 572 // - N-bytes previous response from server 573 memcpy(data + offset, continuationState, continuationStateLen); 574 offset += continuationStateLen; 575 576 // uint16_t paramLength 577 big_endian_store_16(data, 3, offset - 5); 578 579 return offset; 580 } 581 582 583 static uint16_t sdp_client_setup_service_attribute_request(uint8_t * data){ 584 585 uint16_t offset = 0; 586 transactionID++; 587 // uint8_t SDP_PDU_ID_t.SDP_ServiceSearchRequest; 588 data[offset++] = SDP_ServiceAttributeRequest; 589 // uint16_t transactionID 590 big_endian_store_16(data, offset, transactionID); 591 offset += 2; 592 593 // param legnth 594 offset += 2; 595 596 // parameters: 597 // ServiceRecordHandle 598 big_endian_store_32(data, offset, serviceRecordHandle); 599 offset += 4; 600 601 // MaximumAttributeByteCount - uint16_t 0x0007 - 0xffff -> mtu 602 big_endian_store_16(data, offset, mtu); 603 offset += 2; 604 605 // AttibuteIDList 606 uint16_t attribute_id_list_len = de_get_len(attribute_id_list); 607 memcpy(data + offset, attribute_id_list, attribute_id_list_len); 608 offset += attribute_id_list_len; 609 610 // ContinuationState - uint8_t number of cont. bytes N<=16 611 data[offset++] = continuationStateLen; 612 // - N-bytes previous response from server 613 memcpy(data + offset, continuationState, continuationStateLen); 614 offset += continuationStateLen; 615 616 // uint16_t paramLength 617 big_endian_store_16(data, 3, offset - 5); 618 619 return offset; 620 } 621 622 static void sdp_client_parse_service_search_response(uint8_t* packet){ 623 uint16_t offset = 3; 624 uint16_t parameterLength = big_endian_read_16(packet,offset); 625 offset+=2; 626 627 uint16_t totalServiceRecordCount = big_endian_read_16(packet,offset); 628 offset+=2; 629 630 uint16_t currentServiceRecordCount = big_endian_read_16(packet,offset); 631 offset+=2; 632 if (currentServiceRecordCount > totalServiceRecordCount){ 633 log_error("CurrentServiceRecordCount is larger then TotalServiceRecordCount."); 634 return; 635 } 636 637 sdp_client_parse_service_record_handle_list(packet+offset, totalServiceRecordCount, currentServiceRecordCount); 638 offset+=(currentServiceRecordCount * 4); 639 640 continuationStateLen = packet[offset]; 641 offset++; 642 if (continuationStateLen > 16){ 643 log_error("Error parsing ServiceSearchResponse: Number of bytes in continuation state exceedes 16."); 644 return; 645 } 646 memcpy(continuationState, packet+offset, continuationStateLen); 647 offset+=continuationStateLen; 648 649 if (parameterLength != offset - 5){ 650 log_error("Error parsing ServiceSearchResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset); 651 } 652 } 653 654 static void sdp_client_parse_service_attribute_response(uint8_t* packet){ 655 uint16_t offset = 3; 656 uint16_t parameterLength = big_endian_read_16(packet,offset); 657 offset+=2; 658 659 // AttributeListByteCount <= mtu 660 uint16_t attributeListByteCount = big_endian_read_16(packet,offset); 661 offset+=2; 662 663 if (attributeListByteCount > mtu){ 664 log_error("Error parsing ServiceSearchAttributeResponse: Number of bytes in found attribute list is larger then the MaximumAttributeByteCount."); 665 return; 666 } 667 668 // AttributeLists 669 sdp_client_parse_attribute_lists(packet+offset, attributeListByteCount); 670 offset+=attributeListByteCount; 671 672 continuationStateLen = packet[offset]; 673 offset++; 674 675 if (continuationStateLen > 16){ 676 log_error("Error parsing ServiceAttributeResponse: Number of bytes in continuation state exceedes 16."); 677 return; 678 } 679 memcpy(continuationState, packet+offset, continuationStateLen); 680 offset+=continuationStateLen; 681 682 if (parameterLength != offset - 5){ 683 log_error("Error parsing ServiceAttributeResponse: wrong size of parameters, number of expected bytes%u, actual number %u.", parameterLength, offset); 684 } 685 } 686 #endif 687 688 // for testing only 689 void sdp_client_reset(void){ 690 sdp_client_state = INIT; 691 } 692 693 // Public API 694 695 int sdp_client_ready(void){ 696 return sdp_client_state == INIT; 697 } 698 699 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){ 700 if (!sdp_client_ready()) { 701 sdp_client_emit_busy(callback); 702 return; 703 } 704 sdp_parser_init(callback); 705 service_search_pattern = des_service_search_pattern; 706 attribute_id_list = des_attribute_id_list; 707 continuationStateLen = 0; 708 PDU_ID = SDP_ServiceSearchAttributeResponse; 709 710 sdp_client_state = W4_CONNECT; 711 l2cap_create_channel(sdp_client_packet_handler, remote, PSM_SDP, l2cap_max_mtu(), NULL); 712 } 713 714 void sdp_client_query_uuid16(btstack_packet_handler_t callback, bd_addr_t remote, uint16_t uuid){ 715 if (!sdp_client_ready()){ 716 sdp_client_emit_busy(callback); 717 return; 718 } 719 uint8_t * service_service_search_pattern = sdp_service_search_pattern_for_uuid16(uuid); 720 sdp_client_query(callback, remote, service_service_search_pattern, des_attributeIDList); 721 } 722 723 void sdp_client_query_uuid128(btstack_packet_handler_t callback, bd_addr_t remote, const uint8_t* uuid){ 724 if (!sdp_client_ready()){ 725 sdp_client_emit_busy(callback); 726 return; 727 } 728 uint8_t * service_service_search_pattern = sdp_service_search_pattern_for_uuid128(uuid); 729 sdp_client_query(callback, remote, service_service_search_pattern, des_attributeIDList); 730 } 731 732 #ifdef ENABLE_SDP_EXTRA_QUERIES 733 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){ 734 if (!sdp_client_ready()) { 735 sdp_client_emit_busy(callback); 736 return; 737 } 738 sdp_parser_init(callback); 739 serviceRecordHandle = search_service_record_handle; 740 attribute_id_list = des_attribute_id_list; 741 continuationStateLen = 0; 742 PDU_ID = SDP_ServiceAttributeResponse; 743 744 sdp_client_state = W4_CONNECT; 745 l2cap_create_channel(sdp_client_packet_handler, remote, PSM_SDP, l2cap_max_mtu(), NULL); 746 } 747 748 void sdp_client_service_search(btstack_packet_handler_t callback, bd_addr_t remote, uint8_t * des_service_search_pattern){ 749 if (!sdp_client_ready()) { 750 sdp_client_emit_busy(callback); 751 return; 752 } 753 sdp_parser_init(callback); 754 service_search_pattern = des_service_search_pattern; 755 continuationStateLen = 0; 756 PDU_ID = SDP_ServiceSearchResponse; 757 758 sdp_client_state = W4_CONNECT; 759 l2cap_create_channel(sdp_client_packet_handler, remote, PSM_SDP, l2cap_max_mtu(), NULL); 760 } 761 #endif 762 763