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 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 static const char * char_to_nibble = "0123456789ABCDEF"; 170 171 char char_for_nibble(int nibble){ 172 if (nibble < 16){ 173 return char_to_nibble[nibble]; 174 } else { 175 return '?'; 176 } 177 } 178 179 static inline char char_for_high_nibble(int value){ 180 return char_for_nibble((value >> 4) & 0x0f); 181 } 182 183 static inline char char_for_low_nibble(int value){ 184 return char_for_nibble(value & 0x0f); 185 } 186 187 188 int nibble_for_char(char c){ 189 if (c >= '0' && c <= '9') return c - '0'; 190 if (c >= 'a' && c <= 'f') return c - 'a' + 10; 191 if (c >= 'A' && c <= 'F') return c - 'A' + 10; 192 return -1; 193 } 194 195 void printf_hexdump(const void *data, int size){ 196 char buffer[4]; 197 buffer[2] = ' '; 198 buffer[3] = 0; 199 const uint8_t * ptr = (const uint8_t *) data; 200 while (size > 0){ 201 uint8_t byte = *ptr++; 202 buffer[0] = char_for_high_nibble(byte); 203 buffer[1] = char_for_low_nibble(byte); 204 printf("%s", buffer); 205 size--; 206 } 207 printf("\n"); 208 } 209 210 #if defined(ENABLE_LOG_INFO) || defined(ENABLE_LOG_DEBUG) 211 static void log_hexdump(int level, const void * data, int size){ 212 #define ITEMS_PER_LINE 16 213 // template '0x12, ' 214 #define BYTES_PER_BYTE 6 215 char buffer[BYTES_PER_BYTE*ITEMS_PER_LINE+1]; 216 int i, j; 217 j = 0; 218 for (i=0; i<size;i++){ 219 220 // help static analyzer proof that j stays within bounds 221 if (j > BYTES_PER_BYTE * (ITEMS_PER_LINE-1)){ 222 j = 0; 223 } 224 225 uint8_t byte = ((uint8_t *)data)[i]; 226 buffer[j++] = '0'; 227 buffer[j++] = 'x'; 228 buffer[j++] = char_for_high_nibble(byte); 229 buffer[j++] = char_for_low_nibble(byte); 230 buffer[j++] = ','; 231 buffer[j++] = ' '; 232 233 if (j >= BYTES_PER_BYTE * ITEMS_PER_LINE ){ 234 buffer[j] = 0; 235 HCI_DUMP_LOG(level, "%s", buffer); 236 j = 0; 237 } 238 } 239 if (j != 0){ 240 buffer[j] = 0; 241 HCI_DUMP_LOG(level, "%s", buffer); 242 } 243 } 244 #endif 245 246 void log_debug_hexdump(const void *data, int size){ 247 #ifdef ENABLE_LOG_DEBUG 248 log_hexdump(HCI_DUMP_LOG_LEVEL_DEBUG, data, size); 249 #else 250 UNUSED(data); // ok: no code 251 UNUSED(size); // ok: no code 252 #endif 253 } 254 255 void log_info_hexdump(const void *data, int size){ 256 #ifdef ENABLE_LOG_INFO 257 log_hexdump(HCI_DUMP_LOG_LEVEL_INFO, data, size); 258 #else 259 UNUSED(data); // ok: no code 260 UNUSED(size); // ok: no code 261 #endif 262 } 263 264 void log_info_key(const char * name, sm_key_t key){ 265 #ifdef ENABLE_LOG_INFO 266 char buffer[16*2+1]; 267 int i; 268 int j = 0; 269 for (i=0; i<16;i++){ 270 uint8_t byte = key[i]; 271 buffer[j++] = char_for_high_nibble(byte); 272 buffer[j++] = char_for_low_nibble(byte); 273 } 274 buffer[j] = 0; 275 log_info("%-6s %s", name, buffer); 276 #else 277 UNUSED(name); 278 (void)key; 279 #endif 280 } 281 282 // UUIDs are stored in big endian, similar to bd_addr_t 283 284 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 285 const uint8_t bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 286 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 287 288 void uuid_add_bluetooth_prefix(uint8_t *uuid, uint32_t shortUUID){ 289 memcpy(uuid, bluetooth_base_uuid, 16); 290 big_endian_store_32(uuid, 0, shortUUID); 291 } 292 293 int uuid_has_bluetooth_prefix(const uint8_t * uuid128){ 294 return memcmp(&uuid128[4], &bluetooth_base_uuid[4], 12) == 0; 295 } 296 297 static char uuid128_to_str_buffer[32+4+1]; 298 char * uuid128_to_str(const uint8_t * uuid){ 299 int i; 300 int j = 0; 301 // after 4, 6, 8, and 10 bytes = XYXYXYXY-XYXY-XYXY-XYXY-XYXYXYXYXYXY, there's a dash 302 const int dash_locations = (1<<3) | (1<<5) | (1<<7) | (1<<9); 303 for (i=0;i<16;i++){ 304 uint8_t byte = uuid[i]; 305 uuid128_to_str_buffer[j++] = char_for_high_nibble(byte); 306 uuid128_to_str_buffer[j++] = char_for_low_nibble(byte); 307 if (dash_locations & (1<<i)){ 308 uuid128_to_str_buffer[j++] = '-'; 309 } 310 } 311 return uuid128_to_str_buffer; 312 } 313 314 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 315 char * bd_addr_to_str(const bd_addr_t addr){ 316 // orig code 317 // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 318 // sprintf-free code 319 char * p = bd_addr_to_str_buffer; 320 int i; 321 for (i = 0; i < 6 ; i++) { 322 uint8_t byte = addr[i]; 323 *p++ = char_for_high_nibble(byte); 324 *p++ = char_for_low_nibble(byte); 325 *p++ = ':'; 326 } 327 *--p = 0; 328 return (char *) bd_addr_to_str_buffer; 329 } 330 331 static int scan_hex_byte(const char * byte_string){ 332 int upper_nibble = nibble_for_char(*byte_string++); 333 if (upper_nibble < 0) return -1; 334 int lower_nibble = nibble_for_char(*byte_string); 335 if (lower_nibble < 0) return -1; 336 return (upper_nibble << 4) | lower_nibble; 337 } 338 339 int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ 340 uint8_t buffer[BD_ADDR_LEN]; 341 int result = 0; 342 int i; 343 for (i = 0; i < BD_ADDR_LEN; i++) { 344 int single_byte = scan_hex_byte(addr_string); 345 if (single_byte < 0) break; 346 addr_string += 2; 347 buffer[i] = (uint8_t)single_byte; 348 // don't check seperator after last byte 349 if (i == BD_ADDR_LEN - 1) { 350 result = 1; 351 break; 352 } 353 // skip supported separators 354 char next_char = *addr_string; 355 if (next_char == ':' || next_char == '-' || next_char == ' ') { 356 addr_string++; 357 } 358 } 359 360 if (result){ 361 bd_addr_copy(addr, buffer); 362 } 363 return result; 364 } 365 366 uint32_t btstack_atoi(const char *str){ 367 uint32_t val = 0; 368 while (1){ 369 char chr = *str; 370 if (!chr || chr < '0' || chr > '9') 371 return val; 372 val = (val * 10) + (uint8_t)(chr - '0'); 373 str++; 374 } 375 } 376 377 int string_len_for_uint32(uint32_t i){ 378 if (i < 10) return 1; 379 if (i < 100) return 2; 380 if (i < 1000) return 3; 381 if (i < 10000) return 4; 382 if (i < 100000) return 5; 383 if (i < 1000000) return 6; 384 if (i < 10000000) return 7; 385 if (i < 100000000) return 8; 386 if (i < 1000000000) return 9; 387 return 10; 388 } 389 390 int count_set_bits_uint32(uint32_t x){ 391 x = (x & 0x55555555) + ((x >> 1) & 0x55555555); 392 x = (x & 0x33333333) + ((x >> 2) & 0x33333333); 393 x = (x & 0x0F0F0F0F) + ((x >> 4) & 0x0F0F0F0F); 394 x = (x & 0x00FF00FF) + ((x >> 8) & 0x00FF00FF); 395 x = (x & 0x0000FFFF) + ((x >> 16) & 0x0000FFFF); 396 return x; 397 } 398 399 /* 400 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 401 */ 402 403 #define CRC8_INIT 0xFF // Initial FCS value 404 #define CRC8_OK 0xCF // Good final FCS value 405 406 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 407 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 408 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 409 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 410 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 411 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 412 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 413 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 414 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 415 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 416 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 417 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 418 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 419 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 420 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 421 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 422 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 423 }; 424 425 /*-----------------------------------------------------------------------------------*/ 426 static uint8_t crc8(uint8_t *data, uint16_t len){ 427 uint16_t count; 428 uint8_t crc = CRC8_INIT; 429 for (count = 0; count < len; count++){ 430 crc = crc8table[crc ^ data[count]]; 431 } 432 return crc; 433 } 434 435 /*-----------------------------------------------------------------------------------*/ 436 uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 437 uint8_t crc; 438 crc = crc8(data, len); 439 crc = crc8table[crc ^ check_sum]; 440 if (crc == CRC8_OK){ 441 return 0; /* Valid */ 442 } else { 443 return 1; /* Failed */ 444 } 445 } 446 447 /*-----------------------------------------------------------------------------------*/ 448 uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 449 /* Ones complement */ 450 return 0xFF - crc8(data, len); 451 } 452