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