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