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