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