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