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 if (!chr || (chr < '0') || (chr > '9')) 447 return val; 448 val = (val * 10u) + (uint8_t)(chr - '0'); 449 } 450 } 451 452 int string_len_for_uint32(uint32_t i){ 453 if (i < 10) return 1; 454 if (i < 100) return 2; 455 if (i < 1000) return 3; 456 if (i < 10000) return 4; 457 if (i < 100000) return 5; 458 if (i < 1000000) return 6; 459 if (i < 10000000) return 7; 460 if (i < 100000000) return 8; 461 if (i < 1000000000) return 9; 462 return 10; 463 } 464 465 int count_set_bits_uint32(uint32_t x){ 466 uint32_t v = x; 467 v = (v & 0x55555555) + ((v >> 1) & 0x55555555U); 468 v = (v & 0x33333333) + ((v >> 2) & 0x33333333U); 469 v = (v & 0x0F0F0F0F) + ((v >> 4) & 0x0F0F0F0FU); 470 v = (v & 0x00FF00FF) + ((v >> 8) & 0x00FF00FFU); 471 v = (v & 0x0000FFFF) + ((v >> 16) & 0x0000FFFFU); 472 return v; 473 } 474 475 uint8_t btstack_clz(uint32_t value) { 476 btstack_assert( value != 0 ); 477 #if defined(__GNUC__) || defined (__clang__) 478 // use gcc/clang intrinsic 479 return (uint8_t) __builtin_clz(value); 480 #elif defined(_MSC_VER) 481 // use MSVC intrinsic 482 DWORD leading_zero = 0; 483 _BitScanReverse( &leading_zero, value ); 484 return (uint8_t)(31 - leading_zero); 485 #else 486 // divide-and-conquer implementation for 32-bit integers 487 uint32_t x = value; 488 uint8_t r = 0; 489 if ((x & 0xffff0000u) == 0) { 490 x <<= 16; 491 r += 16; 492 } 493 if ((x & 0xff000000u) == 0) { 494 x <<= 8; 495 r += 8; 496 } 497 if ((x & 0xf0000000u) == 0) { 498 x <<= 4; 499 r += 4; 500 } 501 if ((x & 0xc0000000u) == 0) { 502 x <<= 2; 503 r += 2; 504 } 505 if ((x & 0x80000000u) == 0) { 506 x <<= 1; 507 r += 1; 508 } 509 return r; 510 #endif 511 } 512 513 /* 514 * CRC-8 (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 515 */ 516 517 #define CRC8_INIT 0xFF // Initial FCS value 518 #define CRC8_OK 0xCF // Good final FCS value 519 520 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 521 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 522 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 523 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 524 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 525 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 526 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 527 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 528 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 529 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 530 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 531 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 532 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 533 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 534 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 535 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 536 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 537 }; 538 539 /*-----------------------------------------------------------------------------------*/ 540 static uint8_t crc8(uint8_t *data, uint16_t len){ 541 uint16_t count; 542 uint8_t crc = CRC8_INIT; 543 for (count = 0; count < len; count++){ 544 crc = crc8table[crc ^ data[count]]; 545 } 546 return crc; 547 } 548 549 /*-----------------------------------------------------------------------------------*/ 550 uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 551 uint8_t crc; 552 crc = crc8(data, len); 553 crc = crc8table[crc ^ check_sum]; 554 if (crc == CRC8_OK){ 555 return 0; /* Valid */ 556 } else { 557 return 1; /* Failed */ 558 } 559 } 560 561 /*-----------------------------------------------------------------------------------*/ 562 uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 563 /* Ones complement */ 564 return 0xFFu - crc8(data, len); 565 } 566 567 /* 568 * CRC-32 lookup table as calculated by the table generator. Polynomial (normal) 0x04c11db7, ISO 3309 (HDLC) 569 * 570 * Created using pycrc tool (https://pycrc.org) with "crc-32" as model, and "table-driven" algorithm 571 * python3 pycrc.py --model crc-32 --algorithm table-driven --generate h -o crc32.h 572 * python3 pycrc.py --model crc-32 --algorithm table-driven --generate c -o crc32.c 573 */ 574 575 /** 576 * Static table used for the table_driven implementation. 577 */ 578 static const uint32_t crc_table[256] = { 579 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, 580 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 581 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, 582 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, 583 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 584 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, 585 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, 586 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 587 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, 588 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, 589 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 590 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, 591 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, 592 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 593 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, 594 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, 595 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 596 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, 597 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, 598 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 599 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, 600 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, 601 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 602 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, 603 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, 604 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 605 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, 606 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, 607 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 608 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, 609 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, 610 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d 611 }; 612 613 uint32_t btstack_crc32_init(void){ 614 return 0xffffffff; 615 } 616 617 uint32_t btstack_crc32_update(uint32_t crc, const uint8_t * data, uint32_t data_len){ 618 const uint8_t *d = data; 619 uint32_t tbl_idx; 620 621 while (data_len--) { 622 tbl_idx = (crc ^ *d) & 0xff; 623 crc = (crc_table[tbl_idx] ^ (crc >> 8)) & 0xffffffff; 624 d++; 625 } 626 return crc & 0xffffffff; 627 } 628 629 uint32_t btstack_crc32_finalize(uint32_t crc){ 630 return crc ^ 0xffffffff; 631 } 632 633 /*-----------------------------------------------------------------------------------*/ 634 635 uint16_t btstack_next_cid_ignoring_zero(uint16_t current_cid){ 636 uint16_t next_cid; 637 if (current_cid == 0xffff) { 638 next_cid = 1; 639 } else { 640 next_cid = current_cid + 1; 641 } 642 return next_cid; 643 } 644 645 uint16_t btstack_strcpy(char * dst, uint16_t dst_size, const char * src){ 646 uint16_t bytes_to_copy = (uint16_t) btstack_min( dst_size - 1, (uint16_t) strlen(src)); 647 (void) memcpy(dst, src, bytes_to_copy); 648 dst[bytes_to_copy] = 0; 649 return bytes_to_copy + 1; 650 } 651 652 void btstack_strcat(char * dst, uint16_t dst_size, const char * src){ 653 uint16_t src_len = (uint16_t) strlen(src); 654 uint16_t dst_len = (uint16_t) strlen(dst); 655 uint16_t bytes_to_copy = btstack_min( src_len, dst_size - dst_len - 1); 656 (void) memcpy( &dst[dst_len], src, bytes_to_copy); 657 dst[dst_len + bytes_to_copy] = 0; 658 } 659 660 int btstack_printf_strlen(const char * format, ...){ 661 va_list argptr; 662 va_start(argptr, format); 663 char dummy_buffer[1]; 664 int len = vsnprintf(dummy_buffer, sizeof(dummy_buffer), format, argptr); 665 va_end(argptr); 666 return len; 667 } 668 669 uint16_t btstack_snprintf_assert_complete(char * buffer, size_t size, const char * format, ...){ 670 va_list argptr; 671 va_start(argptr, format); 672 int len = vsnprintf(buffer, size, format, argptr); 673 va_end(argptr); 674 675 // check for no error and no truncation 676 btstack_assert(len >= 0); 677 btstack_assert((unsigned int) len < size); 678 return (uint16_t) len; 679 } 680 681 uint16_t btstack_snprintf_best_effort(char * buffer, size_t size, const char * format, ...){ 682 btstack_assert(size > 0); 683 va_list argptr; 684 va_start(argptr, format); 685 int len = vsnprintf(buffer, size, format, argptr); 686 va_end(argptr); 687 if (len < 0) { 688 // error -> len = 0 689 return 0; 690 } else { 691 // min of total string len and buffer size 692 return (uint16_t) btstack_min((uint32_t) len, (uint32_t) size - 1); 693 } 694 } 695 696 uint16_t btstack_virtual_memcpy( 697 const uint8_t * field_data, uint16_t field_len, uint16_t field_offset, // position of field in complete data block 698 uint8_t * buffer, uint16_t buffer_size, uint16_t buffer_offset){ 699 700 uint16_t after_buffer = buffer_offset + buffer_size ; 701 // bail before buffer 702 if ((field_offset + field_len) < buffer_offset){ 703 return 0; 704 } 705 // bail after buffer 706 if (field_offset >= after_buffer){ 707 return 0; 708 } 709 // calc overlap 710 uint16_t bytes_to_copy = field_len; 711 712 uint16_t skip_at_start = 0; 713 if (field_offset < buffer_offset){ 714 skip_at_start = buffer_offset - field_offset; 715 bytes_to_copy -= skip_at_start; 716 } 717 718 uint16_t skip_at_end = 0; 719 if ((field_offset + field_len) > after_buffer){ 720 skip_at_end = (field_offset + field_len) - after_buffer; 721 bytes_to_copy -= skip_at_end; 722 } 723 724 btstack_assert((skip_at_end + skip_at_start) <= field_len); 725 btstack_assert(bytes_to_copy <= field_len); 726 727 memcpy(&buffer[(field_offset + skip_at_start) - buffer_offset], &field_data[skip_at_start], bytes_to_copy); 728 return bytes_to_copy; 729 } 730