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 // orig code 327 // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 328 // sprintf-free code 329 char * p = bd_addr_to_str_buffer; 330 int i; 331 for (i = 0; i < 6 ; i++) { 332 uint8_t byte = addr[i]; 333 *p++ = char_for_high_nibble(byte); 334 *p++ = char_for_low_nibble(byte); 335 *p++ = ':'; 336 } 337 *--p = 0; 338 return (char *) bd_addr_to_str_buffer; 339 } 340 341 void btstack_replace_bd_addr_placeholder(uint8_t * buffer, uint16_t size, const bd_addr_t address){ 342 const int bd_addr_string_len = 17; 343 uint16_t i = 0; 344 while ((i + bd_addr_string_len) <= size){ 345 if (memcmp(&buffer[i], "00:00:00:00:00:00", bd_addr_string_len)) { 346 i++; 347 continue; 348 } 349 // set address 350 (void)memcpy(&buffer[i], bd_addr_to_str(address), bd_addr_string_len); 351 i += bd_addr_string_len; 352 } 353 } 354 355 static int scan_hex_byte(const char * byte_string){ 356 int upper_nibble = nibble_for_char(*byte_string++); 357 if (upper_nibble < 0) return -1; 358 int lower_nibble = nibble_for_char(*byte_string); 359 if (lower_nibble < 0) return -1; 360 return (upper_nibble << 4) | lower_nibble; 361 } 362 363 int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ 364 uint8_t buffer[BD_ADDR_LEN]; 365 int result = 0; 366 int i; 367 for (i = 0; i < BD_ADDR_LEN; i++) { 368 int single_byte = scan_hex_byte(addr_string); 369 if (single_byte < 0) break; 370 addr_string += 2; 371 buffer[i] = (uint8_t)single_byte; 372 // don't check seperator after last byte 373 if (i == (BD_ADDR_LEN - 1)) { 374 result = 1; 375 break; 376 } 377 // skip supported separators 378 char next_char = *addr_string; 379 if ((next_char == ':') || (next_char == '-') || (next_char == ' ')) { 380 addr_string++; 381 } 382 } 383 384 if (result){ 385 bd_addr_copy(addr, buffer); 386 } 387 return result; 388 } 389 390 uint32_t btstack_atoi(const char *str){ 391 uint32_t val = 0; 392 while (true){ 393 char chr = *str; 394 if (!chr || (chr < '0') || (chr > '9')) 395 return val; 396 val = (val * 10u) + (uint8_t)(chr - '0'); 397 str++; 398 } 399 } 400 401 int string_len_for_uint32(uint32_t i){ 402 if (i < 10) return 1; 403 if (i < 100) return 2; 404 if (i < 1000) return 3; 405 if (i < 10000) return 4; 406 if (i < 100000) return 5; 407 if (i < 1000000) return 6; 408 if (i < 10000000) return 7; 409 if (i < 100000000) return 8; 410 if (i < 1000000000) return 9; 411 return 10; 412 } 413 414 int count_set_bits_uint32(uint32_t x){ 415 x = (x & 0x55555555) + ((x >> 1) & 0x55555555); 416 x = (x & 0x33333333) + ((x >> 2) & 0x33333333); 417 x = (x & 0x0F0F0F0F) + ((x >> 4) & 0x0F0F0F0F); 418 x = (x & 0x00FF00FF) + ((x >> 8) & 0x00FF00FF); 419 x = (x & 0x0000FFFF) + ((x >> 16) & 0x0000FFFF); 420 return x; 421 } 422 423 /* 424 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 425 */ 426 427 #define CRC8_INIT 0xFF // Initial FCS value 428 #define CRC8_OK 0xCF // Good final FCS value 429 430 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 431 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 432 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 433 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 434 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 435 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 436 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 437 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 438 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 439 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 440 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 441 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 442 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 443 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 444 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 445 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 446 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 447 }; 448 449 /*-----------------------------------------------------------------------------------*/ 450 static uint8_t crc8(uint8_t *data, uint16_t len){ 451 uint16_t count; 452 uint8_t crc = CRC8_INIT; 453 for (count = 0; count < len; count++){ 454 crc = crc8table[crc ^ data[count]]; 455 } 456 return crc; 457 } 458 459 /*-----------------------------------------------------------------------------------*/ 460 uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 461 uint8_t crc; 462 crc = crc8(data, len); 463 crc = crc8table[crc ^ check_sum]; 464 if (crc == CRC8_OK){ 465 return 0; /* Valid */ 466 } else { 467 return 1; /* Failed */ 468 } 469 } 470 471 /*-----------------------------------------------------------------------------------*/ 472 uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 473 /* Ones complement */ 474 return 0xFFu - crc8(data, len); 475 } 476