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