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