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 int sscan_bd_addr(uint8_t * addr_string, bd_addr_t addr){ 244 unsigned int bd_addr_buffer[BD_ADDR_LEN]; //for sscanf, integer needed 245 // reset result buffer 246 memset(bd_addr_buffer, 0, sizeof(bd_addr_buffer)); 247 248 // parse 249 int result = sscanf( (char *) addr_string, "%2x:%2x:%2x:%2x:%2x:%2x", &bd_addr_buffer[0], &bd_addr_buffer[1], &bd_addr_buffer[2], 250 &bd_addr_buffer[3], &bd_addr_buffer[4], &bd_addr_buffer[5]); 251 252 if (result != BD_ADDR_LEN) return 0; 253 254 // store 255 int i; 256 for (i = 0; i < BD_ADDR_LEN; i++) { 257 addr[i] = (uint8_t) bd_addr_buffer[i]; 258 } 259 return 1; 260 } 261 262 int sscan_link_key(char * addr_string, link_key_t link_key){ 263 unsigned int buffer[LINK_KEY_LEN]; 264 265 // reset result buffer 266 memset(&buffer, 0, sizeof(buffer)); 267 268 // parse 269 int result = sscanf( (char *) addr_string, "%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x%2x", 270 &buffer[0], &buffer[1], &buffer[2], &buffer[3], 271 &buffer[4], &buffer[5], &buffer[6], &buffer[7], 272 &buffer[8], &buffer[9], &buffer[10], &buffer[11], 273 &buffer[12], &buffer[13], &buffer[14], &buffer[15] ); 274 275 if (result != LINK_KEY_LEN) return 0; 276 277 // store 278 int i; 279 uint8_t *p = (uint8_t *) link_key; 280 for (i=0; i<LINK_KEY_LEN; i++ ) { 281 *p++ = (uint8_t) buffer[i]; 282 } 283 return 1; 284 } 285 286 287 // treat standard pairing as Authenticated as it uses a PIN 288 int is_authenticated_link_key(link_key_type_t link_key_type){ 289 switch (link_key_type){ 290 case COMBINATION_KEY: 291 case AUTHENTICATED_COMBINATION_KEY_GENERATED_FROM_P192: 292 case AUTHENTICATED_COMBINATION_KEY_GENERATED_FROM_P256: 293 return 1; 294 default: 295 return 0; 296 } 297 } 298 299 /* 300 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 301 */ 302 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 303 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 304 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 305 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 306 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 307 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 308 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 309 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 310 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 311 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 312 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 313 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 314 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 315 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 316 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 317 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 318 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 319 }; 320 321 #define CRC8_INIT 0xFF // Initial FCS value 322 #define CRC8_OK 0xCF // Good final FCS value 323 /*-----------------------------------------------------------------------------------*/ 324 static uint8_t crc8(uint8_t *data, uint16_t len) 325 { 326 uint16_t count; 327 uint8_t crc = CRC8_INIT; 328 for (count = 0; count < len; count++) 329 crc = crc8table[crc ^ data[count]]; 330 return crc; 331 } 332 333 /*-----------------------------------------------------------------------------------*/ 334 uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum) 335 { 336 uint8_t crc; 337 338 crc = crc8(data, len); 339 340 crc = crc8table[crc ^ check_sum]; 341 if (crc == CRC8_OK) 342 return 0; /* Valid */ 343 else 344 return 1; /* Failed */ 345 346 } 347 348 /*-----------------------------------------------------------------------------------*/ 349 uint8_t crc8_calc(uint8_t *data, uint16_t len) 350 { 351 /* Ones complement */ 352 return 0xFF - crc8(data, len); 353 } 354 355