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