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