[337] | 1 | /*
|
---|
| 2 | * Copyright (C) 2009-2012 by Matthias Ringwald
|
---|
| 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 MATTHIAS RINGWALD 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 btstack@ringwald.ch
|
---|
| 34 | *
|
---|
| 35 | */
|
---|
| 36 |
|
---|
| 37 | /*
|
---|
| 38 | * sdp_util.c
|
---|
| 39 | */
|
---|
| 40 |
|
---|
| 41 | #include <btstack/sdp_util.h>
|
---|
| 42 | #include <btstack/utils.h>
|
---|
| 43 | #include "btstack-config.h"
|
---|
| 44 |
|
---|
| 45 | #include <stdio.h>
|
---|
| 46 | #include <stdlib.h>
|
---|
| 47 | #include <string.h>
|
---|
| 48 | #include <stdint.h>
|
---|
| 49 | #include "debug.h"
|
---|
| 50 |
|
---|
| 51 | #ifdef SDP_DES_DUMP
|
---|
| 52 | // workaround for missing PRIx32 on mspgcc (16-bit MCU)
|
---|
| 53 | #ifndef PRIx32
|
---|
| 54 | #warning Using own: #define PRIx32 "lx"
|
---|
| 55 | #define PRIx32 "lx"
|
---|
| 56 | #endif
|
---|
| 57 | // date element type names
|
---|
| 58 | const char * const type_names[] = { "NIL", "UINT", "INT", "UUID", "STRING", "BOOL", "DES", "DEA", "URL"};
|
---|
| 59 | #endif
|
---|
| 60 |
|
---|
| 61 | // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB
|
---|
| 62 | const uint8_t sdp_bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */
|
---|
| 63 | 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB };
|
---|
| 64 |
|
---|
| 65 | void sdp_normalize_uuid(uint8_t *uuid, uint32_t shortUUID){
|
---|
| 66 | memcpy(uuid, sdp_bluetooth_base_uuid, 16);
|
---|
| 67 | net_store_32(uuid, 0, shortUUID);
|
---|
| 68 | }
|
---|
| 69 |
|
---|
| 70 | int sdp_has_blueooth_base_uuid(uint8_t * uuid128){
|
---|
| 71 | return memcmp(&uuid128[4], &sdp_bluetooth_base_uuid[4], 12) == 0;
|
---|
| 72 | }
|
---|
| 73 |
|
---|
| 74 | // MARK: DataElement getter
|
---|
| 75 | de_size_t de_get_size_type(uint8_t *header){
|
---|
| 76 | return (de_size_t) (header[0] & 7);
|
---|
| 77 | }
|
---|
| 78 |
|
---|
| 79 | de_type_t de_get_element_type(uint8_t *header){
|
---|
| 80 | return (de_type_t) (header[0] >> 3);
|
---|
| 81 | }
|
---|
| 82 |
|
---|
| 83 | int de_get_header_size(uint8_t * header){
|
---|
| 84 | de_size_t de_size = de_get_size_type(header);
|
---|
| 85 | if (de_size <= DE_SIZE_128) {
|
---|
| 86 | return 1;
|
---|
| 87 | }
|
---|
| 88 | return 1 + (1 << (de_size-DE_SIZE_VAR_8));
|
---|
| 89 | }
|
---|
| 90 |
|
---|
| 91 | int de_get_data_size(uint8_t * header){
|
---|
| 92 | uint32_t result = 0;
|
---|
| 93 | de_type_t de_type = de_get_element_type(header);
|
---|
| 94 | de_size_t de_size = de_get_size_type(header);
|
---|
| 95 | switch (de_size){
|
---|
| 96 | case DE_SIZE_VAR_8:
|
---|
| 97 | result = header[1];
|
---|
| 98 | break;
|
---|
| 99 | case DE_SIZE_VAR_16:
|
---|
| 100 | result = READ_NET_16(header,1);
|
---|
| 101 | break;
|
---|
| 102 | case DE_SIZE_VAR_32:
|
---|
| 103 | result = READ_NET_32(header,1);
|
---|
| 104 | break;
|
---|
| 105 | default:
|
---|
| 106 | // case DE_SIZE_8:
|
---|
| 107 | // case DE_SIZE_16:
|
---|
| 108 | // case DE_SIZE_32:
|
---|
| 109 | // case DE_SIZE_64:
|
---|
| 110 | // case DE_SIZE_128:
|
---|
| 111 | if (de_type == DE_NIL) return 0;
|
---|
| 112 | return 1 << de_size;
|
---|
| 113 | }
|
---|
| 114 | return result;
|
---|
| 115 | }
|
---|
| 116 |
|
---|
| 117 | int de_get_len(uint8_t *header){
|
---|
| 118 | return de_get_header_size(header) + de_get_data_size(header);
|
---|
| 119 | }
|
---|
| 120 |
|
---|
| 121 | // @returns OK, if UINT16 value was read
|
---|
| 122 | int de_element_get_uint16(uint8_t * element, uint16_t * value){
|
---|
| 123 | if (de_get_size_type(element) != DE_SIZE_16) return 0;
|
---|
| 124 | *value = READ_NET_16(element, de_get_header_size(element));
|
---|
| 125 | return 1;
|
---|
| 126 | }
|
---|
| 127 |
|
---|
| 128 | // @returns: element is valid UUID
|
---|
| 129 | int de_get_normalized_uuid(uint8_t *uuid128, uint8_t *element){
|
---|
| 130 | de_type_t uuidType = de_get_element_type(element);
|
---|
| 131 | de_size_t uuidSize = de_get_size_type(element);
|
---|
| 132 | uint32_t shortUUID;
|
---|
| 133 | if (uuidType != DE_UUID) return 0;
|
---|
| 134 | switch (uuidSize){
|
---|
| 135 | case DE_SIZE_16:
|
---|
| 136 | shortUUID = READ_NET_16(element, 1);
|
---|
| 137 | break;
|
---|
| 138 | case DE_SIZE_32:
|
---|
| 139 | shortUUID = READ_NET_32(element, 1);
|
---|
| 140 | break;
|
---|
| 141 | case DE_SIZE_128:
|
---|
| 142 | memcpy(uuid128, element+1, 16);
|
---|
| 143 | return 1;
|
---|
| 144 | default:
|
---|
| 145 | return 0;
|
---|
| 146 | }
|
---|
| 147 | sdp_normalize_uuid(uuid128, shortUUID);
|
---|
| 148 | return 1;
|
---|
| 149 | }
|
---|
| 150 |
|
---|
| 151 | // @returns 0 if no UUID16 or UUID32 is present, and UUID32 otherwise
|
---|
| 152 | uint32_t de_get_uuid32(uint8_t * element){
|
---|
| 153 | uint8_t uuid128[16];
|
---|
| 154 | int validUuid128 = de_get_normalized_uuid(uuid128, element);
|
---|
| 155 | int hasBlueoothBaseUuid;
|
---|
| 156 | if (!validUuid128) return 0;
|
---|
| 157 | 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 | void sdp_add_attribute(uint8_t *seq, uint16_t attributeID, uint8_t attributeValue){
|
---|
| 293 | }
|
---|
| 294 |
|
---|
| 295 | // MARK: DataElementSequence traversal
|
---|
| 296 | typedef int (*de_traversal_callback_t)(uint8_t * element, de_type_t type, de_size_t size, void *context);
|
---|
| 297 | static void de_traverse_sequence(uint8_t * element, de_traversal_callback_t handler, void *context){
|
---|
| 298 | de_type_t type = de_get_element_type(element);
|
---|
| 299 | int pos, end_pos;
|
---|
| 300 | if (type != DE_DES) return;
|
---|
| 301 | pos = de_get_header_size(element);
|
---|
| 302 | end_pos = de_get_len(element);
|
---|
| 303 | while (pos < end_pos){
|
---|
| 304 | de_type_t elemType = de_get_element_type(element + pos);
|
---|
| 305 | de_size_t elemSize = de_get_size_type(element + pos);
|
---|
| 306 | uint8_t done = (*handler)(element + pos, elemType, elemSize, context);
|
---|
| 307 | if (done) break;
|
---|
| 308 | pos += de_get_len(element + pos);
|
---|
| 309 | }
|
---|
| 310 | }
|
---|
| 311 |
|
---|
| 312 | // MARK: AttributeList traversal
|
---|
| 313 | typedef int (*sdp_attribute_list_traversal_callback_t)(uint16_t attributeID, uint8_t * attributeValue, de_type_t type, de_size_t size, void *context);
|
---|
| 314 | static void sdp_attribute_list_traverse_sequence(uint8_t * element, sdp_attribute_list_traversal_callback_t handler, void *context){
|
---|
| 315 | de_type_t type = de_get_element_type(element);
|
---|
| 316 | int pos, end_pos;
|
---|
| 317 | if (type != DE_DES) return;
|
---|
| 318 | pos = de_get_header_size(element);
|
---|
| 319 | end_pos = de_get_len(element);
|
---|
| 320 | while (pos < end_pos){
|
---|
| 321 | de_type_t idType = de_get_element_type(element + pos);
|
---|
| 322 | de_size_t idSize = de_get_size_type(element + pos);
|
---|
| 323 | uint16_t attribute_id;
|
---|
| 324 | de_type_t valueType;
|
---|
| 325 | de_size_t valueSize;
|
---|
| 326 | uint8_t done;
|
---|
| 327 | if (idType != DE_UINT || idSize != DE_SIZE_16) break; // wrong type
|
---|
| 328 | attribute_id = READ_NET_16(element, pos + 1);
|
---|
| 329 | pos += 3;
|
---|
| 330 | if (pos >= end_pos) break; // array out of bounds
|
---|
| 331 | valueType = de_get_element_type(element + pos);
|
---|
| 332 | valueSize = de_get_size_type(element + pos);
|
---|
| 333 | done = (*handler)(attribute_id, element + pos, valueType, valueSize, context);
|
---|
| 334 | if (done) break;
|
---|
| 335 | pos += de_get_len(element + pos);
|
---|
| 336 | }
|
---|
| 337 | }
|
---|
| 338 |
|
---|
| 339 | // MARK: AttributeID in AttributeIDList
|
---|
| 340 | // attribute ID in AttributeIDList
|
---|
| 341 | // context { result, attributeID }
|
---|
| 342 | struct sdp_context_attributeID_search {
|
---|
| 343 | int result;
|
---|
| 344 | uint16_t attributeID;
|
---|
| 345 | };
|
---|
| 346 | static int sdp_traversal_attributeID_search(uint8_t * element, de_type_t type, de_size_t size, void *my_context){
|
---|
| 347 | struct sdp_context_attributeID_search * context = (struct sdp_context_attributeID_search *) my_context;
|
---|
| 348 | if (type != DE_UINT) return 0;
|
---|
| 349 | switch (size) {
|
---|
| 350 | case DE_SIZE_16:
|
---|
| 351 | if (READ_NET_16(element, 1) == context->attributeID) {
|
---|
| 352 | context->result = 1;
|
---|
| 353 | return 1;
|
---|
| 354 | }
|
---|
| 355 | break;
|
---|
| 356 | case DE_SIZE_32:
|
---|
| 357 | if (READ_NET_16(element, 1) <= context->attributeID
|
---|
| 358 | && context->attributeID <= READ_NET_16(element, 3)) {
|
---|
| 359 | context->result = 1;
|
---|
| 360 | return 1;
|
---|
| 361 | }
|
---|
| 362 | break;
|
---|
| 363 | default:
|
---|
| 364 | break;
|
---|
| 365 | }
|
---|
| 366 | return 0;
|
---|
| 367 | }
|
---|
| 368 | int sdp_attribute_list_constains_id(uint8_t *attributeIDList, uint16_t attributeID){
|
---|
| 369 | struct sdp_context_attributeID_search attributeID_search;
|
---|
| 370 | attributeID_search.result = 0;
|
---|
| 371 | attributeID_search.attributeID = attributeID;
|
---|
| 372 | de_traverse_sequence(attributeIDList, sdp_traversal_attributeID_search, &attributeID_search);
|
---|
| 373 | return attributeID_search.result;
|
---|
| 374 | }
|
---|
| 375 |
|
---|
| 376 | // MARK: Append Attributes for AttributeIDList
|
---|
| 377 | // pre: buffer contains DES with 2 byte length field
|
---|
| 378 | struct sdp_context_append_attributes {
|
---|
| 379 | uint8_t * buffer;
|
---|
| 380 | uint16_t startOffset; // offset of when to start copying
|
---|
| 381 | uint16_t maxBytes;
|
---|
| 382 | uint16_t usedBytes;
|
---|
| 383 | uint8_t *attributeIDList;
|
---|
| 384 | };
|
---|
| 385 |
|
---|
| 386 | static int sdp_traversal_append_attributes(uint16_t attributeID, uint8_t * attributeValue, de_type_t type, de_size_t size, void *my_context){
|
---|
| 387 | struct sdp_context_append_attributes * context = (struct sdp_context_append_attributes *) my_context;
|
---|
| 388 | if (sdp_attribute_list_constains_id(context->attributeIDList, attributeID)) {
|
---|
| 389 | // DES_HEADER(3) + DES_DATA + (UINT16(3) + attribute)
|
---|
| 390 | uint16_t data_size = READ_NET_16(context->buffer, 1);
|
---|
| 391 | int attribute_len = de_get_len(attributeValue);
|
---|
| 392 | if (3 + data_size + (3 + attribute_len) <= context->maxBytes) {
|
---|
| 393 | // copy Attribute
|
---|
| 394 | de_add_number(context->buffer, DE_UINT, DE_SIZE_16, attributeID);
|
---|
| 395 | data_size += 3; // 3 bytes
|
---|
| 396 | memcpy(context->buffer + 3 + data_size, attributeValue, attribute_len);
|
---|
| 397 | net_store_16(context->buffer,1,data_size+attribute_len);
|
---|
| 398 | } else {
|
---|
| 399 | // not enought space left -> continue with previous element
|
---|
| 400 | return 1;
|
---|
| 401 | }
|
---|
| 402 | }
|
---|
| 403 | return 0;
|
---|
| 404 | }
|
---|
| 405 |
|
---|
| 406 | // maxBytes: maximal size of data element sequence
|
---|
| 407 | uint16_t sdp_append_attributes_in_attributeIDList(uint8_t *record, uint8_t *attributeIDList, uint16_t startOffset, uint16_t maxBytes, uint8_t *buffer){
|
---|
| 408 | struct sdp_context_append_attributes context;
|
---|
| 409 | context.buffer = buffer;
|
---|
| 410 | context.maxBytes = maxBytes;
|
---|
| 411 | context.usedBytes = 0;
|
---|
| 412 | context.startOffset = startOffset;
|
---|
| 413 | context.attributeIDList = attributeIDList;
|
---|
| 414 | sdp_attribute_list_traverse_sequence(record, sdp_traversal_append_attributes, &context);
|
---|
| 415 | return context.usedBytes;
|
---|
| 416 | }
|
---|
| 417 |
|
---|
| 418 | // MARK: Filter attributes that match attribute list from startOffset and a max nr bytes
|
---|
| 419 | struct sdp_context_filter_attributes {
|
---|
| 420 | uint8_t * buffer;
|
---|
| 421 | uint16_t startOffset; // offset of when to start copying
|
---|
| 422 | uint16_t maxBytes;
|
---|
| 423 | uint16_t usedBytes;
|
---|
| 424 | uint8_t *attributeIDList;
|
---|
| 425 | int complete;
|
---|
| 426 | };
|
---|
| 427 |
|
---|
| 428 | // copy data with given start offset and max bytes, returns OK if all data has been copied
|
---|
| 429 | static int spd_append_range(struct sdp_context_filter_attributes* context, uint16_t len, uint8_t *data){
|
---|
| 430 | int ok = 1;
|
---|
| 431 | uint16_t remainder_len = len - context->startOffset;
|
---|
| 432 | if (context->maxBytes < remainder_len){
|
---|
| 433 | remainder_len = context->maxBytes;
|
---|
| 434 | ok = 0;
|
---|
| 435 | }
|
---|
| 436 | memcpy(context->buffer, &data[context->startOffset], remainder_len);
|
---|
| 437 | context->usedBytes += remainder_len;
|
---|
| 438 | context->buffer += remainder_len;
|
---|
| 439 | context->maxBytes -= remainder_len;
|
---|
| 440 | context->startOffset = 0;
|
---|
| 441 | return ok;
|
---|
| 442 | }
|
---|
| 443 |
|
---|
| 444 | static int sdp_traversal_filter_attributes(uint16_t attributeID, uint8_t * attributeValue, de_type_t type, de_size_t size, void *my_context){
|
---|
| 445 | struct sdp_context_filter_attributes * context = (struct sdp_context_filter_attributes *) my_context;
|
---|
| 446 | int attribute_len;
|
---|
| 447 | int ok;
|
---|
| 448 |
|
---|
| 449 | if (!sdp_attribute_list_constains_id(context->attributeIDList, attributeID)) return 0;
|
---|
| 450 |
|
---|
| 451 | // { Attribute ID (Descriptor, big endian 16-bit ID), AttributeValue (data)}
|
---|
| 452 |
|
---|
| 453 | // handle Attribute ID
|
---|
| 454 | if (context->startOffset >= 3){
|
---|
| 455 | context->startOffset -= 3;
|
---|
| 456 | } else {
|
---|
| 457 | uint8_t idBuffer[3];
|
---|
| 458 | int ok;
|
---|
| 459 | de_store_descriptor(idBuffer, DE_UINT, DE_SIZE_16);
|
---|
| 460 | net_store_16(idBuffer,1,attributeID);
|
---|
| 461 |
|
---|
| 462 | ok = spd_append_range(context, 3, idBuffer);
|
---|
| 463 | if (!ok) {
|
---|
| 464 | context->complete = 0;
|
---|
| 465 | return 1;
|
---|
| 466 | }
|
---|
| 467 | }
|
---|
| 468 |
|
---|
| 469 | // handle Attribute Value
|
---|
| 470 | attribute_len = de_get_len(attributeValue);
|
---|
| 471 | if (context->startOffset >= attribute_len) {
|
---|
| 472 | context->startOffset -= attribute_len;
|
---|
| 473 | return 0;
|
---|
| 474 | }
|
---|
| 475 |
|
---|
| 476 | ok = spd_append_range(context, attribute_len, attributeValue);
|
---|
| 477 | if (!ok) {
|
---|
| 478 | context->complete = 0;
|
---|
| 479 | return 1;
|
---|
| 480 | }
|
---|
| 481 | return 0;
|
---|
| 482 | }
|
---|
| 483 |
|
---|
| 484 | int sdp_filter_attributes_in_attributeIDList(uint8_t *record, uint8_t *attributeIDList, uint16_t startOffset, uint16_t maxBytes, uint16_t *usedBytes, uint8_t *buffer){
|
---|
| 485 |
|
---|
| 486 | struct sdp_context_filter_attributes context;
|
---|
| 487 | context.buffer = buffer;
|
---|
| 488 | context.maxBytes = maxBytes;
|
---|
| 489 | context.usedBytes = 0;
|
---|
| 490 | context.startOffset = startOffset;
|
---|
| 491 | context.attributeIDList = attributeIDList;
|
---|
| 492 | context.complete = 1;
|
---|
| 493 |
|
---|
| 494 | sdp_attribute_list_traverse_sequence(record, sdp_traversal_filter_attributes, &context);
|
---|
| 495 |
|
---|
| 496 | *usedBytes = context.usedBytes;
|
---|
| 497 | return context.complete;
|
---|
| 498 | }
|
---|
| 499 |
|
---|
| 500 | // MARK: Get sum of attributes matching attribute list
|
---|
| 501 | struct sdp_context_get_filtered_size {
|
---|
| 502 | uint8_t *attributeIDList;
|
---|
| 503 | uint16_t size;
|
---|
| 504 | };
|
---|
| 505 |
|
---|
| 506 | static int sdp_traversal_get_filtered_size(uint16_t attributeID, uint8_t * attributeValue, de_type_t type, de_size_t size, void *my_context){
|
---|
| 507 | struct sdp_context_get_filtered_size * context = (struct sdp_context_get_filtered_size *) my_context;
|
---|
| 508 | if (sdp_attribute_list_constains_id(context->attributeIDList, attributeID)) {
|
---|
| 509 | context->size += 3 + de_get_len(attributeValue);
|
---|
| 510 | }
|
---|
| 511 | return 0;
|
---|
| 512 | }
|
---|
| 513 |
|
---|
| 514 | int spd_get_filtered_size(uint8_t *record, uint8_t *attributeIDList){
|
---|
| 515 | struct sdp_context_get_filtered_size context;
|
---|
| 516 | context.size = 0;
|
---|
| 517 | context.attributeIDList = attributeIDList;
|
---|
| 518 | sdp_attribute_list_traverse_sequence(record, sdp_traversal_get_filtered_size, &context);
|
---|
| 519 | return context.size;
|
---|
| 520 | }
|
---|
| 521 |
|
---|
| 522 | // MARK: Get AttributeValue for AttributeID
|
---|
| 523 | // find attribute (ELEMENT) by ID
|
---|
| 524 | struct sdp_context_attribute_by_id {
|
---|
| 525 | uint16_t attributeID;
|
---|
| 526 | uint8_t * attributeValue;
|
---|
| 527 | };
|
---|
| 528 | static int sdp_traversal_attribute_by_id(uint16_t attributeID, uint8_t * attributeValue, de_type_t attributeType, de_size_t size, void *my_context){
|
---|
| 529 | struct sdp_context_attribute_by_id * context = (struct sdp_context_attribute_by_id *) my_context;
|
---|
| 530 | if (attributeID == context->attributeID) {
|
---|
| 531 | context->attributeValue = attributeValue;
|
---|
| 532 | return 1;
|
---|
| 533 | }
|
---|
| 534 | return 0;
|
---|
| 535 | }
|
---|
| 536 |
|
---|
| 537 | uint8_t * sdp_get_attribute_value_for_attribute_id(uint8_t * record, uint16_t attributeID){
|
---|
| 538 | struct sdp_context_attribute_by_id context;
|
---|
| 539 | context.attributeValue = NULL;
|
---|
| 540 | context.attributeID = attributeID;
|
---|
| 541 | sdp_attribute_list_traverse_sequence(record, sdp_traversal_attribute_by_id, &context);
|
---|
| 542 | return context.attributeValue;
|
---|
| 543 | }
|
---|
| 544 |
|
---|
| 545 | // MARK: Set AttributeValue for AttributeID
|
---|
| 546 | struct sdp_context_set_attribute_for_id {
|
---|
| 547 | uint16_t attributeID;
|
---|
| 548 | uint32_t attributeValue;
|
---|
| 549 | uint8_t attributeFound;
|
---|
| 550 | };
|
---|
| 551 | 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){
|
---|
| 552 | struct sdp_context_set_attribute_for_id * context = (struct sdp_context_set_attribute_for_id *) my_context;
|
---|
| 553 | if (attributeID == context->attributeID) {
|
---|
| 554 | context->attributeFound = 1;
|
---|
| 555 | switch (size){
|
---|
| 556 | case DE_SIZE_8:
|
---|
| 557 | if (attributeType != DE_NIL){
|
---|
| 558 | attributeValue[1] = context->attributeValue;
|
---|
| 559 | }
|
---|
| 560 | break;
|
---|
| 561 | case DE_SIZE_16:
|
---|
| 562 | net_store_16(attributeValue, 1, context->attributeValue);
|
---|
| 563 | break;
|
---|
| 564 | case DE_SIZE_32:
|
---|
| 565 | net_store_32(attributeValue, 1, context->attributeValue);
|
---|
| 566 | break;
|
---|
| 567 | // Might want to support STRINGS to, copy upto original length
|
---|
| 568 | default:
|
---|
| 569 | break;
|
---|
| 570 | }
|
---|
| 571 | return 1;
|
---|
| 572 | }
|
---|
| 573 | return 0;
|
---|
| 574 | }
|
---|
| 575 | uint8_t sdp_set_attribute_value_for_attribute_id(uint8_t * record, uint16_t attributeID, uint32_t value){
|
---|
| 576 | struct sdp_context_set_attribute_for_id context;
|
---|
| 577 | context.attributeID = attributeID;
|
---|
| 578 | context.attributeValue = value;
|
---|
| 579 | context.attributeFound = 0;
|
---|
| 580 | sdp_attribute_list_traverse_sequence(record, sdp_traversal_set_attribute_for_id, &context);
|
---|
| 581 | return context.attributeFound;
|
---|
| 582 | }
|
---|
| 583 |
|
---|
| 584 | // MARK: ServiceRecord contains UUID
|
---|
| 585 | // service record contains UUID
|
---|
| 586 | // context { normalizedUUID }
|
---|
| 587 | struct sdp_context_contains_uuid128 {
|
---|
| 588 | uint8_t * uuid128;
|
---|
| 589 | int result;
|
---|
| 590 | };
|
---|
| 591 | int sdp_record_contains_UUID128(uint8_t *record, uint8_t *uuid128);
|
---|
| 592 | static int sdp_traversal_contains_UUID128(uint8_t * element, de_type_t type, de_size_t size, void *my_context){
|
---|
| 593 | struct sdp_context_contains_uuid128 * context = (struct sdp_context_contains_uuid128 *) my_context;
|
---|
| 594 | uint8_t normalizedUUID[16];
|
---|
| 595 | if (type == DE_UUID){
|
---|
| 596 | uint8_t uuidOK = de_get_normalized_uuid(normalizedUUID, element);
|
---|
| 597 | context->result = uuidOK && memcmp(context->uuid128, normalizedUUID, 16) == 0;
|
---|
| 598 | }
|
---|
| 599 | if (type == DE_DES){
|
---|
| 600 | context->result = sdp_record_contains_UUID128(element, context->uuid128);
|
---|
| 601 | }
|
---|
| 602 | return context->result;
|
---|
| 603 | }
|
---|
| 604 | int sdp_record_contains_UUID128(uint8_t *record, uint8_t *uuid128){
|
---|
| 605 | struct sdp_context_contains_uuid128 context;
|
---|
| 606 | context.uuid128 = uuid128;
|
---|
| 607 | context.result = 0;
|
---|
| 608 | de_traverse_sequence(record, sdp_traversal_contains_UUID128, &context);
|
---|
| 609 | return context.result;
|
---|
| 610 | }
|
---|
| 611 |
|
---|
| 612 | // MARK: ServiceRecord matches SearchServicePattern
|
---|
| 613 | // if UUID in searchServicePattern is not found in record => false
|
---|
| 614 | // context { result, record }
|
---|
| 615 | struct sdp_context_match_pattern {
|
---|
| 616 | uint8_t * record;
|
---|
| 617 | int result;
|
---|
| 618 | };
|
---|
| 619 | int sdp_traversal_match_pattern(uint8_t * element, de_type_t attributeType, de_size_t size, void *my_context){
|
---|
| 620 | struct sdp_context_match_pattern * context = (struct sdp_context_match_pattern *) my_context;
|
---|
| 621 | uint8_t normalizedUUID[16];
|
---|
| 622 | uint8_t uuidOK = de_get_normalized_uuid(normalizedUUID, element);
|
---|
| 623 | if (!uuidOK || !sdp_record_contains_UUID128(context->record, normalizedUUID)){
|
---|
| 624 | context->result = 0;
|
---|
| 625 | return 1;
|
---|
| 626 | }
|
---|
| 627 | return 0;
|
---|
| 628 | }
|
---|
| 629 | int sdp_record_matches_service_search_pattern(uint8_t *record, uint8_t *serviceSearchPattern){
|
---|
| 630 | struct sdp_context_match_pattern context;
|
---|
| 631 | context.record = record;
|
---|
| 632 | context.result = 1;
|
---|
| 633 | de_traverse_sequence(serviceSearchPattern, sdp_traversal_match_pattern, &context);
|
---|
| 634 | return context.result;
|
---|
| 635 | }
|
---|
| 636 |
|
---|
| 637 | // MARK: Dump DataElement
|
---|
| 638 | // context { indent }
|
---|
| 639 | #ifdef SDP_DES_DUMP
|
---|
| 640 | static int de_traversal_dump_data(uint8_t * element, de_type_t de_type, de_size_t de_size, void *my_context){
|
---|
| 641 | int indent = *(int*) my_context;
|
---|
| 642 | int i;
|
---|
| 643 | int pos;
|
---|
| 644 | int end_pos;
|
---|
| 645 | for (i=0; i<indent;i++) printf(" ");
|
---|
| 646 | pos = de_get_header_size(element);
|
---|
| 647 | end_pos = de_get_len(element);
|
---|
| 648 | printf("type %5s (%u), element len %2u ", type_names[de_type], de_type, end_pos);
|
---|
| 649 | if (de_type == DE_DES) {
|
---|
| 650 | printf("\n");
|
---|
| 651 | indent++;
|
---|
| 652 | de_traverse_sequence(element, de_traversal_dump_data, (void *)&indent);
|
---|
| 653 | } else if (de_type == DE_UUID && de_size == DE_SIZE_128) {
|
---|
| 654 | printf(", value: ");
|
---|
| 655 | printUUID128(element+1);
|
---|
| 656 | printf("\n");
|
---|
| 657 | } else if (de_type == DE_STRING) {
|
---|
| 658 | int len = 0;
|
---|
| 659 | switch (de_size){
|
---|
| 660 | case DE_SIZE_VAR_8:
|
---|
| 661 | len = element[1];
|
---|
| 662 | break;
|
---|
| 663 | case DE_SIZE_VAR_16:
|
---|
| 664 | len = READ_NET_16(element, 1);
|
---|
| 665 | break;
|
---|
| 666 | default:
|
---|
| 667 | break;
|
---|
| 668 | }
|
---|
| 669 | printf("len %u (0x%02x)\n", len, len);
|
---|
| 670 | printf_hexdump(&element[pos], len);
|
---|
| 671 | } else {
|
---|
| 672 | uint32_t value = 0;
|
---|
| 673 | switch (de_size) {
|
---|
| 674 | case DE_SIZE_8:
|
---|
| 675 | if (de_type != DE_NIL){
|
---|
| 676 | value = element[pos];
|
---|
| 677 | }
|
---|
| 678 | break;
|
---|
| 679 | case DE_SIZE_16:
|
---|
| 680 | value = READ_NET_16(element,pos);
|
---|
| 681 | break;
|
---|
| 682 | case DE_SIZE_32:
|
---|
| 683 | value = READ_NET_32(element,pos);
|
---|
| 684 | break;
|
---|
| 685 | default:
|
---|
| 686 | break;
|
---|
| 687 | }
|
---|
| 688 | printf(", value: 0x%08" PRIx32 "\n", value);
|
---|
| 689 | }
|
---|
| 690 | return 0;
|
---|
| 691 | }
|
---|
| 692 | #endif
|
---|
| 693 |
|
---|
| 694 | void de_dump_data_element(uint8_t * record){
|
---|
| 695 | #ifdef SDP_DES_DUMP
|
---|
| 696 | int indent = 0;
|
---|
| 697 | // hack to get root DES, too.
|
---|
| 698 | de_type_t type = de_get_element_type(record);
|
---|
| 699 | de_size_t size = de_get_size_type(record);
|
---|
| 700 | de_traversal_dump_data(record, type, size, (void*) &indent);
|
---|
| 701 | #endif
|
---|
| 702 | }
|
---|
| 703 |
|
---|
| 704 | void sdp_create_spp_service(uint8_t *service, int service_id, const char *name){
|
---|
| 705 |
|
---|
| 706 | uint8_t* attribute;
|
---|
| 707 | de_create_sequence(service);
|
---|
| 708 |
|
---|
| 709 | // 0x0000 "Service Record Handle"
|
---|
| 710 | de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ServiceRecordHandle);
|
---|
| 711 | de_add_number(service, DE_UINT, DE_SIZE_32, 0x10001);
|
---|
| 712 |
|
---|
| 713 | // 0x0001 "Service Class ID List"
|
---|
| 714 | de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ServiceClassIDList);
|
---|
| 715 | attribute = de_push_sequence(service);
|
---|
| 716 | {
|
---|
| 717 | de_add_number(attribute, DE_UUID, DE_SIZE_16, 0x1101 );
|
---|
| 718 | }
|
---|
| 719 | de_pop_sequence(service, attribute);
|
---|
| 720 |
|
---|
| 721 | // 0x0004 "Protocol Descriptor List"
|
---|
| 722 | de_add_number(service, DE_UINT, DE_SIZE_16, SDP_ProtocolDescriptorList);
|
---|
| 723 | attribute = de_push_sequence(service);
|
---|
| 724 | {
|
---|
| 725 | uint8_t* l2cpProtocol = de_push_sequence(attribute);
|
---|
| 726 | uint8_t* rfcomm;
|
---|
| 727 | {
|
---|
| 728 | de_add_number(l2cpProtocol, DE_UUID, DE_SIZE_16, 0x0100);
|
---|
| 729 | }
|
---|
| 730 | de_pop_sequence(attribute, l2cpProtocol);
|
---|
| 731 |
|
---|
| 732 | rfcomm = de_push_sequence(attribute);
|
---|
| 733 | {
|
---|
| 734 | de_add_number(rfcomm, DE_UUID, DE_SIZE_16, 0x0003); // rfcomm_service
|
---|
| 735 | de_add_number(rfcomm, DE_UINT, DE_SIZE_8, service_id); // rfcomm channel
|
---|
| 736 | }
|
---|
| 737 | de_pop_sequence(attribute, rfcomm);
|
---|
| 738 | }
|
---|
| 739 | de_pop_sequence(service, attribute);
|
---|
| 740 |
|
---|
| 741 | // 0x0005 "Public Browse Group"
|
---|
| 742 | de_add_number(service, DE_UINT, DE_SIZE_16, SDP_BrowseGroupList); // public browse group
|
---|
| 743 | attribute = de_push_sequence(service);
|
---|
| 744 | {
|
---|
| 745 | de_add_number(attribute, DE_UUID, DE_SIZE_16, 0x1002 );
|
---|
| 746 | }
|
---|
| 747 | de_pop_sequence(service, attribute);
|
---|
| 748 |
|
---|
| 749 | // 0x0006
|
---|
| 750 | de_add_number(service, DE_UINT, DE_SIZE_16, SDP_LanguageBaseAttributeIDList);
|
---|
| 751 | attribute = de_push_sequence(service);
|
---|
| 752 | {
|
---|
| 753 | de_add_number(attribute, DE_UINT, DE_SIZE_16, 0x656e);
|
---|
| 754 | de_add_number(attribute, DE_UINT, DE_SIZE_16, 0x006a);
|
---|
| 755 | de_add_number(attribute, DE_UINT, DE_SIZE_16, 0x0100);
|
---|
| 756 | }
|
---|
| 757 | de_pop_sequence(service, attribute);
|
---|
| 758 |
|
---|
| 759 | // 0x0009 "Bluetooth Profile Descriptor List"
|
---|
| 760 | de_add_number(service, DE_UINT, DE_SIZE_16, SDP_BluetoothProfileDescriptorList);
|
---|
| 761 | attribute = de_push_sequence(service);
|
---|
| 762 | {
|
---|
| 763 | uint8_t *sppProfile = de_push_sequence(attribute);
|
---|
| 764 | {
|
---|
| 765 | de_add_number(sppProfile, DE_UUID, DE_SIZE_16, 0x1101);
|
---|
| 766 | de_add_number(sppProfile, DE_UINT, DE_SIZE_16, 0x0100);
|
---|
| 767 | }
|
---|
| 768 | de_pop_sequence(attribute, sppProfile);
|
---|
| 769 | }
|
---|
| 770 | de_pop_sequence(service, attribute);
|
---|
| 771 |
|
---|
| 772 | // 0x0100 "ServiceName"
|
---|
| 773 | de_add_number(service, DE_UINT, DE_SIZE_16, 0x0100);
|
---|
| 774 | de_add_data(service, DE_STRING, strlen(name), (uint8_t *) name);
|
---|
| 775 | }
|
---|