1 /*
2 * Copyright (C) 2014 BlueKitchen GmbH
3 *
4 * Redistribution and use in source and binary forms, with or without
5 * modification, are permitted provided that the following conditions
6 * are met:
7 *
8 * 1. Redistributions of source code must retain the above copyright
9 * notice, this list of conditions and the following disclaimer.
10 * 2. Redistributions in binary form must reproduce the above copyright
11 * notice, this list of conditions and the following disclaimer in the
12 * documentation and/or other materials provided with the distribution.
13 * 3. Neither the name of the copyright holders nor the names of
14 * contributors may be used to endorse or promote products derived
15 * from this software without specific prior written permission.
16 * 4. Any redistribution, use, or modification is done solely for
17 * personal benefit and not for any commercial purpose or for
18 * monetary gain.
19 *
20 * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS
21 * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN
24 * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27 * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28 * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
29 * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF
30 * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
31 * SUCH DAMAGE.
32 *
33 * Please inquire about commercial licensing options at
34 * [email protected]
35 *
36 */
37
38 #define BTSTACK_FILE__ "ad_parser.c"
39
40
41 // *****************************************************************************
42 //
43 // Advertising Data Parser
44 //
45 // *****************************************************************************
46
47 #include <stdint.h>
48 #include <string.h>
49
50 #include "bluetooth_data_types.h"
51 #include "btstack_util.h"
52 #include "hci.h"
53 #include "hci_cmd.h"
54
55 #include "ad_parser.h"
56
ad_iterator_init(ad_context_t * context,uint8_t ad_len,const uint8_t * ad_data)57 void ad_iterator_init(ad_context_t *context, uint8_t ad_len, const uint8_t * ad_data){
58 context->data = ad_data;
59 context->length = ad_len;
60 context->offset = 0;
61 }
62
ad_iterator_has_more(const ad_context_t * context)63 bool ad_iterator_has_more(const ad_context_t * context){
64 // assert chunk_len and chunk_type are withing buffer
65 if ((context->offset + 1u) >= context->length) {
66 return false;
67 }
68
69 // assert chunk_len > 0
70 uint8_t chunk_len = context->data[context->offset];
71 if (chunk_len == 0u){
72 return false;
73 }
74
75 // assert complete chunk fits into buffer
76 if ((context->offset + 1u + chunk_len) > context->length) {
77 return false;
78 }
79 return true;
80 }
81
82 // pre: ad_iterator_has_more() == 1
ad_iterator_next(ad_context_t * context)83 void ad_iterator_next(ad_context_t * context){
84 uint8_t chunk_len = context->data[context->offset];
85 context->offset += 1u + chunk_len;
86 }
87
ad_iterator_get_data_len(const ad_context_t * context)88 uint8_t ad_iterator_get_data_len(const ad_context_t * context){
89 return context->data[context->offset] - 1u;
90 }
91
ad_iterator_get_data_type(const ad_context_t * context)92 uint8_t ad_iterator_get_data_type(const ad_context_t * context){
93 return context->data[context->offset + 1u];
94 }
95
ad_iterator_get_data(const ad_context_t * context)96 const uint8_t * ad_iterator_get_data(const ad_context_t * context){
97 return &context->data[context->offset + 2u];
98 }
99
ad_data_contains_uuid16(uint8_t ad_len,const uint8_t * ad_data,uint16_t uuid16)100 bool ad_data_contains_uuid16(uint8_t ad_len, const uint8_t * ad_data, uint16_t uuid16){
101 ad_context_t context;
102 ad_iterator_init(&context, ad_len, ad_data);
103 while ( ad_iterator_has_more(&context) ){
104 uint8_t data_type = ad_iterator_get_data_type(&context);
105 uint8_t data_len = ad_iterator_get_data_len(&context);
106 const uint8_t * data = ad_iterator_get_data(&context);
107
108 uint8_t i;
109 uint8_t ad_uuid128[16], uuid128_bt[16];
110
111 switch (data_type){
112 case BLUETOOTH_DATA_TYPE_INCOMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS:
113 case BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS:
114 for (i=0u; (i + 2u) <= data_len; i+= 2u){
115 uint16_t uuid = (uint16_t) little_endian_read_16(data, (int) i);
116 if ( uuid == uuid16 ) {
117 return true;
118 }
119 }
120 break;
121 case BLUETOOTH_DATA_TYPE_INCOMPLETE_LIST_OF_128_BIT_SERVICE_CLASS_UUIDS:
122 case BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_128_BIT_SERVICE_CLASS_UUIDS:
123 uuid_add_bluetooth_prefix(ad_uuid128, uuid16);
124 reverse_128(ad_uuid128, uuid128_bt);
125 for (i=0u; (i + 16u) <= data_len; i += 16u){
126 if (memcmp(uuid128_bt, &data[i], 16) == 0){
127 return true;
128 };
129 }
130 break;
131 default:
132 break;
133 }
134 ad_iterator_next(&context);
135 }
136 return false;
137 }
138
ad_data_contains_uuid128(uint8_t ad_len,const uint8_t * ad_data,const uint8_t * uuid128)139 bool ad_data_contains_uuid128(uint8_t ad_len, const uint8_t * ad_data, const uint8_t * uuid128){
140 ad_context_t context;
141 // input in big endian/network order, bluetooth data in little endian
142 uint8_t uuid128_le[16];
143 reverse_128(uuid128, uuid128_le);
144 ad_iterator_init(&context, ad_len, ad_data);
145 while ( ad_iterator_has_more(&context) ){
146 uint8_t data_type = ad_iterator_get_data_type(&context);
147 uint8_t data_len = ad_iterator_get_data_len(&context);
148 const uint8_t * data = ad_iterator_get_data(&context);
149
150 uint8_t i;
151 uint8_t ad_uuid128[16];
152
153 switch (data_type){
154 case BLUETOOTH_DATA_TYPE_INCOMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS:
155 case BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_16_BIT_SERVICE_CLASS_UUIDS:
156 for (i = 0u; (i+2u) <= data_len; i += 2u){
157 uint16_t uuid16 = little_endian_read_16(data, (int) i);
158 uuid_add_bluetooth_prefix(ad_uuid128, uuid16);
159
160 if (memcmp(ad_uuid128, uuid128_le, 16) == 0) {
161 return true;
162 }
163 }
164
165 break;
166 case BLUETOOTH_DATA_TYPE_INCOMPLETE_LIST_OF_128_BIT_SERVICE_CLASS_UUIDS:
167 case BLUETOOTH_DATA_TYPE_COMPLETE_LIST_OF_128_BIT_SERVICE_CLASS_UUIDS:
168 for (i = 0u; (i + 16u) <= data_len; i += 16u){
169 if (memcmp(uuid128_le, &data[i], 16) == 0) {
170 return true;
171 }
172 }
173 break;
174 default:
175 break;
176 }
177 ad_iterator_next(&context);
178 }
179 return false;
180 }
181
182