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