xref: /btstack/src/btstack_util.c (revision af0ac87149bcf80f8ae2c1a25eb047d8c63c4557)
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