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