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