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