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 = big_endian_read_16(header,1);
|
---|
101 | break;
|
---|
102 | case DE_SIZE_VAR_32:
|
---|
103 | result = big_endian_read_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 = big_endian_read_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 = big_endian_read_16(element, 1);
|
---|
137 | break;
|
---|
138 | case DE_SIZE_32:
|
---|
139 | shortUUID = big_endian_read_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 big_endian_read_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 = big_endian_read_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 = big_endian_read_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 = big_endian_read_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 = big_endian_read_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 = big_endian_read_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 (big_endian_read_16(element, 1) == context->attributeID) {
|
---|
352 | context->result = 1;
|
---|
353 | return 1;
|
---|
354 | }
|
---|
355 | break;
|
---|
356 | case DE_SIZE_32:
|
---|
357 | if (big_endian_read_16(element, 1) <= context->attributeID
|
---|
358 | && context->attributeID <= big_endian_read_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 = big_endian_read_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 = big_endian_read_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 = big_endian_read_16(element,pos);
|
---|
681 | break;
|
---|
682 | case DE_SIZE_32:
|
---|
683 | value = big_endian_read_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, BLUETOOTH_ATTRIBUTE_SERVICE_CLASS_ID_LIST);
|
---|
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, BLUETOOTH_ATTRIBUTE_PROTOCOL_DESCRIPTOR_LIST);
|
---|
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 | }
|
---|