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