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 bt_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 53 buffer[pos++] = value; 54 buffer[pos++] = value >> 8; 55 } 56 57 void bt_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 net_store_16(uint8_t *buffer, uint16_t pos, uint16_t value){ 65 buffer[pos++] = value >> 8; 66 buffer[pos++] = value; 67 } 68 69 void net_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[3], uint8_t dst[3]){ 92 swapX(src, dst, 3); 93 } 94 void swap56(const uint8_t src[7], uint8_t dst[7]){ 95 swapX(src, dst, 7); 96 } 97 void swap64(const uint8_t src[8], uint8_t dst[8]){ 98 swapX(src, dst, 8); 99 } 100 void swap128(const uint8_t src[16], uint8_t dst[16]){ 101 swapX(src, dst, 16); 102 } 103 104 char char_for_nibble(int nibble){ 105 if (nibble < 10) return '0' + nibble; 106 nibble -= 10; 107 if (nibble < 6) return 'A' + nibble; 108 return '?'; 109 } 110 111 void printf_hexdump(const void *data, int size){ 112 if (size <= 0) return; 113 int i; 114 for (i=0; i<size;i++){ 115 printf("%02X ", ((uint8_t *)data)[i]); 116 } 117 printf("\n"); 118 } 119 120 void hexdump(const void *data, int size){ 121 #ifdef ENABLE_LOG_INFO 122 char buffer[6*16+1]; 123 int i, j; 124 125 uint8_t low = 0x0F; 126 uint8_t high = 0xF0; 127 j = 0; 128 for (i=0; i<size;i++){ 129 uint8_t byte = ((uint8_t *)data)[i]; 130 buffer[j++] = '0'; 131 buffer[j++] = 'x'; 132 buffer[j++] = char_for_nibble((byte & high) >> 4); 133 buffer[j++] = char_for_nibble(byte & low); 134 buffer[j++] = ','; 135 buffer[j++] = ' '; 136 if (j >= 6*16 ){ 137 buffer[j] = 0; 138 log_info("%s", buffer); 139 j = 0; 140 } 141 } 142 if (j != 0){ 143 buffer[j] = 0; 144 log_info("%s", buffer); 145 } 146 #endif 147 } 148 149 void hexdumpf(const void *data, int size){ 150 char buffer[6*16+1]; 151 int i, j; 152 153 uint8_t low = 0x0F; 154 uint8_t high = 0xF0; 155 j = 0; 156 for (i=0; i<size;i++){ 157 uint8_t byte = ((uint8_t *)data)[i]; 158 buffer[j++] = '0'; 159 buffer[j++] = 'x'; 160 buffer[j++] = char_for_nibble((byte & high) >> 4); 161 buffer[j++] = char_for_nibble(byte & low); 162 buffer[j++] = ','; 163 buffer[j++] = ' '; 164 if (j >= 6*16 ){ 165 buffer[j] = 0; 166 printf("%s\n", buffer); 167 j = 0; 168 } 169 } 170 if (j != 0){ 171 buffer[j] = 0; 172 printf("%s\n", buffer); 173 } 174 } 175 176 void log_key(const char * name, sm_key_t key){ 177 log_info("%-6s ", name); 178 hexdump(key, 16); 179 } 180 181 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 182 const uint8_t sdp_bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 183 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 184 185 void sdp_normalize_uuid(uint8_t *uuid, uint32_t shortUUID){ 186 memcpy(uuid, sdp_bluetooth_base_uuid, 16); 187 net_store_32(uuid, 0, shortUUID); 188 } 189 190 int sdp_has_blueooth_base_uuid(uint8_t * uuid128){ 191 return memcmp(&uuid128[4], &sdp_bluetooth_base_uuid[4], 12) == 0; 192 } 193 194 static char uuid128_to_str_buffer[32+4+1]; 195 char * uuid128_to_str(uint8_t * uuid){ 196 sprintf(uuid128_to_str_buffer, "%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x", 197 uuid[0], uuid[1], uuid[2], uuid[3], uuid[4], uuid[5], uuid[6], uuid[7], 198 uuid[8], uuid[9], uuid[10], uuid[11], uuid[12], uuid[13], uuid[14], uuid[15]); 199 return uuid128_to_str_buffer; 200 } 201 void printUUID128(uint8_t *uuid) { 202 printf("%s", uuid128_to_str(uuid)); 203 } 204 205 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 206 char * bd_addr_to_str(bd_addr_t addr){ 207 // orig code 208 // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 209 // sprintf-free code 210 char * p = bd_addr_to_str_buffer; 211 int i; 212 for (i = 0; i < 6 ; i++) { 213 *p++ = char_for_nibble((addr[i] >> 4) & 0x0F); 214 *p++ = char_for_nibble((addr[i] >> 0) & 0x0F); 215 *p++ = ':'; 216 } 217 *--p = 0; 218 return (char *) bd_addr_to_str_buffer; 219 } 220 221 static char link_key_to_str_buffer[LINK_KEY_STR_LEN+1]; // 11223344556677889900112233445566\0 222 char *link_key_to_str(link_key_t link_key){ 223 char * p = link_key_to_str_buffer; 224 int i; 225 for (i = 0; i < LINK_KEY_LEN ; i++) { 226 *p++ = char_for_nibble((link_key[i] >> 4) & 0x0F); 227 *p++ = char_for_nibble((link_key[i] >> 0) & 0x0F); 228 } 229 *p = 0; 230 return (char *) link_key_to_str_buffer; 231 } 232 233 static char link_key_type_to_str_buffer[2]; 234 char *link_key_type_to_str(link_key_type_t link_key){ 235 snprintf(link_key_type_to_str_buffer, sizeof(link_key_type_to_str_buffer), "%d", link_key); 236 return (char *) link_key_type_to_str_buffer; 237 } 238 239 void print_bd_addr( bd_addr_t addr){ 240 log_info("%s", bd_addr_to_str(addr)); 241 } 242 243 #ifndef EMBEDDED 244 int sscan_bd_addr(uint8_t * addr_string, bd_addr_t addr){ 245 unsigned int bd_addr_buffer[BD_ADDR_LEN]; //for sscanf, integer needed 246 // reset result buffer 247 memset(bd_addr_buffer, 0, sizeof(bd_addr_buffer)); 248 249 // parse 250 int result = sscanf( (char *) addr_string, "%2x:%2x:%2x:%2x:%2x:%2x", &bd_addr_buffer[0], &bd_addr_buffer[1], &bd_addr_buffer[2], 251 &bd_addr_buffer[3], &bd_addr_buffer[4], &bd_addr_buffer[5]); 252 253 if (result != BD_ADDR_LEN) return 0; 254 255 // store 256 int i; 257 for (i = 0; i < BD_ADDR_LEN; i++) { 258 addr[i] = (uint8_t) bd_addr_buffer[i]; 259 } 260 return 1; 261 } 262 263 int sscan_link_key(char * addr_string, link_key_t link_key){ 264 unsigned int buffer[LINK_KEY_LEN]; 265 266 // reset result buffer 267 memset(&buffer, 0, sizeof(buffer)); 268 269 // parse 270 int result = sscanf( (char *) addr_string, "%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x", 271 &buffer[0], &buffer[1], &buffer[2], &buffer[3], 272 &buffer[4], &buffer[5], &buffer[6], &buffer[7], 273 &buffer[8], &buffer[9], &buffer[10], &buffer[11], 274 &buffer[12], &buffer[13], &buffer[14], &buffer[15] ); 275 276 if (result != LINK_KEY_LEN) return 0; 277 278 // store 279 int i; 280 uint8_t *p = (uint8_t *) link_key; 281 for (i=0; i<LINK_KEY_LEN; i++ ) { 282 *p++ = (uint8_t) buffer[i]; 283 } 284 return 1; 285 } 286 287 #endif 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