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