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 /* 39 * btstack_util.c 40 * 41 * General utility functions 42 * 43 * Created by Matthias Ringwald on 7/23/09. 44 */ 45 46 #include "btstack_config.h" 47 #include "btstack_util.h" 48 #include <stdio.h> 49 #include <string.h> 50 #include "btstack_debug.h" 51 52 void little_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 53 buffer[pos++] = value; 54 buffer[pos++] = value >> 8; 55 } 56 57 void little_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 58 buffer[pos++] = value; 59 buffer[pos++] = value >> 8; 60 buffer[pos++] = value >> 16; 61 buffer[pos++] = value >> 24; 62 } 63 64 void big_endian_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 65 buffer[pos++] = value >> 8; 66 buffer[pos++] = value; 67 } 68 69 void big_endian_store_32(uint8_t *buffer, uint16_t pos, uint32_t value){ 70 buffer[pos++] = value >> 24; 71 buffer[pos++] = value >> 16; 72 buffer[pos++] = value >> 8; 73 buffer[pos++] = value; 74 } 75 76 void bt_flip_addr(bd_addr_t dest, bd_addr_t src){ 77 dest[0] = src[5]; 78 dest[1] = src[4]; 79 dest[2] = src[3]; 80 dest[3] = src[2]; 81 dest[4] = src[1]; 82 dest[5] = src[0]; 83 } 84 85 // general swap/endianess utils 86 void swapX(const uint8_t *src, uint8_t *dst, int len){ 87 int i; 88 for (i = 0; i < len; i++) 89 dst[len - 1 - i] = src[i]; 90 } 91 void swap24(const uint8_t * src, uint8_t * dst){ 92 swapX(src, dst, 3); 93 } 94 void swap48(const uint8_t * src, uint8_t * dst){ 95 swapX(src, dst, 6); 96 } 97 void swap56(const uint8_t * src, uint8_t * dst){ 98 swapX(src, dst, 7); 99 } 100 void swap64(const uint8_t * src, uint8_t * dst){ 101 swapX(src, dst, 8); 102 } 103 void swap128(const uint8_t * src, uint8_t * dst){ 104 swapX(src, dst, 16); 105 } 106 107 char char_for_nibble(int nibble){ 108 if (nibble < 10) return '0' + nibble; 109 nibble -= 10; 110 if (nibble < 6) return 'A' + nibble; 111 return '?'; 112 } 113 114 void printf_hexdump(const void *data, int size){ 115 if (size <= 0) return; 116 int i; 117 for (i=0; i<size;i++){ 118 printf("%02X ", ((uint8_t *)data)[i]); 119 } 120 printf("\n"); 121 } 122 123 void hexdump(const void *data, int size){ 124 #ifdef ENABLE_LOG_INFO 125 char buffer[6*16+1]; 126 int i, j; 127 128 uint8_t low = 0x0F; 129 uint8_t high = 0xF0; 130 j = 0; 131 for (i=0; i<size;i++){ 132 uint8_t byte = ((uint8_t *)data)[i]; 133 buffer[j++] = '0'; 134 buffer[j++] = 'x'; 135 buffer[j++] = char_for_nibble((byte & high) >> 4); 136 buffer[j++] = char_for_nibble(byte & low); 137 buffer[j++] = ','; 138 buffer[j++] = ' '; 139 if (j >= 6*16 ){ 140 buffer[j] = 0; 141 log_info("%s", buffer); 142 j = 0; 143 } 144 } 145 if (j != 0){ 146 buffer[j] = 0; 147 log_info("%s", buffer); 148 } 149 #endif 150 } 151 152 void hexdumpf(const void *data, int size){ 153 char buffer[6*16+1]; 154 int i, j; 155 156 uint8_t low = 0x0F; 157 uint8_t high = 0xF0; 158 j = 0; 159 for (i=0; i<size;i++){ 160 uint8_t byte = ((uint8_t *)data)[i]; 161 buffer[j++] = '0'; 162 buffer[j++] = 'x'; 163 buffer[j++] = char_for_nibble((byte & high) >> 4); 164 buffer[j++] = char_for_nibble(byte & low); 165 buffer[j++] = ','; 166 buffer[j++] = ' '; 167 if (j >= 6*16 ){ 168 buffer[j] = 0; 169 printf("%s\n", buffer); 170 j = 0; 171 } 172 } 173 if (j != 0){ 174 buffer[j] = 0; 175 printf("%s\n", buffer); 176 } 177 } 178 179 void log_key(const char * name, sm_key_t key){ 180 log_info("%-6s ", name); 181 hexdump(key, 16); 182 } 183 184 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 185 const uint8_t sdp_bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 186 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 187 188 void sdp_normalize_uuid(uint8_t *uuid, uint32_t shortUUID){ 189 memcpy(uuid, sdp_bluetooth_base_uuid, 16); 190 big_endian_store_32(uuid, 0, shortUUID); 191 } 192 193 int sdp_has_blueooth_base_uuid(uint8_t * uuid128){ 194 return memcmp(&uuid128[4], &sdp_bluetooth_base_uuid[4], 12) == 0; 195 } 196 197 static char uuid128_to_str_buffer[32+4+1]; 198 char * uuid128_to_str(uint8_t * uuid){ 199 sprintf(uuid128_to_str_buffer, "%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x", 200 uuid[0], uuid[1], uuid[2], uuid[3], uuid[4], uuid[5], uuid[6], uuid[7], 201 uuid[8], uuid[9], uuid[10], uuid[11], uuid[12], uuid[13], uuid[14], uuid[15]); 202 return uuid128_to_str_buffer; 203 } 204 void printUUID128(uint8_t *uuid) { 205 printf("%s", uuid128_to_str(uuid)); 206 } 207 208 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 209 char * bd_addr_to_str(bd_addr_t addr){ 210 // orig code 211 // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 212 // sprintf-free code 213 char * p = bd_addr_to_str_buffer; 214 int i; 215 for (i = 0; i < 6 ; i++) { 216 *p++ = char_for_nibble((addr[i] >> 4) & 0x0F); 217 *p++ = char_for_nibble((addr[i] >> 0) & 0x0F); 218 *p++ = ':'; 219 } 220 *--p = 0; 221 return (char *) bd_addr_to_str_buffer; 222 } 223 224 static char link_key_to_str_buffer[LINK_KEY_STR_LEN+1]; // 11223344556677889900112233445566\0 225 char *link_key_to_str(link_key_t link_key){ 226 char * p = link_key_to_str_buffer; 227 int i; 228 for (i = 0; i < LINK_KEY_LEN ; i++) { 229 *p++ = char_for_nibble((link_key[i] >> 4) & 0x0F); 230 *p++ = char_for_nibble((link_key[i] >> 0) & 0x0F); 231 } 232 *p = 0; 233 return (char *) link_key_to_str_buffer; 234 } 235 236 static char link_key_type_to_str_buffer[2]; 237 char *link_key_type_to_str(link_key_type_t link_key){ 238 snprintf(link_key_type_to_str_buffer, sizeof(link_key_type_to_str_buffer), "%d", link_key); 239 return (char *) link_key_type_to_str_buffer; 240 } 241 242 void print_bd_addr( bd_addr_t addr){ 243 log_info("%s", bd_addr_to_str(addr)); 244 } 245 246 int sscan_bd_addr(uint8_t * addr_string, bd_addr_t addr){ 247 unsigned int bd_addr_buffer[BD_ADDR_LEN]; //for sscanf, integer needed 248 // reset result buffer 249 memset(bd_addr_buffer, 0, sizeof(bd_addr_buffer)); 250 251 // parse 252 int result = sscanf( (char *) addr_string, "%2x:%2x:%2x:%2x:%2x:%2x", &bd_addr_buffer[0], &bd_addr_buffer[1], &bd_addr_buffer[2], 253 &bd_addr_buffer[3], &bd_addr_buffer[4], &bd_addr_buffer[5]); 254 255 if (result != BD_ADDR_LEN) return 0; 256 257 // store 258 int i; 259 for (i = 0; i < BD_ADDR_LEN; i++) { 260 addr[i] = (uint8_t) bd_addr_buffer[i]; 261 } 262 return 1; 263 } 264 265 int sscan_link_key(char * addr_string, link_key_t link_key){ 266 unsigned int buffer[LINK_KEY_LEN]; 267 268 // reset result buffer 269 memset(&buffer, 0, sizeof(buffer)); 270 271 // parse 272 int result = sscanf( (char *) addr_string, "%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x", 273 &buffer[0], &buffer[1], &buffer[2], &buffer[3], 274 &buffer[4], &buffer[5], &buffer[6], &buffer[7], 275 &buffer[8], &buffer[9], &buffer[10], &buffer[11], 276 &buffer[12], &buffer[13], &buffer[14], &buffer[15] ); 277 278 if (result != LINK_KEY_LEN) return 0; 279 280 // store 281 int i; 282 uint8_t *p = (uint8_t *) link_key; 283 for (i=0; i<LINK_KEY_LEN; i++ ) { 284 *p++ = (uint8_t) buffer[i]; 285 } 286 return 1; 287 } 288 289 290 // treat standard pairing as Authenticated as it uses a PIN 291 int is_authenticated_link_key(link_key_type_t link_key_type){ 292 switch (link_key_type){ 293 case COMBINATION_KEY: 294 case AUTHENTICATED_COMBINATION_KEY_GENERATED_FROM_P192: 295 case AUTHENTICATED_COMBINATION_KEY_GENERATED_FROM_P256: 296 return 1; 297 default: 298 return 0; 299 } 300 } 301 302 /* 303 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 304 */ 305 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 306 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 307 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 308 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 309 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 310 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 311 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 312 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 313 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 314 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 315 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 316 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 317 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 318 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 319 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 320 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 321 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 322 }; 323 324 #define CRC8_INIT 0xFF // Initial FCS value 325 #define CRC8_OK 0xCF // Good final FCS value 326 /*-----------------------------------------------------------------------------------*/ 327 static uint8_t crc8(uint8_t *data, uint16_t len) 328 { 329 uint16_t count; 330 uint8_t crc = CRC8_INIT; 331 for (count = 0; count < len; count++) 332 crc = crc8table[crc ^ data[count]]; 333 return crc; 334 } 335 336 /*-----------------------------------------------------------------------------------*/ 337 uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum) 338 { 339 uint8_t crc; 340 341 crc = crc8(data, len); 342 343 crc = crc8table[crc ^ check_sum]; 344 if (crc == CRC8_OK) 345 return 0; /* Valid */ 346 else 347 return 1; /* Failed */ 348 349 } 350 351 /*-----------------------------------------------------------------------------------*/ 352 uint8_t crc8_calc(uint8_t *data, uint16_t len) 353 { 354 /* Ones complement */ 355 return 0xFF - crc8(data, len); 356 } 357 358