1eb886013SMatthias Ringwald /* 2eb886013SMatthias Ringwald * Copyright (C) 2014 BlueKitchen GmbH 3eb886013SMatthias Ringwald * 4eb886013SMatthias Ringwald * Redistribution and use in source and binary forms, with or without 5eb886013SMatthias Ringwald * modification, are permitted provided that the following conditions 6eb886013SMatthias Ringwald * are met: 7eb886013SMatthias Ringwald * 8eb886013SMatthias Ringwald * 1. Redistributions of source code must retain the above copyright 9eb886013SMatthias Ringwald * notice, this list of conditions and the following disclaimer. 10eb886013SMatthias Ringwald * 2. Redistributions in binary form must reproduce the above copyright 11eb886013SMatthias Ringwald * notice, this list of conditions and the following disclaimer in the 12eb886013SMatthias Ringwald * documentation and/or other materials provided with the distribution. 13eb886013SMatthias Ringwald * 3. Neither the name of the copyright holders nor the names of 14eb886013SMatthias Ringwald * contributors may be used to endorse or promote products derived 15eb886013SMatthias Ringwald * from this software without specific prior written permission. 16eb886013SMatthias Ringwald * 4. Any redistribution, use, or modification is done solely for 17eb886013SMatthias Ringwald * personal benefit and not for any commercial purpose or for 18eb886013SMatthias Ringwald * monetary gain. 19eb886013SMatthias Ringwald * 20eb886013SMatthias Ringwald * THIS SOFTWARE IS PROVIDED BY BLUEKITCHEN GMBH AND CONTRIBUTORS 21eb886013SMatthias Ringwald * ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 22eb886013SMatthias Ringwald * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 232fca4dadSMilanka Ringwald * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL BLUEKITCHEN 242fca4dadSMilanka Ringwald * GMBH OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 25eb886013SMatthias Ringwald * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 26eb886013SMatthias Ringwald * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 27eb886013SMatthias Ringwald * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 28eb886013SMatthias Ringwald * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 29eb886013SMatthias Ringwald * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF 30eb886013SMatthias Ringwald * THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF 31eb886013SMatthias Ringwald * SUCH DAMAGE. 32eb886013SMatthias Ringwald * 33eb886013SMatthias Ringwald * Please inquire about commercial licensing options at 34eb886013SMatthias Ringwald * [email protected] 35eb886013SMatthias Ringwald * 36eb886013SMatthias Ringwald */ 37eb886013SMatthias Ringwald 38e501bae0SMatthias Ringwald #define BTSTACK_FILE__ "btstack_util.c" 39ab2c6ae4SMatthias Ringwald 40eb886013SMatthias Ringwald /* 41eb886013SMatthias Ringwald * General utility functions 42eb886013SMatthias Ringwald */ 43eb886013SMatthias Ringwald 447907f069SMatthias Ringwald #include "btstack_config.h" 4502bdfbf8SMatthias Ringwald #include "btstack_debug.h" 46eb886013SMatthias Ringwald #include "btstack_util.h" 4702bdfbf8SMatthias Ringwald 48ec08441fSMatthias Ringwald #ifdef _MSC_VER 49ec08441fSMatthias Ringwald #include <intrin.h> 50ec08441fSMatthias Ringwald #include <windows.h> 51ec08441fSMatthias Ringwald #endif 52ec08441fSMatthias Ringwald 5346059ae4SMilanka Ringwald #ifdef ENABLE_PRINTF_HEXDUMP 54eb886013SMatthias Ringwald #include <stdio.h> 5546059ae4SMilanka Ringwald #endif 5646059ae4SMilanka Ringwald 57eb886013SMatthias Ringwald #include <string.h> 58eb886013SMatthias Ringwald 5973988a59SMatthias Ringwald /** 6073988a59SMatthias Ringwald * @brief Compare two Bluetooth addresses 6173988a59SMatthias Ringwald * @param a 6273988a59SMatthias Ringwald * @param b 63969fc1c5SMilanka Ringwald * @return 0 if equal 6473988a59SMatthias Ringwald */ 655222912bSMatthias Ringwald int bd_addr_cmp(const bd_addr_t a, const bd_addr_t b){ 6673988a59SMatthias Ringwald return memcmp(a,b, BD_ADDR_LEN); 6773988a59SMatthias Ringwald } 6873988a59SMatthias Ringwald 6973988a59SMatthias Ringwald /** 7073988a59SMatthias Ringwald * @brief Copy Bluetooth address 7173988a59SMatthias Ringwald * @param dest 7273988a59SMatthias Ringwald * @param src 7373988a59SMatthias Ringwald */ 745222912bSMatthias Ringwald void bd_addr_copy(bd_addr_t dest, const bd_addr_t src){ 756535961aSMatthias Ringwald (void)memcpy(dest, src, BD_ADDR_LEN); 7673988a59SMatthias Ringwald } 7773988a59SMatthias Ringwald 7841f9be70SMatthias Ringwald uint16_t little_endian_read_16(const uint8_t * buffer, int position){ 7941f9be70SMatthias Ringwald return (uint16_t)(((uint16_t) buffer[position]) | (((uint16_t)buffer[position+1]) << 8)); 8073988a59SMatthias Ringwald } 8141f9be70SMatthias Ringwald uint32_t little_endian_read_24(const uint8_t * buffer, int position){ 8241f9be70SMatthias Ringwald return ((uint32_t) buffer[position]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t)buffer[position+2]) << 16); 8373988a59SMatthias Ringwald } 8441f9be70SMatthias Ringwald uint32_t little_endian_read_32(const uint8_t * buffer, int position){ 8541f9be70SMatthias Ringwald return ((uint32_t) buffer[position]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t)buffer[position+2]) << 16) | (((uint32_t) buffer[position+3]) << 24); 8673988a59SMatthias Ringwald } 8773988a59SMatthias Ringwald 8841f9be70SMatthias Ringwald void little_endian_store_16(uint8_t * buffer, uint16_t position, uint16_t value){ 8941f9be70SMatthias Ringwald uint16_t pos = position; 90b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)value; 91b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 8); 92eb886013SMatthias Ringwald } 93eb886013SMatthias Ringwald 9441f9be70SMatthias Ringwald void little_endian_store_24(uint8_t * buffer, uint16_t position, uint32_t value){ 9541f9be70SMatthias Ringwald uint16_t pos = position; 96adbdd27aSMilanka Ringwald buffer[pos++] = (uint8_t)(value); 97adbdd27aSMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 8); 98adbdd27aSMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 16); 99adbdd27aSMilanka Ringwald } 100adbdd27aSMilanka Ringwald 10141f9be70SMatthias Ringwald void little_endian_store_32(uint8_t * buffer, uint16_t position, uint32_t value){ 10241f9be70SMatthias Ringwald uint16_t pos = position; 103b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value); 104b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 8); 105b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 16); 106b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 24); 107eb886013SMatthias Ringwald } 108eb886013SMatthias Ringwald 10941f9be70SMatthias Ringwald uint32_t big_endian_read_16(const uint8_t * buffer, int position) { 11041f9be70SMatthias Ringwald return (uint16_t)(((uint16_t) buffer[position+1]) | (((uint16_t)buffer[position]) << 8)); 11173988a59SMatthias Ringwald } 11273988a59SMatthias Ringwald 11341f9be70SMatthias Ringwald uint32_t big_endian_read_24(const uint8_t * buffer, int position) { 11441f9be70SMatthias Ringwald return ( ((uint32_t)buffer[position+2]) | (((uint32_t)buffer[position+1]) << 8) | (((uint32_t) buffer[position]) << 16)); 115e57a2545SMilanka Ringwald } 116e57a2545SMilanka Ringwald 11741f9be70SMatthias Ringwald uint32_t big_endian_read_32(const uint8_t * buffer, int position) { 11841f9be70SMatthias Ringwald return ((uint32_t) buffer[position+3]) | (((uint32_t)buffer[position+2]) << 8) | (((uint32_t)buffer[position+1]) << 16) | (((uint32_t) buffer[position]) << 24); 11973988a59SMatthias Ringwald } 12073988a59SMatthias Ringwald 12141f9be70SMatthias Ringwald void big_endian_store_16(uint8_t * buffer, uint16_t position, uint16_t value){ 12241f9be70SMatthias Ringwald uint16_t pos = position; 123b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 8); 124b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value); 125eb886013SMatthias Ringwald } 126eb886013SMatthias Ringwald 12741f9be70SMatthias Ringwald void big_endian_store_24(uint8_t * buffer, uint16_t position, uint32_t value){ 12841f9be70SMatthias Ringwald uint16_t pos = position; 129b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 16); 130b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 8); 131b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value); 1327f535380SMilanka Ringwald } 1337f535380SMilanka Ringwald 13441f9be70SMatthias Ringwald void big_endian_store_32(uint8_t * buffer, uint16_t position, uint32_t value){ 13541f9be70SMatthias Ringwald uint16_t pos = position; 136b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 24); 137b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 16); 138b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value >> 8); 139b0920f25SMilanka Ringwald buffer[pos++] = (uint8_t)(value); 140eb886013SMatthias Ringwald } 141eb886013SMatthias Ringwald 142eb886013SMatthias Ringwald // general swap/endianess utils 143b45b7749SMilanka Ringwald void reverse_bytes(const uint8_t * src, uint8_t * dest, int len){ 144eb886013SMatthias Ringwald int i; 145eb886013SMatthias Ringwald for (i = 0; i < len; i++) 146b45b7749SMilanka Ringwald dest[len - 1 - i] = src[i]; 147eb886013SMatthias Ringwald } 148b45b7749SMilanka Ringwald void reverse_24(const uint8_t * src, uint8_t * dest){ 149b45b7749SMilanka Ringwald reverse_bytes(src, dest, 3); 150eb886013SMatthias Ringwald } 151b45b7749SMilanka Ringwald void reverse_48(const uint8_t * src, uint8_t * dest){ 152b45b7749SMilanka Ringwald reverse_bytes(src, dest, 6); 153bf1b35bfSMatthias Ringwald } 154b45b7749SMilanka Ringwald void reverse_56(const uint8_t * src, uint8_t * dest){ 155b45b7749SMilanka Ringwald reverse_bytes(src, dest, 7); 156eb886013SMatthias Ringwald } 157b45b7749SMilanka Ringwald void reverse_64(const uint8_t * src, uint8_t * dest){ 158b45b7749SMilanka Ringwald reverse_bytes(src, dest, 8); 159eb886013SMatthias Ringwald } 160b45b7749SMilanka Ringwald void reverse_128(const uint8_t * src, uint8_t * dest){ 161b45b7749SMilanka Ringwald reverse_bytes(src, dest, 16); 162eb886013SMatthias Ringwald } 163b45b7749SMilanka Ringwald void reverse_256(const uint8_t * src, uint8_t * dest){ 164b45b7749SMilanka Ringwald reverse_bytes(src, dest, 32); 165cc7a2d78SMatthias Ringwald } 166eb886013SMatthias Ringwald 167724d70a2SMatthias Ringwald void reverse_bd_addr(const bd_addr_t src, bd_addr_t dest){ 168724d70a2SMatthias Ringwald reverse_bytes(src, dest, 6); 169724d70a2SMatthias Ringwald } 170724d70a2SMatthias Ringwald 17177955af6SMatthias Ringwald bool btstack_is_null(const uint8_t * buffer, uint16_t size){ 17277955af6SMatthias Ringwald uint16_t i; 17377955af6SMatthias Ringwald for (i=0; i < size ; i++){ 17477955af6SMatthias Ringwald if (buffer[i] != 0) { 17577955af6SMatthias Ringwald return false; 17677955af6SMatthias Ringwald } 17777955af6SMatthias Ringwald } 17877955af6SMatthias Ringwald return true; 17977955af6SMatthias Ringwald } 18077955af6SMatthias Ringwald 181ebaeb1beSMatthias Ringwald uint32_t btstack_min(uint32_t a, uint32_t b){ 182c1ab6cc1SMatthias Ringwald return (a < b) ? a : b; 183ebaeb1beSMatthias Ringwald } 184ebaeb1beSMatthias Ringwald 185ebaeb1beSMatthias Ringwald uint32_t btstack_max(uint32_t a, uint32_t b){ 186c1ab6cc1SMatthias Ringwald return (a > b) ? a : b; 187ebaeb1beSMatthias Ringwald } 188ebaeb1beSMatthias Ringwald 18968af3967SMatthias Ringwald /** 190a9d566f7SMatthias Ringwald * @brief Calculate delta between two uint32_t points in time 1916b65794dSMilanka Ringwald * @return time_a - time_b - result > 0 if time_a is newer than time_b 19268af3967SMatthias Ringwald */ 19368af3967SMatthias Ringwald int32_t btstack_time_delta(uint32_t time_a, uint32_t time_b){ 19468af3967SMatthias Ringwald return (int32_t)(time_a - time_b); 19568af3967SMatthias Ringwald } 19668af3967SMatthias Ringwald 197a9d566f7SMatthias Ringwald /** 198a9d566f7SMatthias Ringwald * @brief Calculate delta between two uint16_t points in time 199a9d566f7SMatthias Ringwald * @return time_a - time_b - result > 0 if time_a is newer than time_b 200a9d566f7SMatthias Ringwald */ 201a9d566f7SMatthias Ringwald int16_t btstack_time16_delta(uint16_t time_a, uint16_t time_b){ 202a9d566f7SMatthias Ringwald return (int16_t)(time_a - time_b); 203a9d566f7SMatthias Ringwald } 204c1c58647SMatthias Ringwald 205eb886013SMatthias Ringwald char char_for_nibble(int nibble){ 2068334d3d8SMatthias Ringwald 2078334d3d8SMatthias Ringwald static const char * char_to_nibble = "0123456789ABCDEF"; 2088334d3d8SMatthias Ringwald 209c1c58647SMatthias Ringwald if (nibble < 16){ 210c1c58647SMatthias Ringwald return char_to_nibble[nibble]; 211c1c58647SMatthias Ringwald } else { 212eb886013SMatthias Ringwald return '?'; 213eb886013SMatthias Ringwald } 214c1c58647SMatthias Ringwald } 215eb886013SMatthias Ringwald 216405d63a9SMatthias Ringwald static inline char char_for_high_nibble(int value){ 217405d63a9SMatthias Ringwald return char_for_nibble((value >> 4) & 0x0f); 218405d63a9SMatthias Ringwald } 219405d63a9SMatthias Ringwald 220405d63a9SMatthias Ringwald static inline char char_for_low_nibble(int value){ 221405d63a9SMatthias Ringwald return char_for_nibble(value & 0x0f); 222405d63a9SMatthias Ringwald } 223405d63a9SMatthias Ringwald 224c1c58647SMatthias Ringwald 225a6efb919SMatthias Ringwald int nibble_for_char(char c){ 226c1ab6cc1SMatthias Ringwald if ((c >= '0') && (c <= '9')) return c - '0'; 227c1ab6cc1SMatthias Ringwald if ((c >= 'a') && (c <= 'f')) return c - 'a' + 10; 228c1ab6cc1SMatthias Ringwald if ((c >= 'A') && (c <= 'F')) return c - 'A' + 10; 229a6efb919SMatthias Ringwald return -1; 230a6efb919SMatthias Ringwald } 231a6efb919SMatthias Ringwald 23246059ae4SMilanka Ringwald #ifdef ENABLE_PRINTF_HEXDUMP 233eb886013SMatthias Ringwald void printf_hexdump(const void * data, int size){ 234c1c58647SMatthias Ringwald char buffer[4]; 235c1c58647SMatthias Ringwald buffer[2] = ' '; 236c1c58647SMatthias Ringwald buffer[3] = 0; 2379dbfa930SMatthias Ringwald const uint8_t * ptr = (const uint8_t *) data; 238c1c58647SMatthias Ringwald while (size > 0){ 2399dbfa930SMatthias Ringwald uint8_t byte = *ptr++; 240c1c58647SMatthias Ringwald buffer[0] = char_for_high_nibble(byte); 241c1c58647SMatthias Ringwald buffer[1] = char_for_low_nibble(byte); 242c1c58647SMatthias Ringwald printf("%s", buffer); 243c1c58647SMatthias Ringwald size--; 244eb886013SMatthias Ringwald } 245eb886013SMatthias Ringwald printf("\n"); 246eb886013SMatthias Ringwald } 24746059ae4SMilanka Ringwald #endif 248eb886013SMatthias Ringwald 249cb42147aSMatthias Ringwald #if defined(ENABLE_LOG_INFO) || defined(ENABLE_LOG_DEBUG) 250cb42147aSMatthias Ringwald static void log_hexdump(int level, const void * data, int size){ 251405d63a9SMatthias Ringwald #define ITEMS_PER_LINE 16 252405d63a9SMatthias Ringwald // template '0x12, ' 253405d63a9SMatthias Ringwald #define BYTES_PER_BYTE 6 254405d63a9SMatthias Ringwald char buffer[BYTES_PER_BYTE*ITEMS_PER_LINE+1]; 2557224be7eSMatthias Ringwald int i, j; 256eb886013SMatthias Ringwald j = 0; 257eb886013SMatthias Ringwald for (i=0; i<size;i++){ 2587224be7eSMatthias Ringwald 2597224be7eSMatthias Ringwald // help static analyzer proof that j stays within bounds 260c1ab6cc1SMatthias Ringwald if (j > (BYTES_PER_BYTE * (ITEMS_PER_LINE-1))){ 2617224be7eSMatthias Ringwald j = 0; 2627224be7eSMatthias Ringwald } 2637224be7eSMatthias Ringwald 264eb886013SMatthias Ringwald uint8_t byte = ((uint8_t *)data)[i]; 265eb886013SMatthias Ringwald buffer[j++] = '0'; 266eb886013SMatthias Ringwald buffer[j++] = 'x'; 267405d63a9SMatthias Ringwald buffer[j++] = char_for_high_nibble(byte); 268405d63a9SMatthias Ringwald buffer[j++] = char_for_low_nibble(byte); 269eb886013SMatthias Ringwald buffer[j++] = ','; 270eb886013SMatthias Ringwald buffer[j++] = ' '; 2717224be7eSMatthias Ringwald 272c1ab6cc1SMatthias Ringwald if (j >= (BYTES_PER_BYTE * ITEMS_PER_LINE) ){ 273eb886013SMatthias Ringwald buffer[j] = 0; 274cb42147aSMatthias Ringwald HCI_DUMP_LOG(level, "%s", buffer); 275eb886013SMatthias Ringwald j = 0; 276eb886013SMatthias Ringwald } 277eb886013SMatthias Ringwald } 278eb886013SMatthias Ringwald if (j != 0){ 279eb886013SMatthias Ringwald buffer[j] = 0; 280cb42147aSMatthias Ringwald HCI_DUMP_LOG(level, "%s", buffer); 281eb886013SMatthias Ringwald } 282cb42147aSMatthias Ringwald } 283cb42147aSMatthias Ringwald #endif 284cb42147aSMatthias Ringwald 285cb42147aSMatthias Ringwald void log_debug_hexdump(const void * data, int size){ 286e950fa96SMatthias Ringwald #ifdef ENABLE_LOG_DEBUG 287fa087deaSMatthias Ringwald log_hexdump(HCI_DUMP_LOG_LEVEL_DEBUG, data, size); 288cb42147aSMatthias Ringwald #else 2894bd79111SMatthias Ringwald UNUSED(data); // ok: no code 2904bd79111SMatthias Ringwald UNUSED(size); // ok: no code 291cb42147aSMatthias Ringwald #endif 292cb42147aSMatthias Ringwald } 293cb42147aSMatthias Ringwald 294cb42147aSMatthias Ringwald void log_info_hexdump(const void * data, int size){ 295cb42147aSMatthias Ringwald #ifdef ENABLE_LOG_INFO 296fa087deaSMatthias Ringwald log_hexdump(HCI_DUMP_LOG_LEVEL_INFO, data, size); 297d0662982SMatthias Ringwald #else 2984bd79111SMatthias Ringwald UNUSED(data); // ok: no code 2994bd79111SMatthias Ringwald UNUSED(size); // ok: no code 3008314c363SMatthias Ringwald #endif 3017299c0feSMatthias Ringwald } 302eb886013SMatthias Ringwald 3038314c363SMatthias Ringwald void log_info_key(const char * name, sm_key_t key){ 30402bdfbf8SMatthias Ringwald #ifdef ENABLE_LOG_INFO 30502bdfbf8SMatthias Ringwald char buffer[16*2+1]; 30602bdfbf8SMatthias Ringwald int i; 30702bdfbf8SMatthias Ringwald int j = 0; 30802bdfbf8SMatthias Ringwald for (i=0; i<16;i++){ 30902bdfbf8SMatthias Ringwald uint8_t byte = key[i]; 310405d63a9SMatthias Ringwald buffer[j++] = char_for_high_nibble(byte); 311405d63a9SMatthias Ringwald buffer[j++] = char_for_low_nibble(byte); 31202bdfbf8SMatthias Ringwald } 31302bdfbf8SMatthias Ringwald buffer[j] = 0; 31402bdfbf8SMatthias Ringwald log_info("%-6s %s", name, buffer); 315d0662982SMatthias Ringwald #else 316d0662982SMatthias Ringwald UNUSED(name); 317cb42147aSMatthias Ringwald (void)key; 31802bdfbf8SMatthias Ringwald #endif 319eb886013SMatthias Ringwald } 320eb886013SMatthias Ringwald 3212b604902SMatthias Ringwald // UUIDs are stored in big endian, similar to bd_addr_t 3222b604902SMatthias Ringwald 323eb886013SMatthias Ringwald // Bluetooth Base UUID: 00000000-0000-1000-8000- 00805F9B34FB 3242b604902SMatthias Ringwald const uint8_t bluetooth_base_uuid[] = { 0x00, 0x00, 0x00, 0x00, /* - */ 0x00, 0x00, /* - */ 0x10, 0x00, /* - */ 325eb886013SMatthias Ringwald 0x80, 0x00, /* - */ 0x00, 0x80, 0x5F, 0x9B, 0x34, 0xFB }; 326eb886013SMatthias Ringwald 32748cdff9cSMilanka Ringwald void uuid_add_bluetooth_prefix(uint8_t * uuid128, uint32_t short_uuid){ 328b45b7749SMilanka Ringwald (void)memcpy(uuid128, bluetooth_base_uuid, 16); 32948cdff9cSMilanka Ringwald big_endian_store_32(uuid128, 0, short_uuid); 330eb886013SMatthias Ringwald } 331eb886013SMatthias Ringwald 3325222912bSMatthias Ringwald int uuid_has_bluetooth_prefix(const uint8_t * uuid128){ 3332b604902SMatthias Ringwald return memcmp(&uuid128[4], &bluetooth_base_uuid[4], 12) == 0; 334eb886013SMatthias Ringwald } 335eb886013SMatthias Ringwald 336eb886013SMatthias Ringwald static char uuid128_to_str_buffer[32+4+1]; 3375222912bSMatthias Ringwald char * uuid128_to_str(const uint8_t * uuid){ 3387224be7eSMatthias Ringwald int i; 3397224be7eSMatthias Ringwald int j = 0; 3407224be7eSMatthias Ringwald // after 4, 6, 8, and 10 bytes = XYXYXYXY-XYXY-XYXY-XYXY-XYXYXYXYXYXY, there's a dash 3417224be7eSMatthias Ringwald const int dash_locations = (1<<3) | (1<<5) | (1<<7) | (1<<9); 3427224be7eSMatthias Ringwald for (i=0;i<16;i++){ 343405d63a9SMatthias Ringwald uint8_t byte = uuid[i]; 344405d63a9SMatthias Ringwald uuid128_to_str_buffer[j++] = char_for_high_nibble(byte); 345405d63a9SMatthias Ringwald uuid128_to_str_buffer[j++] = char_for_low_nibble(byte); 3467224be7eSMatthias Ringwald if (dash_locations & (1<<i)){ 3477224be7eSMatthias Ringwald uuid128_to_str_buffer[j++] = '-'; 3487224be7eSMatthias Ringwald } 3497224be7eSMatthias Ringwald } 350eb886013SMatthias Ringwald return uuid128_to_str_buffer; 351eb886013SMatthias Ringwald } 352eb886013SMatthias Ringwald 353eb886013SMatthias Ringwald static char bd_addr_to_str_buffer[6*3]; // 12:45:78:01:34:67\0 35461c3ec28SMatthias Ringwald char * bd_addr_to_str_with_delimiter(const bd_addr_t addr, char delimiter){ 355eb886013SMatthias Ringwald char * p = bd_addr_to_str_buffer; 356eb886013SMatthias Ringwald int i; 357eb886013SMatthias Ringwald for (i = 0; i < 6 ; i++) { 358405d63a9SMatthias Ringwald uint8_t byte = addr[i]; 359405d63a9SMatthias Ringwald *p++ = char_for_high_nibble(byte); 360405d63a9SMatthias Ringwald *p++ = char_for_low_nibble(byte); 36161c3ec28SMatthias Ringwald *p++ = delimiter; 362eb886013SMatthias Ringwald } 363eb886013SMatthias Ringwald *--p = 0; 364eb886013SMatthias Ringwald return (char *) bd_addr_to_str_buffer; 365eb886013SMatthias Ringwald } 366eb886013SMatthias Ringwald 36761c3ec28SMatthias Ringwald char * bd_addr_to_str(const bd_addr_t addr){ 36861c3ec28SMatthias Ringwald return bd_addr_to_str_with_delimiter(addr, ':'); 36961c3ec28SMatthias Ringwald } 37061c3ec28SMatthias Ringwald 3713c9da642SMatthias Ringwald void btstack_replace_bd_addr_placeholder(uint8_t * buffer, uint16_t size, const bd_addr_t address){ 3723c9da642SMatthias Ringwald const int bd_addr_string_len = 17; 37330314625SMatthias Ringwald uint16_t i = 0; 37430314625SMatthias Ringwald while ((i + bd_addr_string_len) <= size){ 3753c9da642SMatthias Ringwald if (memcmp(&buffer[i], "00:00:00:00:00:00", bd_addr_string_len)) { 3763c9da642SMatthias Ringwald i++; 3773c9da642SMatthias Ringwald continue; 3783c9da642SMatthias Ringwald } 3793c9da642SMatthias Ringwald // set address 3803c9da642SMatthias Ringwald (void)memcpy(&buffer[i], bd_addr_to_str(address), bd_addr_string_len); 3813c9da642SMatthias Ringwald i += bd_addr_string_len; 3823c9da642SMatthias Ringwald } 3833c9da642SMatthias Ringwald } 3843c9da642SMatthias Ringwald 385a6efb919SMatthias Ringwald static int scan_hex_byte(const char * byte_string){ 38641f9be70SMatthias Ringwald int upper_nibble = nibble_for_char(byte_string[0]); 387a6efb919SMatthias Ringwald if (upper_nibble < 0) return -1; 38841f9be70SMatthias Ringwald int lower_nibble = nibble_for_char(byte_string[1]); 389a6efb919SMatthias Ringwald if (lower_nibble < 0) return -1; 390a6efb919SMatthias Ringwald return (upper_nibble << 4) | lower_nibble; 391a6efb919SMatthias Ringwald } 392eb886013SMatthias Ringwald 393a6efb919SMatthias Ringwald int sscanf_bd_addr(const char * addr_string, bd_addr_t addr){ 39441f9be70SMatthias Ringwald const char * the_string = addr_string; 395a6efb919SMatthias Ringwald uint8_t buffer[BD_ADDR_LEN]; 396a6efb919SMatthias Ringwald int result = 0; 397eb886013SMatthias Ringwald int i; 398eb886013SMatthias Ringwald for (i = 0; i < BD_ADDR_LEN; i++) { 39941f9be70SMatthias Ringwald int single_byte = scan_hex_byte(the_string); 400a6efb919SMatthias Ringwald if (single_byte < 0) break; 40141f9be70SMatthias Ringwald the_string += 2; 402b0920f25SMilanka Ringwald buffer[i] = (uint8_t)single_byte; 40341f9be70SMatthias Ringwald // don't check separator after last byte 404c1ab6cc1SMatthias Ringwald if (i == (BD_ADDR_LEN - 1)) { 405a6efb919SMatthias Ringwald result = 1; 406a6efb919SMatthias Ringwald break; 407eb886013SMatthias Ringwald } 408cd2e416cSMatthias Ringwald // skip supported separators 40941f9be70SMatthias Ringwald char next_char = *the_string; 4105df9dc78SMatthias Ringwald if ((next_char == ':') || (next_char == '-') || (next_char == ' ')) { 41141f9be70SMatthias Ringwald the_string++; 412cd2e416cSMatthias Ringwald } 413a6efb919SMatthias Ringwald } 414a6efb919SMatthias Ringwald 4159305033eSMatthias Ringwald if (result != 0){ 416a6efb919SMatthias Ringwald bd_addr_copy(addr, buffer); 417a6efb919SMatthias Ringwald } 418a6efb919SMatthias Ringwald return result; 419eb886013SMatthias Ringwald } 4205d067ab1SMatthias Ringwald 4215d067ab1SMatthias Ringwald uint32_t btstack_atoi(const char * str){ 42241f9be70SMatthias Ringwald const char * the_string = str; 4235d067ab1SMatthias Ringwald uint32_t val = 0; 424ff3cc4a5SMatthias Ringwald while (true){ 42541f9be70SMatthias Ringwald char chr = *the_string++; 426c1ab6cc1SMatthias Ringwald if (!chr || (chr < '0') || (chr > '9')) 4275d067ab1SMatthias Ringwald return val; 4284ea43905SMatthias Ringwald val = (val * 10u) + (uint8_t)(chr - '0'); 4295d067ab1SMatthias Ringwald } 4305d067ab1SMatthias Ringwald } 4311f41c2c9SMatthias Ringwald 432d1207cd8SMilanka Ringwald int string_len_for_uint32(uint32_t i){ 433d1207cd8SMilanka Ringwald if (i < 10) return 1; 434d1207cd8SMilanka Ringwald if (i < 100) return 2; 435d1207cd8SMilanka Ringwald if (i < 1000) return 3; 436d1207cd8SMilanka Ringwald if (i < 10000) return 4; 437d1207cd8SMilanka Ringwald if (i < 100000) return 5; 438d1207cd8SMilanka Ringwald if (i < 1000000) return 6; 439d1207cd8SMilanka Ringwald if (i < 10000000) return 7; 440d1207cd8SMilanka Ringwald if (i < 100000000) return 8; 441d1207cd8SMilanka Ringwald if (i < 1000000000) return 9; 442d1207cd8SMilanka Ringwald return 10; 443d1207cd8SMilanka Ringwald } 444d1207cd8SMilanka Ringwald 445d1207cd8SMilanka Ringwald int count_set_bits_uint32(uint32_t x){ 44641f9be70SMatthias Ringwald uint32_t v = x; 44741f9be70SMatthias Ringwald v = (v & 0x55555555) + ((v >> 1) & 0x55555555U); 44841f9be70SMatthias Ringwald v = (v & 0x33333333) + ((v >> 2) & 0x33333333U); 44941f9be70SMatthias Ringwald v = (v & 0x0F0F0F0F) + ((v >> 4) & 0x0F0F0F0FU); 45041f9be70SMatthias Ringwald v = (v & 0x00FF00FF) + ((v >> 8) & 0x00FF00FFU); 45141f9be70SMatthias Ringwald v = (v & 0x0000FFFF) + ((v >> 16) & 0x0000FFFFU); 45241f9be70SMatthias Ringwald return v; 453d1207cd8SMilanka Ringwald } 4541f41c2c9SMatthias Ringwald 455ec08441fSMatthias Ringwald uint8_t btstack_clz(uint32_t value) { 456ec08441fSMatthias Ringwald #if defined(__GNUC__) || defined (__clang__) 457ec08441fSMatthias Ringwald // use gcc/clang intrinsic 458ec08441fSMatthias Ringwald return (uint8_t) __builtin_clz(value); 459ec08441fSMatthias Ringwald #elif defined(_MSC_VER) 460ec08441fSMatthias Ringwald // use MSVC intrinsic 461ec08441fSMatthias Ringwald DWORD leading_zero = 0; 462ec08441fSMatthias Ringwald if (_BitScanReverse( &leading_zero, value )){ 463ec08441fSMatthias Ringwald return (uint8_t)(31 - leading_zero); 464ec08441fSMatthias Ringwald } else { 465ec08441fSMatthias Ringwald return 32; 466ec08441fSMatthias Ringwald } 467ec08441fSMatthias Ringwald #else 468ec08441fSMatthias Ringwald // divide-and-conquer implementation for 32-bit integers 469*af0ac871SAlec Cantor uint32_t x = value; 470ec08441fSMatthias Ringwald if (x == 0) return 32; 471ec08441fSMatthias Ringwald uint8_t r = 0; 472ec08441fSMatthias Ringwald if ((x & 0xffff0000u) == 0) { 473ec08441fSMatthias Ringwald x <<= 16; 474ec08441fSMatthias Ringwald r += 16; 475ec08441fSMatthias Ringwald } 476ec08441fSMatthias Ringwald if ((x & 0xff000000u) == 0) { 477ec08441fSMatthias Ringwald x <<= 8; 478ec08441fSMatthias Ringwald r += 8; 479ec08441fSMatthias Ringwald } 480ec08441fSMatthias Ringwald if ((x & 0xf0000000u) == 0) { 481ec08441fSMatthias Ringwald x <<= 4; 482ec08441fSMatthias Ringwald r += 4; 483ec08441fSMatthias Ringwald } 484ec08441fSMatthias Ringwald if ((x & 0xc0000000u) == 0) { 485ec08441fSMatthias Ringwald x <<= 2; 486ec08441fSMatthias Ringwald r += 2; 487ec08441fSMatthias Ringwald } 488ec08441fSMatthias Ringwald if ((x & 0x80000000u) == 0) { 489ec08441fSMatthias Ringwald x <<= 1; 490ec08441fSMatthias Ringwald r += 1; 491ec08441fSMatthias Ringwald } 492ec08441fSMatthias Ringwald return r; 493ec08441fSMatthias Ringwald #endif 494ec08441fSMatthias Ringwald } 495ec08441fSMatthias Ringwald 4961f41c2c9SMatthias Ringwald /* 4971f41c2c9SMatthias Ringwald * CRC (reversed crc) lookup table as calculated by the table generator in ETSI TS 101 369 V6.3.0. 4981f41c2c9SMatthias Ringwald */ 4991f41c2c9SMatthias Ringwald 5001f41c2c9SMatthias Ringwald #define CRC8_INIT 0xFF // Initial FCS value 5011f41c2c9SMatthias Ringwald #define CRC8_OK 0xCF // Good final FCS value 5021f41c2c9SMatthias Ringwald 5031f41c2c9SMatthias Ringwald static const uint8_t crc8table[256] = { /* reversed, 8-bit, poly=0x07 */ 5041f41c2c9SMatthias Ringwald 0x00, 0x91, 0xE3, 0x72, 0x07, 0x96, 0xE4, 0x75, 0x0E, 0x9F, 0xED, 0x7C, 0x09, 0x98, 0xEA, 0x7B, 5051f41c2c9SMatthias Ringwald 0x1C, 0x8D, 0xFF, 0x6E, 0x1B, 0x8A, 0xF8, 0x69, 0x12, 0x83, 0xF1, 0x60, 0x15, 0x84, 0xF6, 0x67, 5061f41c2c9SMatthias Ringwald 0x38, 0xA9, 0xDB, 0x4A, 0x3F, 0xAE, 0xDC, 0x4D, 0x36, 0xA7, 0xD5, 0x44, 0x31, 0xA0, 0xD2, 0x43, 5071f41c2c9SMatthias Ringwald 0x24, 0xB5, 0xC7, 0x56, 0x23, 0xB2, 0xC0, 0x51, 0x2A, 0xBB, 0xC9, 0x58, 0x2D, 0xBC, 0xCE, 0x5F, 5081f41c2c9SMatthias Ringwald 0x70, 0xE1, 0x93, 0x02, 0x77, 0xE6, 0x94, 0x05, 0x7E, 0xEF, 0x9D, 0x0C, 0x79, 0xE8, 0x9A, 0x0B, 5091f41c2c9SMatthias Ringwald 0x6C, 0xFD, 0x8F, 0x1E, 0x6B, 0xFA, 0x88, 0x19, 0x62, 0xF3, 0x81, 0x10, 0x65, 0xF4, 0x86, 0x17, 5101f41c2c9SMatthias Ringwald 0x48, 0xD9, 0xAB, 0x3A, 0x4F, 0xDE, 0xAC, 0x3D, 0x46, 0xD7, 0xA5, 0x34, 0x41, 0xD0, 0xA2, 0x33, 5111f41c2c9SMatthias Ringwald 0x54, 0xC5, 0xB7, 0x26, 0x53, 0xC2, 0xB0, 0x21, 0x5A, 0xCB, 0xB9, 0x28, 0x5D, 0xCC, 0xBE, 0x2F, 5121f41c2c9SMatthias Ringwald 0xE0, 0x71, 0x03, 0x92, 0xE7, 0x76, 0x04, 0x95, 0xEE, 0x7F, 0x0D, 0x9C, 0xE9, 0x78, 0x0A, 0x9B, 5131f41c2c9SMatthias Ringwald 0xFC, 0x6D, 0x1F, 0x8E, 0xFB, 0x6A, 0x18, 0x89, 0xF2, 0x63, 0x11, 0x80, 0xF5, 0x64, 0x16, 0x87, 5141f41c2c9SMatthias Ringwald 0xD8, 0x49, 0x3B, 0xAA, 0xDF, 0x4E, 0x3C, 0xAD, 0xD6, 0x47, 0x35, 0xA4, 0xD1, 0x40, 0x32, 0xA3, 5151f41c2c9SMatthias Ringwald 0xC4, 0x55, 0x27, 0xB6, 0xC3, 0x52, 0x20, 0xB1, 0xCA, 0x5B, 0x29, 0xB8, 0xCD, 0x5C, 0x2E, 0xBF, 5161f41c2c9SMatthias Ringwald 0x90, 0x01, 0x73, 0xE2, 0x97, 0x06, 0x74, 0xE5, 0x9E, 0x0F, 0x7D, 0xEC, 0x99, 0x08, 0x7A, 0xEB, 5171f41c2c9SMatthias Ringwald 0x8C, 0x1D, 0x6F, 0xFE, 0x8B, 0x1A, 0x68, 0xF9, 0x82, 0x13, 0x61, 0xF0, 0x85, 0x14, 0x66, 0xF7, 5181f41c2c9SMatthias Ringwald 0xA8, 0x39, 0x4B, 0xDA, 0xAF, 0x3E, 0x4C, 0xDD, 0xA6, 0x37, 0x45, 0xD4, 0xA1, 0x30, 0x42, 0xD3, 5191f41c2c9SMatthias Ringwald 0xB4, 0x25, 0x57, 0xC6, 0xB3, 0x22, 0x50, 0xC1, 0xBA, 0x2B, 0x59, 0xC8, 0xBD, 0x2C, 0x5E, 0xCF 5201f41c2c9SMatthias Ringwald }; 5211f41c2c9SMatthias Ringwald 5221f41c2c9SMatthias Ringwald /*-----------------------------------------------------------------------------------*/ 5231f41c2c9SMatthias Ringwald static uint8_t crc8(uint8_t *data, uint16_t len){ 5241f41c2c9SMatthias Ringwald uint16_t count; 5251f41c2c9SMatthias Ringwald uint8_t crc = CRC8_INIT; 5261f41c2c9SMatthias Ringwald for (count = 0; count < len; count++){ 5271f41c2c9SMatthias Ringwald crc = crc8table[crc ^ data[count]]; 5281f41c2c9SMatthias Ringwald } 5291f41c2c9SMatthias Ringwald return crc; 5301f41c2c9SMatthias Ringwald } 5311f41c2c9SMatthias Ringwald 5321f41c2c9SMatthias Ringwald /*-----------------------------------------------------------------------------------*/ 53363dd1c76SMatthias Ringwald uint8_t btstack_crc8_check(uint8_t *data, uint16_t len, uint8_t check_sum){ 5341f41c2c9SMatthias Ringwald uint8_t crc; 5351f41c2c9SMatthias Ringwald crc = crc8(data, len); 5361f41c2c9SMatthias Ringwald crc = crc8table[crc ^ check_sum]; 5371f41c2c9SMatthias Ringwald if (crc == CRC8_OK){ 5381f41c2c9SMatthias Ringwald return 0; /* Valid */ 5391f41c2c9SMatthias Ringwald } else { 5401f41c2c9SMatthias Ringwald return 1; /* Failed */ 5411f41c2c9SMatthias Ringwald } 5421f41c2c9SMatthias Ringwald } 5431f41c2c9SMatthias Ringwald 5441f41c2c9SMatthias Ringwald /*-----------------------------------------------------------------------------------*/ 54563dd1c76SMatthias Ringwald uint8_t btstack_crc8_calc(uint8_t *data, uint16_t len){ 5461f41c2c9SMatthias Ringwald /* Ones complement */ 5474ea43905SMatthias Ringwald return 0xFFu - crc8(data, len); 5481f41c2c9SMatthias Ringwald } 549a73d0b9dSMilanka Ringwald 550a73d0b9dSMilanka Ringwald uint16_t btstack_next_cid_ignoring_zero(uint16_t current_cid){ 551a73d0b9dSMilanka Ringwald uint16_t next_cid; 552a73d0b9dSMilanka Ringwald if (current_cid == 0xffff) { 553a73d0b9dSMilanka Ringwald next_cid = 1; 554a73d0b9dSMilanka Ringwald } else { 555a73d0b9dSMilanka Ringwald next_cid = current_cid + 1; 556a73d0b9dSMilanka Ringwald } 557a73d0b9dSMilanka Ringwald return next_cid; 558a73d0b9dSMilanka Ringwald } 559de22414aSMatthias Ringwald 560ed7a067cSMilanka Ringwald uint16_t btstack_strcpy(char * dst, uint16_t dst_size, const char * src){ 561981f700bSMatthias Ringwald uint16_t bytes_to_copy = (uint16_t) btstack_min( dst_size - 1, strlen(src)); 562de22414aSMatthias Ringwald (void) memcpy(dst, src, bytes_to_copy); 563981f700bSMatthias Ringwald dst[bytes_to_copy] = 0; 564ed7a067cSMilanka Ringwald return bytes_to_copy + 1; 565de22414aSMatthias Ringwald } 5663ece4788SMatthias Ringwald 5673ece4788SMatthias Ringwald void btstack_strcat(char * dst, uint16_t dst_size, const char * src){ 5683ece4788SMatthias Ringwald uint16_t src_len = (uint16_t) strlen(src); 5693ece4788SMatthias Ringwald uint16_t dst_len = (uint16_t) strlen(dst); 5703ece4788SMatthias Ringwald uint16_t bytes_to_copy = btstack_min( src_len, dst_size - dst_len - 1); 5713ece4788SMatthias Ringwald (void) memcpy( &dst[dst_len], src, bytes_to_copy); 5723ece4788SMatthias Ringwald dst[dst_len + bytes_to_copy] = 0; 5733ece4788SMatthias Ringwald } 574f819d1d9SMilanka Ringwald 575f819d1d9SMilanka Ringwald uint16_t btstack_virtual_memcpy( 576f819d1d9SMilanka Ringwald const uint8_t * field_data, uint16_t field_len, uint16_t field_offset, // position of field in complete data block 577f819d1d9SMilanka Ringwald uint8_t * buffer, uint16_t buffer_size, uint16_t buffer_offset){ 578f819d1d9SMilanka Ringwald 579f819d1d9SMilanka Ringwald uint16_t after_buffer = buffer_offset + buffer_size ; 580f819d1d9SMilanka Ringwald // bail before buffer 581f819d1d9SMilanka Ringwald if ((field_offset + field_len) < buffer_offset){ 582f819d1d9SMilanka Ringwald return 0; 583f819d1d9SMilanka Ringwald } 584f819d1d9SMilanka Ringwald // bail after buffer 585f819d1d9SMilanka Ringwald if (field_offset >= after_buffer){ 586f819d1d9SMilanka Ringwald return 0; 587f819d1d9SMilanka Ringwald } 588f819d1d9SMilanka Ringwald // calc overlap 589f819d1d9SMilanka Ringwald uint16_t bytes_to_copy = field_len; 590f819d1d9SMilanka Ringwald 591f819d1d9SMilanka Ringwald uint16_t skip_at_start = 0; 592f819d1d9SMilanka Ringwald if (field_offset < buffer_offset){ 593f819d1d9SMilanka Ringwald skip_at_start = buffer_offset - field_offset; 594f819d1d9SMilanka Ringwald bytes_to_copy -= skip_at_start; 595f819d1d9SMilanka Ringwald } 596f819d1d9SMilanka Ringwald 597f819d1d9SMilanka Ringwald uint16_t skip_at_end = 0; 598f819d1d9SMilanka Ringwald if ((field_offset + field_len) > after_buffer){ 599f819d1d9SMilanka Ringwald skip_at_end = (field_offset + field_len) - after_buffer; 600f819d1d9SMilanka Ringwald bytes_to_copy -= skip_at_end; 601f819d1d9SMilanka Ringwald } 602f819d1d9SMilanka Ringwald 603f819d1d9SMilanka Ringwald btstack_assert((skip_at_end + skip_at_start) <= field_len); 604f819d1d9SMilanka Ringwald btstack_assert(bytes_to_copy <= field_len); 605f819d1d9SMilanka Ringwald 606f819d1d9SMilanka Ringwald memcpy(&buffer[(field_offset + skip_at_start) - buffer_offset], &field_data[skip_at_start], bytes_to_copy); 607f819d1d9SMilanka Ringwald return bytes_to_copy; 608f819d1d9SMilanka Ringwald } 609