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 MATTHIAS 24 * RINGWALD 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 * btstack_util.c 42 * 43 * General utility functions 44 * 45 * Created by Matthias Ringwald on 7/23/09. 46 */ 47 48 #include "btstack_config.h" 49 #include "btstack_debug.h" 50 #include "btstack_util.h" 51 52 #ifdef ENABLE_PRINTF_HEXDUMP 53 #include <stdio.h> 54 #endif 55 56 #include <string.h> 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 pos){ 78 return (uint16_t)(((uint16_t) buffer[pos]) | (((uint16_t)buffer[(pos)+1]) << 8)); 79 } 80 uint32_t little_endian_read_24(const uint8_t * buffer, int pos){ 81 return ((uint32_t) buffer[pos]) | (((uint32_t)buffer[(pos)+1]) << 8) | (((uint32_t)buffer[(pos)+2]) << 16); 82 } 83 uint32_t little_endian_read_32(const uint8_t * buffer, int pos){ 84 return ((uint32_t) buffer[pos]) | (((uint32_t)buffer[(pos)+1]) << 8) | (((uint32_t)buffer[(pos)+2]) << 16) | (((uint32_t) buffer[(pos)+3]) << 24); 85 } 86 87 void little_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 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 pos, uint32_t value){ 93 buffer[pos++] = (uint8_t)(value); 94 buffer[pos++] = (uint8_t)(value >> 8); 95 buffer[pos++] = (uint8_t)(value >> 16); 96 } 97 98 void little_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 99 buffer[pos++] = (uint8_t)(value); 100 buffer[pos++] = (uint8_t)(value >> 8); 101 buffer[pos++] = (uint8_t)(value >> 16); 102 buffer[pos++] = (uint8_t)(value >> 24); 103 } 104 105 uint32_t big_endian_read_16( const uint8_t * buffer, int pos) { 106 return (uint16_t)(((uint16_t) buffer[(pos)+1]) | (((uint16_t)buffer[ pos ]) << 8)); 107 } 108 109 uint32_t big_endian_read_24( const uint8_t * buffer, int pos) { 110 return ( ((uint32_t)buffer[(pos)+2]) | (((uint32_t)buffer[(pos)+1]) << 8) | (((uint32_t) buffer[pos]) << 16)); 111 } 112 113 uint32_t big_endian_read_32( const uint8_t * buffer, int pos) { 114 return ((uint32_t) buffer[(pos)+3]) | (((uint32_t)buffer[(pos)+2]) << 8) | (((uint32_t)buffer[(pos)+1]) << 16) | (((uint32_t) buffer[pos]) << 24); 115 } 116 117 void big_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 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 pos, uint32_t value){ 123 buffer[pos++] = (uint8_t)(value >> 16); 124 buffer[pos++] = (uint8_t)(value >> 8); 125 buffer[pos++] = (uint8_t)(value); 126 } 127 128 void big_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 129 buffer[pos++] = (uint8_t)(value >> 24); 130 buffer[pos++] = (uint8_t)(value >> 16); 131 buffer[pos++] = (uint8_t)(value >> 8); 132 buffer[pos++] = (uint8_t)(value); 133 } 134 135 // general swap/endianess utils 136 void reverse_bytes(const uint8_t *src, uint8_t *dst, int len){ 137 int i; 138 for (i = 0; i < len; i++) 139 dst[len - 1 - i] = src[i]; 140 } 141 void reverse_24(const uint8_t * src, uint8_t * dst){ 142 reverse_bytes(src, dst, 3); 143 } 144 void reverse_48(const uint8_t * src, uint8_t * dst){ 145 reverse_bytes(src, dst, 6); 146 } 147 void reverse_56(const uint8_t * src, uint8_t * dst){ 148 reverse_bytes(src, dst, 7); 149 } 150 void reverse_64(const uint8_t * src, uint8_t * dst){ 151 reverse_bytes(src, dst, 8); 152 } 153 void reverse_128(const uint8_t * src, uint8_t * dst){ 154 reverse_bytes(src, dst, 16); 155 } 156 void reverse_256(const uint8_t * src, uint8_t * dst){ 157 reverse_bytes(src, dst, 32); 158 } 159 160 void reverse_bd_addr(const bd_addr_t src, bd_addr_t dest){ 161 reverse_bytes(src, dest, 6); 162 } 163 164 uint32_t btstack_min(uint32_t a, uint32_t b){ 165 return (a < b) ? a : b; 166 } 167 168 uint32_t btstack_max(uint32_t a, uint32_t b){ 169 return (a > b) ? a : b; 170 } 171 172 /** 173 * @brief Calculate delta between two points in time 174 * @returns time_a - time_b - result > 0 if time_a is newer than time_b 175 */ 176 int32_t btstack_time_delta(uint32_t time_a, uint32_t time_b){ 177 return (int32_t)(time_a - time_b); 178 } 179 180 181 char char_for_nibble(int nibble){ 182 183 static const char * char_to_nibble = "0123456789ABCDEF"; 184 185 if (nibble < 16){ 186 return char_to_nibble[nibble]; 187 } else { 188 return '?'; 189 } 190 } 191 192 static inline char char_for_high_nibble(int value){ 193 return char_for_nibble((value >> 4) & 0x0f); 194 } 195 196 static inline char char_for_low_nibble(int value){ 197 return char_for_nibble(value & 0x0f); 198 } 199 200 201 int nibble_for_char(char c){ 202 if ((c >= '0') && (c <= '9')) return c - '0'; 203 if ((c >= 'a') && (c <= 'f')) return c - 'a' + 10; 204 if ((c >= 'A') && (c <= 'F')) return c - 'A' + 10; 205 return -1; 206 } 207 208 #ifdef ENABLE_PRINTF_HEXDUMP 209 void printf_hexdump(const void *data, int size){ 210 char buffer[4]; 211 buffer[2] = ' '; 212 buffer[3] = 0; 213 const uint8_t * ptr = (const uint8_t *) data; 214 while (size > 0){ 215 uint8_t byte = *ptr++; 216 buffer[0] = char_for_high_nibble(byte); 217 buffer[1] = char_for_low_nibble(byte); 218 printf("%s", buffer); 219 size--; 220 } 221 printf("\n"); 222 } 223 #endif 224 225 #if defined(ENABLE_LOG_INFO) || defined(ENABLE_LOG_DEBUG) 226 static void log_hexdump(int level, const void * data, int size){ 227 #define ITEMS_PER_LINE 16 228 // template '0x12, ' 229 #define BYTES_PER_BYTE 6 230 char buffer[BYTES_PER_BYTE*ITEMS_PER_LINE+1]; 231 int i, j; 232 j = 0; 233 for (i=0; i<size;i++){ 234 235 // help static analyzer proof that j stays within bounds 236 if (j > (BYTES_PER_BYTE * (ITEMS_PER_LINE-1))){ 237 j = 0; 238 } 239 240 uint8_t byte = ((uint8_t *)data)[i]; 241 buffer[j++] = '0'; 242 buffer[j++] = 'x'; 243 buffer[j++] = char_for_high_nibble(byte); 244 buffer[j++] = char_for_low_nibble(byte); 245 buffer[j++] = ','; 246 buffer[j++] = ' '; 247 248 if (j >= (BYTES_PER_BYTE * ITEMS_PER_LINE) ){ 249 buffer[j] = 0; 250 HCI_DUMP_LOG(level, "%s", buffer); 251 j = 0; 252 } 253 } 254 if (j != 0){ 255 buffer[j] = 0; 256 HCI_DUMP_LOG(level, "%s", buffer); 257 } 258 } 259 #endif 260 261 void log_debug_hexdump(const void *data, int size){ 262 #ifdef ENABLE_LOG_DEBUG 263 log_hexdump(HCI_DUMP_LOG_LEVEL_DEBUG, data, size); 264 #else 265 UNUSED(data); // ok: no code 266 UNUSED(size); // ok: no code 267 #endif 268 } 269 270 void log_info_hexdump(const void *data, int size){ 271 #ifdef ENABLE_LOG_INFO 272 log_hexdump(HCI_DUMP_LOG_LEVEL_INFO, data, size); 273 #else 274 UNUSED(data); // ok: no code 275 UNUSED(size); // ok: no code 276 #endif 277 } 278 279 void log_info_key(const char * name, sm_key_t key){ 280 #ifdef ENABLE_LOG_INFO 281 char buffer[16*2+1]; 282 int i; 283 int j = 0; 284 for (i=0; i<16;i++){ 285 uint8_t byte = key[i]; 286 buffer[j++] = char_for_high_nibble(byte); 287 buffer[j++] = char_for_low_nibble(byte); 288 } 289 buffer[j] = 0; 290 log_info("%-6s %s", name, buffer); 291 #else 292 UNUSED(name); 293 (void)key; 294 #endif 295 } 296 297 // UUIDs are stored in big endian, similar to bd_addr_t 298 299 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 300 const uint8_t bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 301 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 302 303 void uuid_add_bluetooth_prefix(uint8_t *uuid, uint32_t shortUUID){ 304 (void)memcpy(uuid, bluetooth_base_uuid, 16); 305 big_endian_store_32(uuid, 0, shortUUID); 306 } 307 308 int uuid_has_bluetooth_prefix(const uint8_t * uuid128){ 309 return memcmp(&uuid128[4], &bluetooth_base_uuid[4], 12) == 0; 310 } 311 312 static char uuid128_to_str_buffer[32+4+1]; 313 char * uuid128_to_str(const uint8_t * uuid){ 314 int i; 315 int j = 0; 316 // after 4, 6, 8, and 10 bytes = XYXYXYXY-XYXY-XYXY-XYXY-XYXYXYXYXYXY, there's a dash 317 const int dash_locations = (1<<3) | (1<<5) | (1<<7) | (1<<9); 318 for (i=0;i<16;i++){ 319 uint8_t byte = uuid[i]; 320 uuid128_to_str_buffer[j++] = char_for_high_nibble(byte); 321 uuid128_to_str_buffer[j++] = char_for_low_nibble(byte); 322 if (dash_locations & (1<<i)){ 323 uuid128_to_str_buffer[j++] = '-'; 324 } 325 } 326 return uuid128_to_str_buffer; 327 } 328 329 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 330 char * bd_addr_to_str(const bd_addr_t addr){ 331 char * p = bd_addr_to_str_buffer; 332 int i; 333 for (i = 0; i < 6 ; i++) { 334 uint8_t byte = addr[i]; 335 *p++ = char_for_high_nibble(byte); 336 *p++ = char_for_low_nibble(byte); 337 *p++ = ':'; 338 } 339 *--p = 0; 340 return (char *) bd_addr_to_str_buffer; 341 } 342 343 void btstack_replace_bd_addr_placeholder(uint8_t * buffer, uint16_t size, const bd_addr_t address){ 344 const int bd_addr_string_len = 17; 345 uint16_t i = 0; 346 while ((i + bd_addr_string_len) <= size){ 347 if (memcmp(&buffer[i], "00:00:00:00:00:00", bd_addr_string_len)) { 348 i++; 349 continue; 350 } 351 // set address 352 (void)memcpy(&buffer[i], bd_addr_to_str(address), bd_addr_string_len); 353 i += bd_addr_string_len; 354 } 355 } 356 357 static int scan_hex_byte(const char * byte_string){ 358 int upper_nibble = nibble_for_char(*byte_string++); 359 if (upper_nibble < 0) return -1; 360 int lower_nibble = nibble_for_char(*byte_string); 361 if (lower_nibble < 0) return -1; 362 return (upper_nibble << 4) | lower_nibble; 363 } 364 365 int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ 366 uint8_t buffer[BD_ADDR_LEN]; 367 int result = 0; 368 int i; 369 for (i = 0; i < BD_ADDR_LEN; i++) { 370 int single_byte = scan_hex_byte(addr_string); 371 if (single_byte < 0) break; 372 addr_string += 2; 373 buffer[i] = (uint8_t)single_byte; 374 // don't check seperator after last byte 375 if (i == (BD_ADDR_LEN - 1)) { 376 result = 1; 377 break; 378 } 379 // skip supported separators 380 char next_char = *addr_string; 381 if ((next_char == ':') || (next_char == '-') || (next_char == ' ')) { 382 addr_string++; 383 } 384 } 385 386 if (result != 0){ 387 bd_addr_copy(addr, buffer); 388 } 389 return result; 390 } 391 392 uint32_t btstack_atoi(const char *str){ 393 uint32_t val = 0; 394 while (true){ 395 char chr = *str; 396 if (!chr || (chr < '0') || (chr > '9')) 397 return val; 398 val = (val * 10u) + (uint8_t)(chr - '0'); 399 str++; 400 } 401 } 402 403 int string_len_for_uint32(uint32_t i){ 404 if (i < 10) return 1; 405 if (i < 100) return 2; 406 if (i < 1000) return 3; 407 if (i < 10000) return 4; 408 if (i < 100000) return 5; 409 if (i < 1000000) return 6; 410 if (i < 10000000) return 7; 411 if (i < 100000000) return 8; 412 if (i < 1000000000) return 9; 413 return 10; 414 } 415 416 int count_set_bits_uint32(uint32_t x){ 417 x = (x & 0x55555555) + ((x >> 1) & 0x55555555); 418 x = (x & 0x33333333) + ((x >> 2) & 0x33333333); 419 x = (x & 0x0F0F0F0F) + ((x >> 4) & 0x0F0F0F0F); 420 x = (x & 0x00FF00FF) + ((x >> 8) & 0x00FF00FF); 421 x = (x & 0x0000FFFF) + ((x >> 16) & 0x0000FFFF); 422 return x; 423 } 424 425 /* 426 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 427 */ 428 429 #define CRC8_INIT 0xFF // Initial FCS value 430 #define CRC8_OK 0xCF // Good final FCS value 431 432 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 433 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 434 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 435 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 436 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 437 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 438 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 439 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 440 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 441 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 442 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 443 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 444 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 445 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 446 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 447 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 448 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 449 }; 450 451 /*-----------------------------------------------------------------------------------*/ 452 static uint8_t crc8(uint8_t *data, uint16_t len){ 453 uint16_t count; 454 uint8_t crc = CRC8_INIT; 455 for (count = 0; count < len; count++){ 456 crc = crc8table[crc ^ data[count]]; 457 } 458 return crc; 459 } 460 461 /*-----------------------------------------------------------------------------------*/ 462 uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 463 uint8_t crc; 464 crc = crc8(data, len); 465 crc = crc8table[crc ^ check_sum]; 466 if (crc == CRC8_OK){ 467 return 0; /* Valid */ 468 } else { 469 return 1; /* Failed */ 470 } 471 } 472 473 /*-----------------------------------------------------------------------------------*/ 474 uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 475 /* Ones complement */ 476 return 0xFFu - crc8(data, len); 477 } 478