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 #define BTSTACK_FILE__ "btstack_util.c" 39 40 /* 41 * btstack_util.c 42 * 43 * General utility functions 44 * 45 * Created by Matthias Ringwald on 7/23/09. 46 */ 47 48 #include "btstack_config.h" 49 #include "btstack_debug.h" 50 #include "btstack_util.h" 51 52 #include <stdio.h> 53 #include <string.h> 54 55 /** 56 * @brief Compare two Bluetooth addresses 57 * @param a 58 * @param b 59 * @return 0 if equal 60 */ 61 int bd_addr_cmp(const bd_addr_t a, const bd_addr_t b){ 62 return memcmp(a,b, BD_ADDR_LEN); 63 } 64 65 /** 66 * @brief Copy Bluetooth address 67 * @param dest 68 * @param src 69 */ 70 void bd_addr_copy(bd_addr_t dest, const bd_addr_t src){ 71 (void)memcpy(dest, src, BD_ADDR_LEN); 72 } 73 74 uint16_t little_endian_read_16(const uint8_t * buffer, int pos){ 75 return (uint16_t)(((uint16_t) buffer[pos]) | (((uint16_t)buffer[(pos)+1]) << 8)); 76 } 77 uint32_t little_endian_read_24(const uint8_t * buffer, int pos){ 78 return ((uint32_t) buffer[pos]) | (((uint32_t)buffer[(pos)+1]) << 8) | (((uint32_t)buffer[(pos)+2]) << 16); 79 } 80 uint32_t little_endian_read_32(const uint8_t * buffer, int pos){ 81 return ((uint32_t) buffer[pos]) | (((uint32_t)buffer[(pos)+1]) << 8) | (((uint32_t)buffer[(pos)+2]) << 16) | (((uint32_t) buffer[(pos)+3]) << 24); 82 } 83 84 void little_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 85 buffer[pos++] = (uint8_t)value; 86 buffer[pos++] = (uint8_t)(value >> 8); 87 } 88 89 void little_endian_store_24(uint8_t *buffer, uint16_t pos, uint32_t value){ 90 buffer[pos++] = (uint8_t)(value); 91 buffer[pos++] = (uint8_t)(value >> 8); 92 buffer[pos++] = (uint8_t)(value >> 16); 93 } 94 95 void little_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 96 buffer[pos++] = (uint8_t)(value); 97 buffer[pos++] = (uint8_t)(value >> 8); 98 buffer[pos++] = (uint8_t)(value >> 16); 99 buffer[pos++] = (uint8_t)(value >> 24); 100 } 101 102 uint32_t big_endian_read_16( const uint8_t * buffer, int pos) { 103 return (uint16_t)(((uint16_t) buffer[(pos)+1]) | (((uint16_t)buffer[ pos ]) << 8)); 104 } 105 106 uint32_t big_endian_read_24( const uint8_t * buffer, int pos) { 107 return ( ((uint32_t)buffer[(pos)+2]) | (((uint32_t)buffer[(pos)+1]) << 8) | (((uint32_t) buffer[pos]) << 16)); 108 } 109 110 uint32_t big_endian_read_32( const uint8_t * buffer, int pos) { 111 return ((uint32_t) buffer[(pos)+3]) | (((uint32_t)buffer[(pos)+2]) << 8) | (((uint32_t)buffer[(pos)+1]) << 16) | (((uint32_t) buffer[pos]) << 24); 112 } 113 114 void big_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 115 buffer[pos++] = (uint8_t)(value >> 8); 116 buffer[pos++] = (uint8_t)(value); 117 } 118 119 void big_endian_store_24(uint8_t *buffer, uint16_t pos, uint32_t value){ 120 buffer[pos++] = (uint8_t)(value >> 16); 121 buffer[pos++] = (uint8_t)(value >> 8); 122 buffer[pos++] = (uint8_t)(value); 123 } 124 125 void big_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 126 buffer[pos++] = (uint8_t)(value >> 24); 127 buffer[pos++] = (uint8_t)(value >> 16); 128 buffer[pos++] = (uint8_t)(value >> 8); 129 buffer[pos++] = (uint8_t)(value); 130 } 131 132 // general swap/endianess utils 133 void reverse_bytes(const uint8_t *src, uint8_t *dst, int len){ 134 int i; 135 for (i = 0; i < len; i++) 136 dst[len - 1 - i] = src[i]; 137 } 138 void reverse_24(const uint8_t * src, uint8_t * dst){ 139 reverse_bytes(src, dst, 3); 140 } 141 void reverse_48(const uint8_t * src, uint8_t * dst){ 142 reverse_bytes(src, dst, 6); 143 } 144 void reverse_56(const uint8_t * src, uint8_t * dst){ 145 reverse_bytes(src, dst, 7); 146 } 147 void reverse_64(const uint8_t * src, uint8_t * dst){ 148 reverse_bytes(src, dst, 8); 149 } 150 void reverse_128(const uint8_t * src, uint8_t * dst){ 151 reverse_bytes(src, dst, 16); 152 } 153 void reverse_256(const uint8_t * src, uint8_t * dst){ 154 reverse_bytes(src, dst, 32); 155 } 156 157 void reverse_bd_addr(const bd_addr_t src, bd_addr_t dest){ 158 reverse_bytes(src, dest, 6); 159 } 160 161 uint32_t btstack_min(uint32_t a, uint32_t b){ 162 return (a < b) ? a : b; 163 } 164 165 uint32_t btstack_max(uint32_t a, uint32_t b){ 166 return (a > b) ? a : b; 167 } 168 169 /** 170 * @brief Calculate delta between two points in time 171 * @returns time_a - time_b - result > 0 if time_a is newer than time_b 172 */ 173 int32_t btstack_time_delta(uint32_t time_a, uint32_t time_b){ 174 return (int32_t)(time_a - time_b); 175 } 176 177 static const char * char_to_nibble = "0123456789ABCDEF"; 178 179 char char_for_nibble(int nibble){ 180 if (nibble < 16){ 181 return char_to_nibble[nibble]; 182 } else { 183 return '?'; 184 } 185 } 186 187 static inline char char_for_high_nibble(int value){ 188 return char_for_nibble((value >> 4) & 0x0f); 189 } 190 191 static inline char char_for_low_nibble(int value){ 192 return char_for_nibble(value & 0x0f); 193 } 194 195 196 int nibble_for_char(char c){ 197 if ((c >= '0') && (c <= '9')) return c - '0'; 198 if ((c >= 'a') && (c <= 'f')) return c - 'a' + 10; 199 if ((c >= 'A') && (c <= 'F')) return c - 'A' + 10; 200 return -1; 201 } 202 203 void printf_hexdump(const void *data, int size){ 204 char buffer[4]; 205 buffer[2] = ' '; 206 buffer[3] = 0; 207 const uint8_t * ptr = (const uint8_t *) data; 208 while (size > 0){ 209 uint8_t byte = *ptr++; 210 buffer[0] = char_for_high_nibble(byte); 211 buffer[1] = char_for_low_nibble(byte); 212 printf("%s", buffer); 213 size--; 214 } 215 printf("\n"); 216 } 217 218 #if defined(ENABLE_LOG_INFO) || defined(ENABLE_LOG_DEBUG) 219 static void log_hexdump(int level, const void * data, int size){ 220 #define ITEMS_PER_LINE 16 221 // template '0x12, ' 222 #define BYTES_PER_BYTE 6 223 char buffer[BYTES_PER_BYTE*ITEMS_PER_LINE+1]; 224 int i, j; 225 j = 0; 226 for (i=0; i<size;i++){ 227 228 // help static analyzer proof that j stays within bounds 229 if (j > (BYTES_PER_BYTE * (ITEMS_PER_LINE-1))){ 230 j = 0; 231 } 232 233 uint8_t byte = ((uint8_t *)data)[i]; 234 buffer[j++] = '0'; 235 buffer[j++] = 'x'; 236 buffer[j++] = char_for_high_nibble(byte); 237 buffer[j++] = char_for_low_nibble(byte); 238 buffer[j++] = ','; 239 buffer[j++] = ' '; 240 241 if (j >= (BYTES_PER_BYTE * ITEMS_PER_LINE) ){ 242 buffer[j] = 0; 243 HCI_DUMP_LOG(level, "%s", buffer); 244 j = 0; 245 } 246 } 247 if (j != 0){ 248 buffer[j] = 0; 249 HCI_DUMP_LOG(level, "%s", buffer); 250 } 251 } 252 #endif 253 254 void log_debug_hexdump(const void *data, int size){ 255 #ifdef ENABLE_LOG_DEBUG 256 log_hexdump(HCI_DUMP_LOG_LEVEL_DEBUG, data, size); 257 #else 258 UNUSED(data); // ok: no code 259 UNUSED(size); // ok: no code 260 #endif 261 } 262 263 void log_info_hexdump(const void *data, int size){ 264 #ifdef ENABLE_LOG_INFO 265 log_hexdump(HCI_DUMP_LOG_LEVEL_INFO, data, size); 266 #else 267 UNUSED(data); // ok: no code 268 UNUSED(size); // ok: no code 269 #endif 270 } 271 272 void log_info_key(const char * name, sm_key_t key){ 273 #ifdef ENABLE_LOG_INFO 274 char buffer[16*2+1]; 275 int i; 276 int j = 0; 277 for (i=0; i<16;i++){ 278 uint8_t byte = key[i]; 279 buffer[j++] = char_for_high_nibble(byte); 280 buffer[j++] = char_for_low_nibble(byte); 281 } 282 buffer[j] = 0; 283 log_info("%-6s %s", name, buffer); 284 #else 285 UNUSED(name); 286 (void)key; 287 #endif 288 } 289 290 // UUIDs are stored in big endian, similar to bd_addr_t 291 292 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 293 const uint8_t bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 294 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 295 296 void uuid_add_bluetooth_prefix(uint8_t *uuid, uint32_t shortUUID){ 297 (void)memcpy(uuid, bluetooth_base_uuid, 16); 298 big_endian_store_32(uuid, 0, shortUUID); 299 } 300 301 int uuid_has_bluetooth_prefix(const uint8_t * uuid128){ 302 return memcmp(&uuid128[4], &bluetooth_base_uuid[4], 12) == 0; 303 } 304 305 static char uuid128_to_str_buffer[32+4+1]; 306 char * uuid128_to_str(const uint8_t * uuid){ 307 int i; 308 int j = 0; 309 // after 4, 6, 8, and 10 bytes = XYXYXYXY-XYXY-XYXY-XYXY-XYXYXYXYXYXY, there's a dash 310 const int dash_locations = (1<<3) | (1<<5) | (1<<7) | (1<<9); 311 for (i=0;i<16;i++){ 312 uint8_t byte = uuid[i]; 313 uuid128_to_str_buffer[j++] = char_for_high_nibble(byte); 314 uuid128_to_str_buffer[j++] = char_for_low_nibble(byte); 315 if (dash_locations & (1<<i)){ 316 uuid128_to_str_buffer[j++] = '-'; 317 } 318 } 319 return uuid128_to_str_buffer; 320 } 321 322 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 323 char * bd_addr_to_str(const bd_addr_t addr){ 324 // orig code 325 // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 326 // sprintf-free code 327 char * p = bd_addr_to_str_buffer; 328 int i; 329 for (i = 0; i < 6 ; i++) { 330 uint8_t byte = addr[i]; 331 *p++ = char_for_high_nibble(byte); 332 *p++ = char_for_low_nibble(byte); 333 *p++ = ':'; 334 } 335 *--p = 0; 336 return (char *) bd_addr_to_str_buffer; 337 } 338 339 static int scan_hex_byte(const char * byte_string){ 340 int upper_nibble = nibble_for_char(*byte_string++); 341 if (upper_nibble < 0) return -1; 342 int lower_nibble = nibble_for_char(*byte_string); 343 if (lower_nibble < 0) return -1; 344 return (upper_nibble << 4) | lower_nibble; 345 } 346 347 int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ 348 uint8_t buffer[BD_ADDR_LEN]; 349 int result = 0; 350 int i; 351 for (i = 0; i < BD_ADDR_LEN; i++) { 352 int single_byte = scan_hex_byte(addr_string); 353 if (single_byte < 0) break; 354 addr_string += 2; 355 buffer[i] = (uint8_t)single_byte; 356 // don't check seperator after last byte 357 if (i == (BD_ADDR_LEN - 1)) { 358 result = 1; 359 break; 360 } 361 // skip supported separators 362 char next_char = *addr_string; 363 if ((next_char == ':') || (next_char == '-') || (next_char == ' ')) { 364 addr_string++; 365 } 366 } 367 368 if (result){ 369 bd_addr_copy(addr, buffer); 370 } 371 return result; 372 } 373 374 uint32_t btstack_atoi(const char *str){ 375 uint32_t val = 0; 376 while (true){ 377 char chr = *str; 378 if (!chr || (chr < '0') || (chr > '9')) 379 return val; 380 val = (val * 10) + (uint8_t)(chr - '0'); 381 str++; 382 } 383 } 384 385 int string_len_for_uint32(uint32_t i){ 386 if (i < 10) return 1; 387 if (i < 100) return 2; 388 if (i < 1000) return 3; 389 if (i < 10000) return 4; 390 if (i < 100000) return 5; 391 if (i < 1000000) return 6; 392 if (i < 10000000) return 7; 393 if (i < 100000000) return 8; 394 if (i < 1000000000) return 9; 395 return 10; 396 } 397 398 int count_set_bits_uint32(uint32_t x){ 399 x = (x & 0x55555555) + ((x >> 1) & 0x55555555); 400 x = (x & 0x33333333) + ((x >> 2) & 0x33333333); 401 x = (x & 0x0F0F0F0F) + ((x >> 4) & 0x0F0F0F0F); 402 x = (x & 0x00FF00FF) + ((x >> 8) & 0x00FF00FF); 403 x = (x & 0x0000FFFF) + ((x >> 16) & 0x0000FFFF); 404 return x; 405 } 406 407 /* 408 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 409 */ 410 411 #define CRC8_INIT 0xFF // Initial FCS value 412 #define CRC8_OK 0xCF // Good final FCS value 413 414 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 415 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 416 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 417 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 418 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 419 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 420 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 421 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 422 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 423 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 424 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 425 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 426 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 427 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 428 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 429 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 430 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 431 }; 432 433 /*-----------------------------------------------------------------------------------*/ 434 static uint8_t crc8(uint8_t *data, uint16_t len){ 435 uint16_t count; 436 uint8_t crc = CRC8_INIT; 437 for (count = 0; count < len; count++){ 438 crc = crc8table[crc ^ data[count]]; 439 } 440 return crc; 441 } 442 443 /*-----------------------------------------------------------------------------------*/ 444 uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 445 uint8_t crc; 446 crc = crc8(data, len); 447 crc = crc8table[crc ^ check_sum]; 448 if (crc == CRC8_OK){ 449 return 0; /* Valid */ 450 } else { 451 return 1; /* Failed */ 452 } 453 } 454 455 /*-----------------------------------------------------------------------------------*/ 456 uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 457 /* Ones complement */ 458 return 0xFF - crc8(data, len); 459 } 460