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 hexdumpf(const void *data, int size){ 124 char buffer[6*16+1]; 125 int i, j; 126 127 uint8_t low = 0x0F; 128 uint8_t high = 0xF0; 129 j = 0; 130 for (i=0; i<size;i++){ 131 uint8_t byte = ((uint8_t *)data)[i]; 132 buffer[j++] = '0'; 133 buffer[j++] = 'x'; 134 buffer[j++] = char_for_nibble((byte & high) >> 4); 135 buffer[j++] = char_for_nibble(byte & low); 136 buffer[j++] = ','; 137 buffer[j++] = ' '; 138 if (j >= 6*16 ){ 139 buffer[j] = 0; 140 printf("%s\n", buffer); 141 j = 0; 142 } 143 } 144 if (j != 0){ 145 buffer[j] = 0; 146 printf("%s\n", buffer); 147 } 148 } 149 150 // void log_info_hexdump(..){ 151 // 152 // } 153 154 void hexdump(const void *data, int size){ 155 #ifdef ENABLE_LOG_INFO 156 char buffer[6*16+1]; 157 int i, j; 158 159 uint8_t low = 0x0F; 160 uint8_t high = 0xF0; 161 j = 0; 162 for (i=0; i<size;i++){ 163 uint8_t byte = ((uint8_t *)data)[i]; 164 buffer[j++] = '0'; 165 buffer[j++] = 'x'; 166 buffer[j++] = char_for_nibble((byte & high) >> 4); 167 buffer[j++] = char_for_nibble(byte & low); 168 buffer[j++] = ','; 169 buffer[j++] = ' '; 170 if (j >= 6*16 ){ 171 buffer[j] = 0; 172 log_info("%s", buffer); 173 j = 0; 174 } 175 } 176 if (j != 0){ 177 buffer[j] = 0; 178 log_info("%s", buffer); 179 } 180 #endif 181 } 182 183 void log_key(const char * name, sm_key_t key){ 184 log_info("%-6s ", name); 185 hexdump(key, 16); 186 } 187 188 // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 189 const uint8_t sdp_bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 190 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 191 192 void sdp_normalize_uuid(uint8_t *uuid, uint32_t shortUUID){ 193 memcpy(uuid, sdp_bluetooth_base_uuid, 16); 194 big_endian_store_32(uuid, 0, shortUUID); 195 } 196 197 int sdp_has_blueooth_base_uuid(uint8_t * uuid128){ 198 return memcmp(&uuid128[4], &sdp_bluetooth_base_uuid[4], 12) == 0; 199 } 200 201 static char uuid128_to_str_buffer[32+4+1]; 202 char * uuid128_to_str(uint8_t * uuid){ 203 sprintf(uuid128_to_str_buffer, "%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-%02x%02x%02x%02x%02x%02x", 204 uuid[0], uuid[1], uuid[2], uuid[3], uuid[4], uuid[5], uuid[6], uuid[7], 205 uuid[8], uuid[9], uuid[10], uuid[11], uuid[12], uuid[13], uuid[14], uuid[15]); 206 return uuid128_to_str_buffer; 207 } 208 void printUUID128(uint8_t *uuid) { 209 printf("%s", uuid128_to_str(uuid)); 210 } 211 212 static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 213 char * bd_addr_to_str(bd_addr_t addr){ 214 // orig code 215 // sprintf(bd_addr_to_str_buffer, "%02x:%02x:%02x:%02x:%02x:%02x", addr[0], addr[1], addr[2], addr[3], addr[4], addr[5]); 216 // sprintf-free code 217 char * p = bd_addr_to_str_buffer; 218 int i; 219 for (i = 0; i < 6 ; i++) { 220 *p++ = char_for_nibble((addr[i] >> 4) & 0x0F); 221 *p++ = char_for_nibble((addr[i] >> 0) & 0x0F); 222 *p++ = ':'; 223 } 224 *--p = 0; 225 return (char *) bd_addr_to_str_buffer; 226 } 227 228 void print_bd_addr( bd_addr_t addr){ 229 log_info("%s", bd_addr_to_str(addr)); 230 } 231 232 int sscan_bd_addr(uint8_t * addr_string, bd_addr_t addr){ 233 unsigned int bd_addr_buffer[BD_ADDR_LEN]; //for sscanf, integer needed 234 // reset result buffer 235 memset(bd_addr_buffer, 0, sizeof(bd_addr_buffer)); 236 237 // parse 238 int result = sscanf( (char *) addr_string, "%2x:%2x:%2x:%2x:%2x:%2x", &bd_addr_buffer[0], &bd_addr_buffer[1], &bd_addr_buffer[2], 239 &bd_addr_buffer[3], &bd_addr_buffer[4], &bd_addr_buffer[5]); 240 241 if (result != BD_ADDR_LEN) return 0; 242 243 // store 244 int i; 245 for (i = 0; i < BD_ADDR_LEN; i++) { 246 addr[i] = (uint8_t) bd_addr_buffer[i]; 247 } 248 return 1; 249 } 250 251 252 /* 253 * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 254 */ 255 static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 256 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 257 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 258 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 259 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 260 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 261 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 262 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 263 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 264 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 265 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 266 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 267 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 268 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 269 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 270 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 271 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 272 }; 273 274 #define CRC8_INIT 0xFF // Initial FCS value 275 #define CRC8_OK 0xCF // Good final FCS value 276 /*-----------------------------------------------------------------------------------*/ 277 static uint8_t crc8(uint8_t *data, uint16_t len) 278 { 279 uint16_t count; 280 uint8_t crc = CRC8_INIT; 281 for (count = 0; count < len; count++) 282 crc = crc8table[crc ^ data[count]]; 283 return crc; 284 } 285 286 /*-----------------------------------------------------------------------------------*/ 287 uint8_t crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum) 288 { 289 uint8_t crc; 290 291 crc = crc8(data, len); 292 293 crc = crc8table[crc ^ check_sum]; 294 if (crc == CRC8_OK) 295 return 0; /* Valid */ 296 else 297 return 1; /* Failed */ 298 299 } 300 301 /*-----------------------------------------------------------------------------------*/ 302 uint8_t crc8_calc(uint8_t *data, uint16_t len) 303 { 304 /* Ones complement */ 305 return 0xFF - crc8(data, len); 306 } 307 308