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 BLUEKITCHEN 24 * GMBH 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 #define BTSTACK_FILE__ "sdp_util.c" 39 40 /* 41 * sdp_util.c 42 */ 43 44 #include "bluetooth.h" 45 #include "btstack_config.h" 46 #include "btstack_debug.h" 47 #include "btstack_util.h" 48 #include "classic/core.h" 49 #include "classic/sdp_util.h" 50 51 #include <stdlib.h> 52 #include <string.h> 53 #include <stdint.h> 54 #include <inttypes.h> // PRIx32 55 56 #ifdef ENABLE_SDP_DES_DUMP 57 #include <stdio.h> 58 #endif 59 60 #ifdef ENABLE_SDP_DES_DUMP 61 // workaround for missing PRIx32 on mspgcc (16-bit MCU) 62 #ifndef PRIx32 63 #warning Using own: #define PRIx32 "lx" 64 #define PRIx32 "lx" 65 #endif 66 // date element type names 67 const char * const type_names[] = { "NIL", "UINT", "INT", "UUID", "STRING", "BOOL", "DES", "DEA", "URL"}; 68 #endif 69 70 static uint8_t des_service_search_pattern_uuid16[] = {0x35, 0x03, 0x19, 0x00, 0x00}; 71 static uint8_t des_service_search_pattern_uuid128[] = { 72 0x35, 0x11, 0x1c, 73 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 74 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; 75 76 // MARK: DataElement getter 77 de_size_t de_get_size_type(const uint8_t *header){ 78 return (de_size_t) (header[0] & 7); 79 } 80 81 de_type_t de_get_element_type(const uint8_t *header){ 82 return (de_type_t) (header[0] >> 3); 83 } 84 85 uint32_t de_get_header_size(const uint8_t * header){ 86 de_size_t de_size = de_get_size_type(header); 87 if (de_size <= DE_SIZE_128) { 88 return 1; 89 } 90 return 1 + (1 << (de_size-DE_SIZE_VAR_8)); 91 } 92 93 uint32_t de_get_data_size(const uint8_t * header){ 94 uint32_t result = 0; 95 de_type_t de_type = de_get_element_type(header); 96 de_size_t de_size = de_get_size_type(header); 97 switch (de_size){ 98 case DE_SIZE_VAR_8: 99 result = header[1]; 100 break; 101 case DE_SIZE_VAR_16: 102 result = big_endian_read_16(header,1); 103 break; 104 case DE_SIZE_VAR_32: 105 result = big_endian_read_32(header,1); 106 break; 107 default: 108 // case DE_SIZE_8: 109 // case DE_SIZE_16: 110 // case DE_SIZE_32: 111 // case DE_SIZE_64: 112 // case DE_SIZE_128: 113 if (de_type == DE_NIL) return 0; 114 return 1 << de_size; 115 } 116 return result; 117 } 118 119 int de_get_len(const uint8_t *header){ 120 return de_get_header_size(header) + de_get_data_size(header); 121 } 122 123 // returns data element length if data element fits in size 124 uint32_t de_get_len_safe(const uint8_t * header, uint32_t size){ 125 if (1 > size) return 0; 126 uint32_t header_size = de_get_header_size(header); 127 if (header_size > size) return 0; 128 uint32_t data_size = de_get_data_size(header); 129 if (data_size > size) return 0; 130 uint32_t de_len = header_size + data_size; 131 if (de_len > size) return 0; 132 return de_len; 133 } 134 135 // @return OK, if UINT16 value was read 136 int de_element_get_uint16(const uint8_t * element, uint16_t * value){ 137 if (de_get_size_type(element) != DE_SIZE_16) return 0; 138 *value = big_endian_read_16(element, de_get_header_size(element)); 139 return 1; 140 } 141 142 // @return: element is valid UUID 143 int de_get_normalized_uuid(uint8_t *uuid128, const uint8_t *element){ 144 de_type_t uuidType = de_get_element_type(element); 145 de_size_t uuidSize = de_get_size_type(element); 146 if (uuidType != DE_UUID) return 0; 147 uint32_t shortUUID; 148 switch (uuidSize){ 149 case DE_SIZE_16: 150 shortUUID = big_endian_read_16(element, 1); 151 break; 152 case DE_SIZE_32: 153 shortUUID = big_endian_read_32(element, 1); 154 break; 155 case DE_SIZE_128: 156 (void)memcpy(uuid128, element + 1, 16); 157 return 1; 158 default: 159 return 0; 160 } 161 uuid_add_bluetooth_prefix(uuid128, shortUUID); 162 return 1; 163 } 164 165 // @return 0 if no UUID16 or UUID32 is present, and UUID32 otherwise 166 uint32_t de_get_uuid32(const uint8_t * element){ 167 uint8_t uuid128[16]; 168 int validUuid128 = de_get_normalized_uuid(uuid128, element); 169 if (!validUuid128) return 0; 170 int hasBlueoothBaseUuid = uuid_has_bluetooth_prefix(uuid128); 171 if (!hasBlueoothBaseUuid) return 0; 172 return big_endian_read_32(uuid128, 0); 173 } 174 175 const uint8_t * de_get_string(const uint8_t * element){ 176 if (de_get_element_type(element) != DE_STRING) return NULL; 177 return &element[de_get_header_size(element)]; 178 } 179 180 // functions to create record 181 static void de_store_descriptor(uint8_t * header, de_type_t type, de_size_t size){ 182 header[0] = (type << 3) | size; 183 } 184 185 void de_store_descriptor_with_len(uint8_t * header, de_type_t type, de_size_t size, uint32_t len){ 186 header[0] = (type << 3) | size; 187 switch (size){ 188 case DE_SIZE_VAR_8: 189 header[1] = len; 190 break; 191 case DE_SIZE_VAR_16: 192 big_endian_store_16(header, 1, len); 193 break; 194 case DE_SIZE_VAR_32: 195 big_endian_store_32(header, 1, len); 196 break; 197 default: 198 break; 199 } 200 } 201 202 // MARK: DataElement creation 203 204 /* starts a new sequence in empty buffer - first call */ 205 void de_create_sequence(uint8_t *header){ 206 de_store_descriptor_with_len( header, DE_DES, DE_SIZE_VAR_16, 0); // DES, 2 Byte Length 207 } 208 209 /* starts a sub-sequence, @return handle for sub-sequence */ 210 uint8_t * de_push_sequence(uint8_t *header){ 211 int element_len = de_get_len(header); 212 de_store_descriptor_with_len(header+element_len, DE_DES, DE_SIZE_VAR_16, 0); // DES, 2 Byte Length 213 return header + element_len; 214 } 215 216 /* closes the current sequence and updates the parent sequence */ 217 void de_pop_sequence(uint8_t * parent, uint8_t * child){ 218 int child_len = de_get_len(child); 219 int data_size_parent = big_endian_read_16(parent,1); 220 big_endian_store_16(parent, 1, data_size_parent + child_len); 221 } 222 223 /* adds a single number value and 16+32 bit UUID to the sequence */ 224 void de_add_number(uint8_t *seq, de_type_t type, de_size_t size, uint32_t value){ 225 int data_size = big_endian_read_16(seq,1); 226 int element_size = 1; // e.g. for DE_TYPE_NIL 227 de_store_descriptor(seq+3+data_size, type, size); 228 switch (size){ 229 case DE_SIZE_8: 230 if (type != DE_NIL){ 231 seq[4+data_size] = value; 232 element_size = 2; 233 } 234 break; 235 case DE_SIZE_16: 236 big_endian_store_16(seq, 4+data_size, value); 237 element_size = 3; 238 break; 239 case DE_SIZE_32: 240 big_endian_store_32(seq, 4+data_size, value); 241 element_size = 5; 242 break; 243 default: 244 break; 245 } 246 big_endian_store_16(seq, 1, data_size+element_size); 247 } 248 249 /* add a single block of data, e.g. as DE_STRING, DE_URL */ 250 void de_add_data( uint8_t *seq, de_type_t type, uint16_t size, uint8_t *data){ 251 int data_size = big_endian_read_16(seq,1); 252 if (size > 0xff) { 253 // use 16-bit lengh information (3 byte header) 254 de_store_descriptor_with_len(seq+3+data_size, type, DE_SIZE_VAR_16, size); 255 data_size += 3; 256 } else { 257 // use 8-bit lengh information (2 byte header) 258 de_store_descriptor_with_len(seq+3+data_size, type, DE_SIZE_VAR_8, size); 259 data_size += 2; 260 } 261 if (size > 0){ 262 (void)memcpy(seq + 3 + data_size, data, size); 263 data_size += size; 264 } 265 big_endian_store_16(seq, 1, data_size); 266 } 267 268 void de_add_uuid128(uint8_t * seq, uint8_t * uuid){ 269 int data_size = big_endian_read_16(seq,1); 270 de_store_descriptor(seq+3+data_size, DE_UUID, DE_SIZE_128); 271 (void)memcpy(seq + 4 + data_size, uuid, 16); 272 big_endian_store_16(seq, 1, data_size+1+16); 273 } 274 275 // MARK: DES iterator 276 bool des_iterator_init(des_iterator_t * it, uint8_t * element){ 277 de_type_t type = de_get_element_type(element); 278 if (type != DE_DES) return false; 279 280 it->element = element; 281 it->pos = de_get_header_size(element); 282 it->length = de_get_len(element); 283 return true; 284 } 285 286 de_type_t des_iterator_get_type (des_iterator_t * it){ 287 return de_get_element_type(&it->element[it->pos]); 288 } 289 290 uint16_t des_iterator_get_size (des_iterator_t * it){ 291 int length = de_get_len(&it->element[it->pos]); 292 int header_size = de_get_header_size(&it->element[it->pos]); 293 return length - header_size; 294 } 295 296 bool des_iterator_has_more(des_iterator_t * it){ 297 return it->pos < it->length; 298 } 299 300 uint8_t * des_iterator_get_element(des_iterator_t * it){ 301 if (!des_iterator_has_more(it)) return NULL; 302 return &it->element[it->pos]; 303 } 304 305 void des_iterator_next(des_iterator_t * it){ 306 int element_len = de_get_len(&it->element[it->pos]); 307 it->pos += element_len; 308 } 309 310 // MARK: DataElementSequence traversal 311 typedef int (*de_traversal_callback_t)(uint8_t * element, de_type_t type, de_size_t size, void *context); 312 static void de_traverse_sequence(uint8_t * element, de_traversal_callback_t handler, void *context){ 313 de_type_t type = de_get_element_type(element); 314 if (type != DE_DES) return; 315 int pos = de_get_header_size(element); 316 int end_pos = de_get_len(element); 317 while (pos < end_pos){ 318 de_type_t elemType = de_get_element_type(element + pos); 319 de_size_t elemSize = de_get_size_type(element + pos); 320 uint8_t done = (*handler)(element + pos, elemType, elemSize, context); 321 if (done) break; 322 pos += de_get_len(element + pos); 323 } 324 } 325 326 // MARK: AttributeList traversal 327 typedef int (*sdp_attribute_list_traversal_callback_t)(uint16_t attributeID, uint8_t * attributeValue, de_type_t type, de_size_t size, void *context); 328 static void sdp_attribute_list_traverse_sequence(uint8_t * element, sdp_attribute_list_traversal_callback_t handler, void *context){ 329 de_type_t type = de_get_element_type(element); 330 if (type != DE_DES) return; 331 int pos = de_get_header_size(element); 332 int end_pos = de_get_len(element); 333 while (pos < end_pos){ 334 de_type_t idType = de_get_element_type(element + pos); 335 de_size_t idSize = de_get_size_type(element + pos); 336 if ( (idType != DE_UINT) || (idSize != DE_SIZE_16) ) break; // wrong type 337 uint16_t attribute_id = big_endian_read_16(element, pos + 1); 338 pos += 3; 339 if (pos >= end_pos) break; // array out of bounds 340 de_type_t valueType = de_get_element_type(element + pos); 341 de_size_t valueSize = de_get_size_type(element + pos); 342 uint8_t done = (*handler)(attribute_id, element + pos, valueType, valueSize, context); 343 if (done) break; 344 pos += de_get_len(element + pos); 345 } 346 } 347 348 // MARK: AttributeID in AttributeIDList 349 // attribute ID in AttributeIDList 350 // context { result, attributeID } 351 struct sdp_context_attributeID_search { 352 int result; 353 uint16_t attributeID; 354 }; 355 static int sdp_traversal_attributeID_search(uint8_t * element, de_type_t type, de_size_t size, void *my_context){ 356 struct sdp_context_attributeID_search * context = (struct sdp_context_attributeID_search *) my_context; 357 if (type != DE_UINT) return 0; 358 switch (size) { 359 case DE_SIZE_16: 360 if (big_endian_read_16(element, 1) == context->attributeID) { 361 context->result = 1; 362 return 1; 363 } 364 break; 365 case DE_SIZE_32: 366 if ((big_endian_read_16(element, 1) <= context->attributeID) 367 && (context->attributeID <= big_endian_read_16(element, 3))) { 368 context->result = 1; 369 return 1; 370 } 371 break; 372 default: 373 break; 374 } 375 return 0; 376 } 377 378 int sdp_attribute_list_constains_id(uint8_t *attributeIDList, uint16_t attributeID){ 379 struct sdp_context_attributeID_search attributeID_search; 380 attributeID_search.result = 0; 381 attributeID_search.attributeID = attributeID; 382 de_traverse_sequence(attributeIDList, sdp_traversal_attributeID_search, &attributeID_search); 383 return attributeID_search.result; 384 } 385 386 // MARK: Append Attributes for AttributeIDList 387 // pre: buffer contains DES with 2 byte length field 388 struct sdp_context_append_attributes { 389 uint8_t * buffer; 390 uint16_t startOffset; // offset of when to start copying 391 uint16_t maxBytes; 392 uint16_t usedBytes; 393 uint8_t *attributeIDList; 394 }; 395 396 static int sdp_traversal_append_attributes(uint16_t attributeID, uint8_t * attributeValue, de_type_t de_type, de_size_t de_size, void *my_context){ 397 UNUSED(de_type); 398 UNUSED(de_size); 399 struct sdp_context_append_attributes * context = (struct sdp_context_append_attributes *) my_context; 400 if (sdp_attribute_list_constains_id(context->attributeIDList, attributeID)) { 401 // DES_HEADER(3) + DES_DATA + (UINT16(3) + attribute) 402 uint16_t data_size = big_endian_read_16(context->buffer, 1); 403 int attribute_len = de_get_len(attributeValue); 404 if ((3 + data_size + (3 + attribute_len)) <= context->maxBytes) { 405 // copy Attribute 406 de_add_number(context->buffer, DE_UINT, DE_SIZE_16, attributeID); 407 data_size += 3; // 3 bytes 408 (void)memcpy(context->buffer + 3 + data_size, attributeValue, 409 attribute_len); 410 big_endian_store_16(context->buffer,1,data_size+attribute_len); 411 } else { 412 // not enought space left -> continue with previous element 413 return 1; 414 } 415 } 416 return 0; 417 } 418 419 // maxBytes: maximal size of data element sequence 420 uint16_t sdp_append_attributes_in_attributeIDList(uint8_t *record, uint8_t *attributeIDList, uint16_t startOffset, uint16_t maxBytes, uint8_t *buffer){ 421 struct sdp_context_append_attributes context; 422 context.buffer = buffer; 423 context.maxBytes = maxBytes; 424 context.usedBytes = 0; 425 context.startOffset = startOffset; 426 context.attributeIDList = attributeIDList; 427 sdp_attribute_list_traverse_sequence(record, sdp_traversal_append_attributes, &context); 428 return context.usedBytes; 429 } 430 431 // MARK: Filter attributes that match attribute list from startOffset and a max nr bytes 432 struct sdp_context_filter_attributes { 433 uint8_t * buffer; 434 uint16_t startOffset; // offset of when to start copying 435 uint16_t maxBytes; 436 uint16_t usedBytes; 437 uint8_t *attributeIDList; 438 int complete; 439 }; 440 441 // copy data with given start offset and max bytes, returns OK if all data has been copied 442 static int spd_append_range(struct sdp_context_filter_attributes* context, uint16_t len, uint8_t *data){ 443 int ok = 1; 444 uint16_t remainder_len = len - context->startOffset; 445 if (context->maxBytes < remainder_len){ 446 remainder_len = context->maxBytes; 447 ok = 0; 448 } 449 (void)memcpy(context->buffer, &data[context->startOffset], remainder_len); 450 context->usedBytes += remainder_len; 451 context->buffer += remainder_len; 452 context->maxBytes -= remainder_len; 453 context->startOffset = 0; 454 return ok; 455 } 456 457 static int sdp_traversal_filter_attributes(uint16_t attributeID, uint8_t * attributeValue, de_type_t de_type, de_size_t de_size, void *my_context){ 458 UNUSED(de_type); 459 UNUSED(de_size); 460 461 struct sdp_context_filter_attributes * context = (struct sdp_context_filter_attributes *) my_context; 462 463 if (!sdp_attribute_list_constains_id(context->attributeIDList, attributeID)) return 0; 464 465 // { Attribute ID (Descriptor, big endian 16-bit ID), AttributeValue (data)} 466 467 // handle Attribute ID 468 if (context->startOffset >= 3){ 469 context->startOffset -= 3; 470 } else { 471 uint8_t idBuffer[3]; 472 de_store_descriptor(idBuffer, DE_UINT, DE_SIZE_16); 473 big_endian_store_16(idBuffer,1,attributeID); 474 475 int ok = spd_append_range(context, 3, idBuffer); 476 if (!ok) { 477 context->complete = 0; 478 return 1; 479 } 480 } 481 482 // handle Attribute Value 483 int attribute_len = de_get_len(attributeValue); 484 if (context->startOffset >= attribute_len) { 485 context->startOffset -= attribute_len; 486 return 0; 487 } 488 489 int ok = spd_append_range(context, attribute_len, attributeValue); 490 if (!ok) { 491 context->complete = 0; 492 return 1; 493 } 494 return 0; 495 } 496 497 int sdp_filter_attributes_in_attributeIDList(uint8_t *record, uint8_t *attributeIDList, uint16_t startOffset, uint16_t maxBytes, uint16_t *usedBytes, uint8_t *buffer){ 498 499 struct sdp_context_filter_attributes context; 500 context.buffer = buffer; 501 context.maxBytes = maxBytes; 502 context.usedBytes = 0; 503 context.startOffset = startOffset; 504 context.attributeIDList = attributeIDList; 505 context.complete = 1; 506 507 sdp_attribute_list_traverse_sequence(record, sdp_traversal_filter_attributes, &context); 508 509 *usedBytes = context.usedBytes; 510 return context.complete; 511 } 512 513 // MARK: Get sum of attributes matching attribute list 514 struct sdp_context_get_filtered_size { 515 uint8_t *attributeIDList; 516 uint16_t size; 517 }; 518 519 static int sdp_traversal_get_filtered_size(uint16_t attributeID, uint8_t * attributeValue, de_type_t de_type, de_size_t de_size, void *my_context){ 520 UNUSED(de_type); 521 UNUSED(de_size); 522 523 struct sdp_context_get_filtered_size * context = (struct sdp_context_get_filtered_size *) my_context; 524 if (sdp_attribute_list_constains_id(context->attributeIDList, attributeID)) { 525 context->size += 3 + de_get_len(attributeValue); 526 } 527 return 0; 528 } 529 530 int spd_get_filtered_size(uint8_t *record, uint8_t *attributeIDList){ 531 struct sdp_context_get_filtered_size context; 532 context.size = 0; 533 context.attributeIDList = attributeIDList; 534 sdp_attribute_list_traverse_sequence(record, sdp_traversal_get_filtered_size, &context); 535 return context.size; 536 } 537 538 // MARK: Get AttributeValue for AttributeID 539 // find attribute (ELEMENT) by ID 540 struct sdp_context_attribute_by_id { 541 uint16_t attributeID; 542 uint8_t * attributeValue; 543 }; 544 static int sdp_traversal_attribute_by_id(uint16_t attributeID, uint8_t * attributeValue, de_type_t de_type, de_size_t de_size, void *my_context){ 545 UNUSED(de_type); 546 UNUSED(de_size); 547 548 struct sdp_context_attribute_by_id * context = (struct sdp_context_attribute_by_id *) my_context; 549 if (attributeID == context->attributeID) { 550 context->attributeValue = attributeValue; 551 return 1; 552 } 553 return 0; 554 } 555 556 uint8_t * sdp_get_attribute_value_for_attribute_id(uint8_t * record, uint16_t attributeID){ 557 struct sdp_context_attribute_by_id context; 558 context.attributeValue = NULL; 559 context.attributeID = attributeID; 560 sdp_attribute_list_traverse_sequence(record, sdp_traversal_attribute_by_id, &context); 561 return context.attributeValue; 562 } 563 564 // MARK: Set AttributeValue for AttributeID 565 struct sdp_context_set_attribute_for_id { 566 uint16_t attributeID; 567 uint32_t attributeValue; 568 uint8_t attributeFound; 569 }; 570 static int sdp_traversal_set_attribute_for_id(uint16_t attributeID, uint8_t * attributeValue, de_type_t attributeType, de_size_t size, void *my_context){ 571 struct sdp_context_set_attribute_for_id * context = (struct sdp_context_set_attribute_for_id *) my_context; 572 if (attributeID == context->attributeID) { 573 context->attributeFound = 1; 574 switch (size){ 575 case DE_SIZE_8: 576 if (attributeType != DE_NIL){ 577 attributeValue[1] = context->attributeValue; 578 } 579 break; 580 case DE_SIZE_16: 581 big_endian_store_16(attributeValue, 1, context->attributeValue); 582 break; 583 case DE_SIZE_32: 584 big_endian_store_32(attributeValue, 1, context->attributeValue); 585 break; 586 // Might want to support STRINGS to, copy upto original length 587 default: 588 break; 589 } 590 return 1; 591 } 592 return 0; 593 } 594 uint8_t sdp_set_attribute_value_for_attribute_id(uint8_t * record, uint16_t attributeID, uint32_t value){ 595 struct sdp_context_set_attribute_for_id context; 596 context.attributeID = attributeID; 597 context.attributeValue = value; 598 context.attributeFound = 0; 599 sdp_attribute_list_traverse_sequence(record, sdp_traversal_set_attribute_for_id, &context); 600 return context.attributeFound; 601 } 602 603 // MARK: ServiceRecord contains UUID 604 // service record contains UUID 605 // context { normalizedUUID } 606 struct sdp_context_contains_uuid128 { 607 uint8_t * uuid128; 608 int result; 609 }; 610 int sdp_record_contains_UUID128(uint8_t *record, uint8_t *uuid128); 611 static int sdp_traversal_contains_UUID128(uint8_t * element, de_type_t type, de_size_t de_size, void *my_context){ 612 UNUSED(de_size); 613 614 struct sdp_context_contains_uuid128 * context = (struct sdp_context_contains_uuid128 *) my_context; 615 uint8_t normalizedUUID[16]; 616 if (type == DE_UUID){ 617 uint8_t uuidOK = de_get_normalized_uuid(normalizedUUID, element); 618 context->result = uuidOK && (memcmp(context->uuid128, normalizedUUID, 16) == 0); 619 } 620 if (type == DE_DES){ 621 context->result = sdp_record_contains_UUID128(element, context->uuid128); 622 } 623 return context->result; 624 } 625 int sdp_record_contains_UUID128(uint8_t *record, uint8_t *uuid128){ 626 struct sdp_context_contains_uuid128 context; 627 context.uuid128 = uuid128; 628 context.result = 0; 629 de_traverse_sequence(record, sdp_traversal_contains_UUID128, &context); 630 return context.result; 631 } 632 633 // MARK: ServiceRecord matches SearchServicePattern 634 // if UUID in searchServicePattern is not found in record => false 635 // context { result, record } 636 struct sdp_context_match_pattern { 637 uint8_t * record; 638 int result; 639 }; 640 641 int sdp_traversal_match_pattern(uint8_t * element, de_type_t de_type, de_size_t de_size, void *my_context){ 642 UNUSED(de_type); 643 UNUSED(de_size); 644 645 struct sdp_context_match_pattern * context = (struct sdp_context_match_pattern *) my_context; 646 uint8_t normalizedUUID[16]; 647 uint8_t uuidOK = de_get_normalized_uuid(normalizedUUID, element); 648 if (!uuidOK || !sdp_record_contains_UUID128(context->record, normalizedUUID)){ 649 context->result = 0; 650 return 1; 651 } 652 return 0; 653 } 654 int sdp_record_matches_service_search_pattern(uint8_t *record, uint8_t *serviceSearchPattern){ 655 struct sdp_context_match_pattern context; 656 context.record = record; 657 context.result = 1; 658 de_traverse_sequence(serviceSearchPattern, sdp_traversal_match_pattern, &context); 659 return context.result; 660 } 661 662 // MARK: Dump DataElement 663 // context { indent } 664 #ifdef ENABLE_SDP_DES_DUMP 665 static int de_traversal_dump_data(uint8_t * element, de_type_t de_type, de_size_t de_size, void *my_context){ 666 int indent = *(int*) my_context; 667 int i; 668 for (i=0; i<indent;i++) printf(" "); 669 unsigned int pos = de_get_header_size(element); 670 unsigned int end_pos = de_get_len(element); 671 printf("type %5s (%u), element len %2u ", type_names[de_type], de_type, end_pos); 672 if (de_type == DE_DES) { 673 printf("\n"); 674 indent++; 675 de_traverse_sequence(element, de_traversal_dump_data, (void *)&indent); 676 } else if (de_type == DE_UUID && de_size == DE_SIZE_128) { 677 printf(", value: %s\n", uuid128_to_str(element+1)); 678 } else if (de_type == DE_STRING) { 679 unsigned int len = 0; 680 switch (de_size){ 681 case DE_SIZE_VAR_8: 682 len = element[1]; 683 break; 684 case DE_SIZE_VAR_16: 685 len = big_endian_read_16(element, 1); 686 break; 687 default: 688 break; 689 } 690 printf("len %u (0x%02x)\n", len, len); 691 printf_hexdump(&element[pos], len); 692 } else { 693 uint32_t value = 0; 694 switch (de_size) { 695 case DE_SIZE_8: 696 if (de_type != DE_NIL){ 697 value = element[pos]; 698 } 699 break; 700 case DE_SIZE_16: 701 value = big_endian_read_16(element,pos); 702 break; 703 case DE_SIZE_32: 704 value = big_endian_read_32(element,pos); 705 break; 706 default: 707 break; 708 } 709 printf(", value: 0x%08" PRIx32 "\n", value); 710 } 711 return 0; 712 } 713 #endif 714 715 void de_dump_data_element(const uint8_t * record){ 716 #ifdef ENABLE_SDP_DES_DUMP 717 int indent = 0; 718 // hack to get root DES, too. 719 de_type_t type = de_get_element_type(record); 720 de_size_t size = de_get_size_type(record); 721 de_traversal_dump_data((uint8_t *) record, type, size, (void*) &indent); 722 #else 723 UNUSED(record); 724 #endif 725 } 726 727 uint8_t* sdp_service_search_pattern_for_uuid16(uint16_t uuid16){ 728 big_endian_store_16(des_service_search_pattern_uuid16, 3, uuid16); 729 return (uint8_t*)des_service_search_pattern_uuid16; 730 } 731 732 uint8_t* sdp_service_search_pattern_for_uuid128(const uint8_t * uuid128){ 733 (void)memcpy(&des_service_search_pattern_uuid128[3], uuid128, 16); 734 return (uint8_t*)des_service_search_pattern_uuid128; 735 } 736 737