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 #ifdef _MSC_VER 45 #include <Windows.h> 46 #include <intrin.h> 47 #endif 48 49 #include "btstack_config.h" 50 #include "btstack_debug.h" 51 #include "btstack_util.h" 52 53 #include <stdio.h> // vsnprintf 54 #include <string.h> // memcpy 55 56 57 58 /** 59 * @brief Compare two Bluetooth addresses 60 * @param a 61 * @param b 62 * @return 0 if equal 63 */ 64 int bd_addr_cmp(const bd_addr_t a, const bd_addr_t b){ 65 return memcmp(a,b, BD_ADDR_LEN); 66 } 67 68 /** 69 * @brief Copy Bluetooth address 70 * @param dest 71 * @param src 72 */ 73 void bd_addr_copy(bd_addr_t dest, const bd_addr_t src){ 74 (void)memcpy(dest, src, BD_ADDR_LEN); 75 } 76 77 uint8_t little_endian_read_08(const uint8_t* buffer, int position) { 78 return (uint8_t)buffer[position]; 79 } 80 uint16_t little_endian_read_16(const uint8_t * buffer, int position){ 81 return (uint16_t)(((uint16_t) buffer[position]) | (((uint16_t)buffer[position+1]) << 8)); 82 } 83 uint32_t little_endian_read_24(const uint8_t * buffer, int position){ 84 return ((uint32_t) buffer[position]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t)buffer[position+2]) << 16); 85 } 86 uint32_t little_endian_read_32(const uint8_t * buffer, int position){ 87 return ((uint32_t) buffer[position]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t)buffer[position+2]) << 16) | (((uint32_t) buffer[position+3]) << 24); 88 } 89 90 void little_endian_store_08(uint8_t* buffer, uint16_t position, uint8_t value) { 91 uint16_t pos = position; 92 buffer[pos] = value; 93 } 94 95 void little_endian_store_16(uint8_t * buffer, uint16_t position, uint16_t value){ 96 uint16_t pos = position; 97 buffer[pos++] = (uint8_t)value; 98 buffer[pos++] = (uint8_t)(value >> 8); 99 } 100 101 void little_endian_store_24(uint8_t * buffer, uint16_t position, uint32_t value){ 102 uint16_t pos = position; 103 buffer[pos++] = (uint8_t)(value); 104 buffer[pos++] = (uint8_t)(value >> 8); 105 buffer[pos++] = (uint8_t)(value >> 16); 106 } 107 108 void little_endian_store_32(uint8_t * buffer, uint16_t position, uint32_t value){ 109 uint16_t pos = position; 110 buffer[pos++] = (uint8_t)(value); 111 buffer[pos++] = (uint8_t)(value >> 8); 112 buffer[pos++] = (uint8_t)(value >> 16); 113 buffer[pos++] = (uint8_t)(value >> 24); 114 } 115 116 uint32_t big_endian_read_08(const uint8_t* buffer, int position) { 117 return buffer[position]; 118 } 119 120 uint32_t big_endian_read_16(const uint8_t * buffer, int position) { 121 return (uint16_t)(((uint16_t) buffer[position+1]) | (((uint16_t)buffer[position]) << 8)); 122 } 123 124 uint32_t big_endian_read_24(const uint8_t * buffer, int position) { 125 return ( ((uint32_t)buffer[position+2]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t) buffer[position]) << 16)); 126 } 127 128 uint32_t big_endian_read_32(const uint8_t * buffer, int position) { 129 return ((uint32_t) buffer[position+3]) | (((uint32_t)buffer[position+2]) << 8) | (((uint32_t)buffer[position+1]) << 16) | (((uint32_t) buffer[position]) << 24); 130 } 131 132 void big_endian_store_08(uint8_t* buffer, uint16_t position, uint8_t value) { 133 uint16_t pos = position; 134 buffer[pos++] = (uint8_t)(value); 135 } 136 137 void big_endian_store_16(uint8_t * buffer, uint16_t position, uint16_t value){ 138 uint16_t pos = position; 139 buffer[pos++] = (uint8_t)(value >> 8); 140 buffer[pos++] = (uint8_t)(value); 141 } 142 143 void big_endian_store_24(uint8_t * buffer, uint16_t position, uint32_t value){ 144 uint16_t pos = position; 145 buffer[pos++] = (uint8_t)(value >> 16); 146 buffer[pos++] = (uint8_t)(value >> 8); 147 buffer[pos++] = (uint8_t)(value); 148 } 149 150 void big_endian_store_32(uint8_t * buffer, uint16_t position, uint32_t value){ 151 uint16_t pos = position; 152 buffer[pos++] = (uint8_t)(value >> 24); 153 buffer[pos++] = (uint8_t)(value >> 16); 154 buffer[pos++] = (uint8_t)(value >> 8); 155 buffer[pos++] = (uint8_t)(value); 156 } 157 158 // general swap/endianess utils 159 void reverse_bytes(const uint8_t * src, uint8_t * dest, int len){ 160 int i; 161 for (i = 0; i < len; i++) 162 dest[len - 1 - i] = src[i]; 163 } 164 void reverse_24(const uint8_t * src, uint8_t * dest){ 165 reverse_bytes(src, dest, 3); 166 } 167 void reverse_48(const uint8_t * src, uint8_t * dest){ 168 reverse_bytes(src, dest, 6); 169 } 170 void reverse_56(const uint8_t * src, uint8_t * dest){ 171 reverse_bytes(src, dest, 7); 172 } 173 void reverse_64(const uint8_t * src, uint8_t * dest){ 174 reverse_bytes(src, dest, 8); 175 } 176 void reverse_128(const uint8_t * src, uint8_t * dest){ 177 reverse_bytes(src, dest, 16); 178 } 179 void reverse_256(const uint8_t * src, uint8_t * dest){ 180 reverse_bytes(src, dest, 32); 181 } 182 183 void reverse_bd_addr(const bd_addr_t src, bd_addr_t dest){ 184 reverse_bytes(src, dest, 6); 185 } 186 187 bool btstack_is_null(const uint8_t * buffer, uint16_t size){ 188 uint16_t i; 189 for (i=0; i < size ; i++){ 190 if (buffer[i] != 0) { 191 return false; 192 } 193 } 194 return true; 195 } 196 197 bool btstack_is_null_bd_addr( const bd_addr_t addr ){ 198 return btstack_is_null( addr, sizeof(bd_addr_t) ); 199 } 200 201 uint32_t btstack_min(uint32_t a, uint32_t b){ 202 return (a < b) ? a : b; 203 } 204 205 uint32_t btstack_max(uint32_t a, uint32_t b){ 206 return (a > b) ? a : b; 207 } 208 209 /** 210 * @brief Calculate delta between two uint32_t points in time 211 * @return time_a - time_b - result > 0 if time_a is newer than time_b 212 */ 213 int32_t btstack_time_delta(uint32_t time_a, uint32_t time_b){ 214 return (int32_t)(time_a - time_b); 215 } 216 217 /** 218 * @brief Calculate delta between two uint16_t points in time 219 * @return time_a - time_b - result > 0 if time_a is newer than time_b 220 */ 221 int16_t btstack_time16_delta(uint16_t time_a, uint16_t time_b){ 222 return (int16_t)(time_a - time_b); 223 } 224 225 char char_for_nibble(uint8_t nibble){ 226 227 static const char * char_to_nibble = "0123456789ABCDEF"; 228 229 if (nibble < 16){ 230 return char_to_nibble[nibble]; 231 } else { 232 return '?'; 233 } 234 } 235 236 static inline char char_for_high_nibble(int value){ 237 return char_for_nibble((value >> 4) & 0x0f); 238 } 239 240 static inline char char_for_low_nibble(int value){ 241 return char_for_nibble(value & 0x0f); 242 } 243 244 245 int nibble_for_char(char c){ 246 if ((c >= '0') && (c <= '9')) return c - '0'; 247 if ((c >= 'a') && (c <= 'f')) return c - 'a' + 10; 248 if ((c >= 'A') && (c <= 'F')) return c - 'A' + 10; 249 return -1; 250 } 251 252 #ifdef ENABLE_PRINTF_HEXDUMP 253 void printf_hexdump(const void * data, int size){ 254 char buffer[4]; 255 buffer[2] = ' '; 256 buffer[3] = 0; 257 const uint8_t * ptr = (const uint8_t *) data; 258 while (size > 0){ 259 uint8_t byte = *ptr++; 260 buffer[0] = char_for_high_nibble(byte); 261 buffer[1] = char_for_low_nibble(byte); 262 printf("%s", buffer); 263 size--; 264 } 265 printf("\n"); 266 } 267 #endif 268 269 #if defined(ENABLE_LOG_INFO) || defined(ENABLE_LOG_DEBUG) 270 static void log_hexdump(int level, const void * data, int size){ 271 #define ITEMS_PER_LINE 16 272 // template '0x12, ' 273 #define BYTES_PER_BYTE 6 274 char buffer[BYTES_PER_BYTE*ITEMS_PER_LINE+1]; 275 int i, j; 276 j = 0; 277 for (i=0; i<size;i++){ 278 279 // help static analyzer proof that j stays within bounds 280 if (j > (BYTES_PER_BYTE * (ITEMS_PER_LINE-1))){ 281 j = 0; 282 } 283 284 uint8_t byte = ((uint8_t *)data)[i]; 285 buffer[j++] = '0'; 286 buffer[j++] = 'x'; 287 buffer[j++] = char_for_high_nibble(byte); 288 buffer[j++] = char_for_low_nibble(byte); 289 buffer[j++] = ','; 290 buffer[j++] = ' '; 291 292 if (j >= (BYTES_PER_BYTE * ITEMS_PER_LINE) ){ 293 buffer[j] = 0; 294 hci_dump_log(level, "%s", buffer); 295 j = 0; 296 } 297 } 298 if (j != 0){ 299 buffer[j] = 0; 300 hci_dump_log(level, "%s", buffer); 301 } 302 } 303 #endif 304 305 void log_debug_hexdump(const void * data, int size){ 306 #ifdef ENABLE_LOG_DEBUG 307 log_hexdump(HCI_DUMP_LOG_LEVEL_DEBUG, data, size); 308 #else 309 UNUSED(data); // ok: no code 310 UNUSED(size); // ok: no code 311 #endif 312 } 313 314 void log_info_hexdump(const void * data, int size){ 315 #ifdef ENABLE_LOG_INFO 316 log_hexdump(HCI_DUMP_LOG_LEVEL_INFO, data, size); 317 #else 318 UNUSED(data); // ok: no code 319 UNUSED(size); // ok: no code 320 #endif 321 } 322 323 void log_info_key(const char * name, sm_key_t key){ 324 #ifdef ENABLE_LOG_INFO 325 char buffer[16*2+1]; 326 int i; 327 int j = 0; 328 for (i=0; i<16;i++){ 329 uint8_t byte = key[i]; 330 buffer[j++] = char_for_high_nibble(byte); 331 buffer[j++] = char_for_low_nibble(byte); 332 } 333 buffer[j] = 0; 334 log_info("%-6s %s", name, buffer); 335 #else 336 UNUSED(name); 337 (void)key; 338 #endif 339 } 340 341 // UUIDs are stored in big endian, similar to bd_addr_t 342 343 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 344 const uint8_t bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 345 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 346 347 void uuid_add_bluetooth_prefix(uint8_t * uuid128, uint32_t short_uuid){ 348 (void)memcpy(uuid128, bluetooth_base_uuid, 16); 349 big_endian_store_32(uuid128, 0, short_uuid); 350 } 351 352 bool uuid_has_bluetooth_prefix(const uint8_t * uuid128){ 353 return memcmp(&uuid128[4], &bluetooth_base_uuid[4], 12) == 0; 354 } 355 356 static char uuid128_to_str_buffer[32+4+1]; 357 char * uuid128_to_str(const uint8_t * uuid){ 358 int i; 359 int j = 0; 360 // after 4, 6, 8, and 10 bytes = XYXYXYXY-XYXY-XYXY-XYXY-XYXYXYXYXYXY, there's a dash 361 const int dash_locations = (1<<3) | (1<<5) | (1<<7) | (1<<9); 362 for (i=0;i<16;i++){ 363 uint8_t byte = uuid[i]; 364 uuid128_to_str_buffer[j++] = char_for_high_nibble(byte); 365 uuid128_to_str_buffer[j++] = char_for_low_nibble(byte); 366 if (dash_locations & (1<<i)){ 367 uuid128_to_str_buffer[j++] = '-'; 368 } 369 } 370 return uuid128_to_str_buffer; 371 } 372 373 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 374 char * bd_addr_to_str_with_delimiter(const bd_addr_t addr, char delimiter){ 375 char * p = bd_addr_to_str_buffer; 376 int i; 377 for (i = 0; i < 6 ; i++) { 378 uint8_t byte = addr[i]; 379 *p++ = char_for_high_nibble(byte); 380 *p++ = char_for_low_nibble(byte); 381 *p++ = delimiter; 382 } 383 *--p = 0; 384 return (char *) bd_addr_to_str_buffer; 385 } 386 387 char * bd_addr_to_str(const bd_addr_t addr){ 388 return bd_addr_to_str_with_delimiter(addr, ':'); 389 } 390 391 void btstack_replace_bd_addr_placeholder(uint8_t * buffer, uint16_t size, const bd_addr_t address){ 392 const int bd_addr_string_len = 17; 393 uint16_t i = 0; 394 while ((i + bd_addr_string_len) <= size){ 395 if (memcmp(&buffer[i], "00:00:00:00:00:00", bd_addr_string_len)) { 396 i++; 397 continue; 398 } 399 // set address 400 (void)memcpy(&buffer[i], bd_addr_to_str(address), bd_addr_string_len); 401 i += bd_addr_string_len; 402 } 403 } 404 405 static int scan_hex_byte(const char * byte_string){ 406 int upper_nibble = nibble_for_char(byte_string[0]); 407 if (upper_nibble < 0) return -1; 408 int lower_nibble = nibble_for_char(byte_string[1]); 409 if (lower_nibble < 0) return -1; 410 return (upper_nibble << 4) | lower_nibble; 411 } 412 413 int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ 414 const char * the_string = addr_string; 415 uint8_t buffer[BD_ADDR_LEN]; 416 int result = 0; 417 int i; 418 for (i = 0; i < BD_ADDR_LEN; i++) { 419 int single_byte = scan_hex_byte(the_string); 420 if (single_byte < 0) break; 421 the_string += 2; 422 buffer[i] = (uint8_t)single_byte; 423 // don't check separator after last byte 424 if (i == (BD_ADDR_LEN - 1)) { 425 result = 1; 426 break; 427 } 428 // skip supported separators 429 char next_char = *the_string; 430 if ((next_char == ':') || (next_char == '-') || (next_char == ' ')) { 431 the_string++; 432 } 433 } 434 435 if (result != 0){ 436 bd_addr_copy(addr, buffer); 437 } 438 return result; 439 } 440 441 uint32_t btstack_atoi(const char * str){ 442 const char * the_string = str; 443 uint32_t val = 0; 444 while (true){ 445 char chr = *the_string++; 446 // skip whitespace 447 if (((chr >= 0x09) && (chr <= 0x0d)) || (chr == ' ')) { 448 continue; 449 } 450 if (!chr || (chr < '0') || (chr > '9')){ 451 return val; 452 } 453 val = (val * 10u) + (uint8_t)(chr - '0'); 454 } 455 } 456 457 int string_len_for_uint32(uint32_t i){ 458 if (i < 10) return 1; 459 if (i < 100) return 2; 460 if (i < 1000) return 3; 461 if (i < 10000) return 4; 462 if (i < 100000) return 5; 463 if (i < 1000000) return 6; 464 if (i < 10000000) return 7; 465 if (i < 100000000) return 8; 466 if (i < 1000000000) return 9; 467 return 10; 468 } 469 470 int count_set_bits_uint32(uint32_t x){ 471 uint32_t v = x; 472 v = (v & 0x55555555) + ((v >> 1) & 0x55555555U); 473 v = (v & 0x33333333) + ((v >> 2) & 0x33333333U); 474 v = (v & 0x0F0F0F0F) + ((v >> 4) & 0x0F0F0F0FU); 475 v = (v & 0x00FF00FF) + ((v >> 8) & 0x00FF00FFU); 476 v = (v & 0x0000FFFF) + ((v >> 16) & 0x0000FFFFU); 477 return v; 478 } 479 480 uint8_t btstack_clz(uint32_t value) { 481 btstack_assert( value != 0 ); 482 #if defined(__GNUC__) || defined (__clang__) 483 // use gcc/clang intrinsic 484 return (uint8_t) __builtin_clz(value); 485 #elif defined(_MSC_VER) 486 // use MSVC intrinsic 487 DWORD leading_zero = 0; 488 _BitScanReverse( &leading_zero, value ); 489 return (uint8_t)(31 - leading_zero); 490 #else 491 // divide-and-conquer implementation for 32-bit integers 492 uint32_t x = value; 493 uint8_t r = 0; 494 if ((x & 0xffff0000u) == 0) { 495 x <<= 16; 496 r += 16; 497 } 498 if ((x & 0xff000000u) == 0) { 499 x <<= 8; 500 r += 8; 501 } 502 if ((x & 0xf0000000u) == 0) { 503 x <<= 4; 504 r += 4; 505 } 506 if ((x & 0xc0000000u) == 0) { 507 x <<= 2; 508 r += 2; 509 } 510 if ((x & 0x80000000u) == 0) { 511 x <<= 1; 512 r += 1; 513 } 514 return r; 515 #endif 516 } 517 518 /* 519 * CRC-8 (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 520 */ 521 522 #define CRC8_INIT 0xFF // Initial FCS value 523 #define CRC8_OK 0xCF // Good final FCS value 524 525 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 526 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 527 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 528 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 529 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 530 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 531 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 532 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 533 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 534 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 535 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 536 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 537 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 538 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 539 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 540 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 541 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 542 }; 543 544 /*-----------------------------------------------------------------------------------*/ 545 static uint8_t crc8(uint8_t *data, uint16_t len){ 546 uint16_t count; 547 uint8_t crc = CRC8_INIT; 548 for (count = 0; count < len; count++){ 549 crc = crc8table[crc ^ data[count]]; 550 } 551 return crc; 552 } 553 554 /*-----------------------------------------------------------------------------------*/ 555 uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 556 uint8_t crc; 557 crc = crc8(data, len); 558 crc = crc8table[crc ^ check_sum]; 559 if (crc == CRC8_OK){ 560 return 0; /* Valid */ 561 } else { 562 return 1; /* Failed */ 563 } 564 } 565 566 /*-----------------------------------------------------------------------------------*/ 567 uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 568 /* Ones complement */ 569 return 0xFFu - crc8(data, len); 570 } 571 572 /* 573 * CRC-32 lookup table as calculated by the table generator. Polynomial (normal) 0x04c11db7, ISO 3309 (HDLC) 574 * 575 * Created using pycrc tool (https://pycrc.org) with "crc-32" as model, and "table-driven" algorithm 576 * python3 pycrc.py --model crc-32 --algorithm table-driven --generate h -o crc32.h 577 * python3 pycrc.py --model crc-32 --algorithm table-driven --generate c -o crc32.c 578 */ 579 580 /** 581 * Static table used for the table_driven implementation. 582 */ 583 static const uint32_t crc_table[256] = { 584 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 585 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 586 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 587 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, 588 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 589 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, 590 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, 591 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 592 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, 593 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, 594 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 595 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, 596 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 597 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 598 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, 599 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, 600 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 601 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, 602 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 603 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 604 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, 605 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, 606 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 607 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, 608 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, 609 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 610 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, 611 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 612 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 613 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 614 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 615 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d 616 }; 617 618 uint32_t btstack_crc32_init(void){ 619 return 0xffffffff; 620 } 621 622 uint32_t btstack_crc32_update(uint32_t crc, const uint8_t * data, uint32_t data_len){ 623 const uint8_t *d = data; 624 uint32_t tbl_idx; 625 626 while (data_len--) { 627 tbl_idx = (crc ^ *d) & 0xff; 628 crc = (crc_table[tbl_idx] ^ (crc >> 8)) & 0xffffffff; 629 d++; 630 } 631 return crc & 0xffffffff; 632 } 633 634 uint32_t btstack_crc32_finalize(uint32_t crc){ 635 return crc ^ 0xffffffff; 636 } 637 638 /*-----------------------------------------------------------------------------------*/ 639 640 uint16_t btstack_next_cid_ignoring_zero(uint16_t current_cid){ 641 uint16_t next_cid; 642 if (current_cid == 0xffff) { 643 next_cid = 1; 644 } else { 645 next_cid = current_cid + 1; 646 } 647 return next_cid; 648 } 649 650 uint16_t btstack_strcpy(char * dst, uint16_t dst_size, const char * src){ 651 uint16_t bytes_to_copy = (uint16_t) btstack_min( dst_size - 1, (uint16_t) strlen(src)); 652 (void) memcpy(dst, src, bytes_to_copy); 653 dst[bytes_to_copy] = 0; 654 return bytes_to_copy + 1; 655 } 656 657 void btstack_strcat(char * dst, uint16_t dst_size, const char * src){ 658 uint16_t src_len = (uint16_t) strlen(src); 659 uint16_t dst_len = (uint16_t) strlen(dst); 660 uint16_t bytes_to_copy = btstack_min( src_len, dst_size - dst_len - 1); 661 (void) memcpy( &dst[dst_len], src, bytes_to_copy); 662 dst[dst_len + bytes_to_copy] = 0; 663 } 664 665 int btstack_printf_strlen(const char * format, ...){ 666 va_list argptr; 667 va_start(argptr, format); 668 char dummy_buffer[1]; 669 int len = vsnprintf(dummy_buffer, sizeof(dummy_buffer), format, argptr); 670 va_end(argptr); 671 return len; 672 } 673 674 uint16_t btstack_snprintf_assert_complete(char * buffer, size_t size, const char * format, ...){ 675 va_list argptr; 676 va_start(argptr, format); 677 int len = vsnprintf(buffer, size, format, argptr); 678 va_end(argptr); 679 680 // check for no error and no truncation 681 btstack_assert(len >= 0); 682 btstack_assert((unsigned int) len < size); 683 return (uint16_t) len; 684 } 685 686 uint16_t btstack_snprintf_best_effort(char * buffer, size_t size, const char * format, ...){ 687 btstack_assert(size > 0); 688 va_list argptr; 689 va_start(argptr, format); 690 int len = vsnprintf(buffer, size, format, argptr); 691 va_end(argptr); 692 if (len < 0) { 693 // error -> len = 0 694 return 0; 695 } else { 696 // min of total string len and buffer size 697 return (uint16_t) btstack_min((uint32_t) len, (uint32_t) size - 1); 698 } 699 } 700 701 uint16_t btstack_virtual_memcpy( 702 const uint8_t * field_data, uint16_t field_len, uint16_t field_offset, // position of field in complete data block 703 uint8_t * buffer, uint16_t buffer_size, uint16_t buffer_offset){ 704 705 uint16_t after_buffer = buffer_offset + buffer_size ; 706 // bail before buffer 707 if ((field_offset + field_len) < buffer_offset){ 708 return 0; 709 } 710 // bail after buffer 711 if (field_offset >= after_buffer){ 712 return 0; 713 } 714 // calc overlap 715 uint16_t bytes_to_copy = field_len; 716 717 uint16_t skip_at_start = 0; 718 if (field_offset < buffer_offset){ 719 skip_at_start = buffer_offset - field_offset; 720 bytes_to_copy -= skip_at_start; 721 } 722 723 uint16_t skip_at_end = 0; 724 if ((field_offset + field_len) > after_buffer){ 725 skip_at_end = (field_offset + field_len) - after_buffer; 726 bytes_to_copy -= skip_at_end; 727 } 728 729 btstack_assert((skip_at_end + skip_at_start) <= field_len); 730 btstack_assert(bytes_to_copy <= field_len); 731 732 memcpy(&buffer[(field_offset + skip_at_start) - buffer_offset], &field_data[skip_at_start], bytes_to_copy); 733 return bytes_to_copy; 734 } 735