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