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