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 | * utils.c
|
---|
39 | *
|
---|
40 | * General utility functions
|
---|
41 | *
|
---|
42 | * Created by Matthias Ringwald on 7/23/09.
|
---|
43 | */
|
---|
44 |
|
---|
45 | #include "btstack-config.h"
|
---|
46 | #include <btstack/utils.h>
|
---|
47 | #include <stdio.h>
|
---|
48 | #include <string.h>
|
---|
49 | #include "debug.h"
|
---|
50 |
|
---|
51 | void little_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){
|
---|
52 | buffer[pos++] = value;
|
---|
53 | buffer[pos++] = value >> 8;
|
---|
54 | }
|
---|
55 |
|
---|
56 | void little_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){
|
---|
57 | buffer[pos++] = value;
|
---|
58 | buffer[pos++] = value >> 8;
|
---|
59 | buffer[pos++] = value >> 16;
|
---|
60 | buffer[pos++] = value >> 24;
|
---|
61 | }
|
---|
62 |
|
---|
63 | void net_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){
|
---|
64 | buffer[pos++] = value >> 8;
|
---|
65 | buffer[pos++] = value;
|
---|
66 | }
|
---|
67 |
|
---|
68 | void net_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){
|
---|
69 | buffer[pos++] = value >> 24;
|
---|
70 | buffer[pos++] = value >> 16;
|
---|
71 | buffer[pos++] = value >> 8;
|
---|
72 | buffer[pos++] = value;
|
---|
73 | }
|
---|
74 |
|
---|
75 | void reverse_bd_addr(bd_addr_t src, bd_addr_t dest){
|
---|
76 | dest[0] = src[5];
|
---|
77 | dest[1] = src[4];
|
---|
78 | dest[2] = src[3];
|
---|
79 | dest[3] = src[2];
|
---|
80 | dest[4] = src[1];
|
---|
81 | dest[5] = src[0];
|
---|
82 | }
|
---|
83 |
|
---|
84 | // general swap/endianess utils
|
---|
85 | void swapX(const uint8_t *src, uint8_t *dst, int len){
|
---|
86 | int i;
|
---|
87 | for (i = 0; i < len; i++)
|
---|
88 | dst[len - 1 - i] = src[i];
|
---|
89 | }
|
---|
90 | void swap24(const uint8_t src[3], uint8_t dst[3]){
|
---|
91 | swapX(src, dst, 3);
|
---|
92 | }
|
---|
93 | void swap56(const uint8_t src[7], uint8_t dst[7]){
|
---|
94 | swapX(src, dst, 7);
|
---|
95 | }
|
---|
96 | void swap64(const uint8_t src[8], uint8_t dst[8]){
|
---|
97 | swapX(src, dst, 8);
|
---|
98 | }
|
---|
99 | void swap128(const uint8_t src[16], uint8_t dst[16]){
|
---|
100 | swapX(src, dst, 16);
|
---|
101 | }
|
---|
102 |
|
---|
103 | char char_for_nibble(int nibble){
|
---|
104 | if (nibble < 10) return '0' + nibble;
|
---|
105 | nibble -= 10;
|
---|
106 | if (nibble < 6) return 'A' + nibble;
|
---|
107 | return '?';
|
---|
108 | }
|
---|
109 |
|
---|
110 | void printf_hexdump(const void *data, int size){
|
---|
111 | int i;
|
---|
112 | if (size <= 0) return;
|
---|
113 | for(i=0; i<size; i++) {
|
---|
114 | printf("%02X ", ((uint8_t *)data)[i]);
|
---|
115 | }
|
---|
116 | printf("\n");
|
---|
117 | }
|
---|
118 |
|
---|
119 | void hexdump(const void *data, int size){
|
---|
120 | char buffer[6*16+1];
|
---|
121 | int i, j;
|
---|
122 |
|
---|
123 | uint8_t low = 0x0F;
|
---|
124 | uint8_t high = 0xF0;
|
---|
125 | j = 0;
|
---|
126 | for (i=0; i<size;i++){
|
---|
127 | uint8_t byte = ((uint8_t *)data)[i];
|
---|
128 | buffer[j++] = '0';
|
---|
129 | buffer[j++] = 'x';
|
---|
130 | buffer[j++] = char_for_nibble((byte & high) >> 4);
|
---|
131 | buffer[j++] = char_for_nibble(byte & low);
|
---|
132 | buffer[j++] = ',';
|
---|
133 | buffer[j++] = ' ';
|
---|
134 | if (j >= 6*16 ){
|
---|
135 | buffer[j] = 0;
|
---|
136 | log_info("%s", buffer);
|
---|
137 | j = 0;
|
---|
138 | }
|
---|
139 | }
|
---|
140 | if (j != 0){
|
---|
141 | buffer[j] = 0;
|
---|
142 | log_info("%s", buffer);
|
---|
143 | }
|
---|
144 | }
|
---|
145 |
|
---|
146 | void hexdumpf(const void *data, int size){
|
---|
147 | char buffer[6*16+1];
|
---|
148 | int i, j;
|
---|
149 |
|
---|
150 | uint8_t low = 0x0F;
|
---|
151 | uint8_t high = 0xF0;
|
---|
152 | j = 0;
|
---|
153 | for (i=0; i<size;i++){
|
---|
154 | uint8_t byte = ((uint8_t *)data)[i];
|
---|
155 | buffer[j++] = '0';
|
---|
156 | buffer[j++] = 'x';
|
---|
157 | buffer[j++] = char_for_nibble((byte & high) >> 4);
|
---|
158 | buffer[j++] = char_for_nibble(byte & low);
|
---|
159 | buffer[j++] = ',';
|
---|
160 | buffer[j++] = ' ';
|
---|
161 | if (j >= 6*16 ){
|
---|
162 | buffer[j] = 0;
|
---|
163 | printf("%s\n", buffer);
|
---|
164 | j = 0;
|
---|
165 | }
|
---|
166 | }
|
---|
167 | if (j != 0){
|
---|
168 | buffer[j] = 0;
|
---|
169 | printf("%s\n", buffer);
|
---|
170 | }
|
---|
171 | }
|
---|
172 |
|
---|
173 | void log_key(const char * name, sm_key_t key){
|
---|
174 | log_info("%-6s ", name);
|
---|
175 | hexdump(key, 16);
|
---|
176 | }
|
---|
177 |
|
---|
178 | void printUUID128(uint8_t *uuid) {
|
---|
179 | printf("%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x",
|
---|
180 | uuid[0], uuid[1], uuid[2], uuid[3], uuid[4], uuid[5], uuid[6], uuid[7],
|
---|
181 | uuid[8], uuid[9], uuid[10], uuid[11], uuid[12], uuid[13], uuid[14], uuid[15]);
|
---|
182 | }
|
---|
183 |
|
---|
184 | static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0
|
---|
185 | char * bd_addr_to_str(bd_addr_t addr){
|
---|
186 | // orig code
|
---|
187 | // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]);
|
---|
188 | // sprintf-free code
|
---|
189 | char * p = bd_addr_to_str_buffer;
|
---|
190 | int i;
|
---|
191 | for (i = 0; i < 6 ; i++) {
|
---|
192 | *p++ = char_for_nibble((addr[i] >> 4) & 0x0F);
|
---|
193 | *p++ = char_for_nibble((addr[i] >> 0) & 0x0F);
|
---|
194 | *p++ = ':';
|
---|
195 | }
|
---|
196 | *--p = 0;
|
---|
197 | return (char *) bd_addr_to_str_buffer;
|
---|
198 | }
|
---|
199 |
|
---|
200 | static char link_key_to_str_buffer[LINK_KEY_LEN*2+1]; // 11223344556677889900112233445566\0
|
---|
201 | char *link_key_to_str(link_key_t link_key){
|
---|
202 | char * p = link_key_to_str_buffer;
|
---|
203 | int i;
|
---|
204 | for (i = 0; i < LINK_KEY_LEN ; i++) {
|
---|
205 | *p++ = char_for_nibble((link_key[i] >> 4) & 0x0F);
|
---|
206 | *p++ = char_for_nibble((link_key[i] >> 0) & 0x0F);
|
---|
207 | }
|
---|
208 | *p = 0;
|
---|
209 | return (char *) link_key_to_str_buffer;
|
---|
210 | }
|
---|
211 |
|
---|
212 | static char link_key_type_to_str_buffer[2];
|
---|
213 | char *link_key_type_to_str(link_key_type_t link_key){
|
---|
214 | snprintf(link_key_type_to_str_buffer, sizeof(link_key_type_to_str_buffer), "%d", link_key);
|
---|
215 | return (char *) link_key_type_to_str_buffer;
|
---|
216 | }
|
---|
217 |
|
---|
218 | void print_bd_addr( bd_addr_t addr){
|
---|
219 | log_info("%s", bd_addr_to_str(addr));
|
---|
220 | }
|
---|
221 |
|
---|
222 | #ifndef EMBEDDED
|
---|
223 | int sscan_bd_addr(uint8_t * addr_string, bd_addr_t addr){
|
---|
224 | unsigned int bd_addr_buffer[BD_ADDR_LEN]; //for sscanf, integer needed
|
---|
225 | // reset result buffer
|
---|
226 | memset(bd_addr_buffer, 0, sizeof(bd_addr_buffer));
|
---|
227 |
|
---|
228 | // parse
|
---|
229 | int result = sscanf( (char *) addr_string, "%2x:%2x:%2x:%2x:%2x:%2x", &bd_addr_buffer[0], &bd_addr_buffer[1], &bd_addr_buffer[2],
|
---|
230 | &bd_addr_buffer[3], &bd_addr_buffer[4], &bd_addr_buffer[5]);
|
---|
231 |
|
---|
232 | if (result != BD_ADDR_LEN) return 0;
|
---|
233 |
|
---|
234 | // store
|
---|
235 | int i;
|
---|
236 | for (i = 0; i < BD_ADDR_LEN; i++) {
|
---|
237 | addr[i] = (uint8_t) bd_addr_buffer[i];
|
---|
238 | }
|
---|
239 | return 1;
|
---|
240 | }
|
---|
241 |
|
---|
242 | int sscan_link_key(char * addr_string, link_key_t link_key){
|
---|
243 | unsigned int buffer[LINK_KEY_LEN];
|
---|
244 |
|
---|
245 | // reset result buffer
|
---|
246 | memset(&buffer, 0, sizeof(buffer));
|
---|
247 |
|
---|
248 | // parse
|
---|
249 | int result = sscanf( (char *) addr_string, "%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x",
|
---|
250 | &buffer[0], &buffer[1], &buffer[2], &buffer[3],
|
---|
251 | &buffer[4], &buffer[5], &buffer[6], &buffer[7],
|
---|
252 | &buffer[8], &buffer[9], &buffer[10], &buffer[11],
|
---|
253 | &buffer[12], &buffer[13], &buffer[14], &buffer[15] );
|
---|
254 |
|
---|
255 | if (result != LINK_KEY_LEN) return 0;
|
---|
256 |
|
---|
257 | // store
|
---|
258 | int i;
|
---|
259 | uint8_t *p = (uint8_t *) link_key;
|
---|
260 | for (i=0; i<LINK_KEY_LEN; i++ ) {
|
---|
261 | *p++ = (uint8_t) buffer[i];
|
---|
262 | }
|
---|
263 | return 1;
|
---|
264 | }
|
---|
265 |
|
---|
266 | #endif
|
---|
267 |
|
---|
268 |
|
---|
269 | // treat standard pairing as Authenticated as it uses a PIN
|
---|
270 | int is_authenticated_link_key(link_key_type_t link_key_type){
|
---|
271 | switch (link_key_type){
|
---|
272 | case COMBINATION_KEY:
|
---|
273 | case AUTHENTICATED_COMBINATION_KEY_GENERATED_FROM_P192:
|
---|
274 | case AUTHENTICATED_COMBINATION_KEY_GENERATED_FROM_P256:
|
---|
275 | return 1;
|
---|
276 | default:
|
---|
277 | return 0;
|
---|
278 | }
|
---|
279 | }
|
---|
280 |
|
---|
281 | /*
|
---|
282 | * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0.
|
---|
283 | */
|
---|
284 | static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */
|
---|
285 | 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B,
|
---|
286 | 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67,
|
---|
287 | 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43,
|
---|
288 | 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F,
|
---|
289 | 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B,
|
---|
290 | 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17,
|
---|
291 | 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33,
|
---|
292 | 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F,
|
---|
293 | 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B,
|
---|
294 | 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87,
|
---|
295 | 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3,
|
---|
296 | 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF,
|
---|
297 | 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB,
|
---|
298 | 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7,
|
---|
299 | 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3,
|
---|
300 | 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF
|
---|
301 | };
|
---|
302 |
|
---|
303 | #define CRC8_INIT 0xFF // Initial FCS value
|
---|
304 | #define CRC8_OK 0xCF // Good final FCS value
|
---|
305 | /*-----------------------------------------------------------------------------------*/
|
---|
306 | uint8_t crc8(uint8_t *data, uint16_t len)
|
---|
307 | {
|
---|
308 | uint16_t count;
|
---|
309 | uint8_t crc = CRC8_INIT;
|
---|
310 | for (count = 0; count < len; count++)
|
---|
311 | crc = crc8table[crc ^ data[count]];
|
---|
312 | return crc;
|
---|
313 | }
|
---|
314 |
|
---|
315 | /*-----------------------------------------------------------------------------------*/
|
---|
316 | uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum)
|
---|
317 | {
|
---|
318 | uint8_t crc;
|
---|
319 |
|
---|
320 | crc = crc8(data, len);
|
---|
321 |
|
---|
322 | crc = crc8table[crc ^ check_sum];
|
---|
323 | if (crc == CRC8_OK)
|
---|
324 | return 0; /* Valid */
|
---|
325 | else
|
---|
326 | return 1; /* Failed */
|
---|
327 |
|
---|
328 | }
|
---|
329 |
|
---|
330 | /*-----------------------------------------------------------------------------------*/
|
---|
331 | uint8_t crc8_calc(uint8_t *data, uint16_t len)
|
---|
332 | {
|
---|
333 | /* Ones complement */
|
---|
334 | return 0xFF - crc8(data, len);
|
---|
335 | }
|
---|
336 |
|
---|