1 /*
2 * Copyright (c) 2017-2021 Nordic Semiconductor ASA
3 * Copyright (c) 2015-2016 Intel Corporation
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7 #include <stdbool.h>
8 #include <stddef.h>
9 #include <stdint.h>
10 #include <string.h>
11
12 #include <zephyr/autoconf.h>
13 #include <zephyr/bluetooth/bluetooth.h>
14 #include <zephyr/logging/log.h>
15 #include <zephyr/net_buf.h>
16 #include <zephyr/sys/check.h>
17
18 #define LOG_LEVEL CONFIG_BT_HCI_CORE_LOG_LEVEL
19 LOG_MODULE_REGISTER(bt_data);
20
bt_data_parse(struct net_buf_simple * ad,bool (* func)(struct bt_data * data,void * user_data),void * user_data)21 void bt_data_parse(struct net_buf_simple *ad,
22 bool (*func)(struct bt_data *data, void *user_data),
23 void *user_data)
24 {
25 while (ad->len > 1) {
26 struct bt_data data;
27 uint8_t len;
28
29 len = net_buf_simple_pull_u8(ad);
30 if (len == 0U) {
31 /* Early termination */
32 return;
33 }
34
35 if (len > ad->len) {
36 LOG_WRN("malformed advertising data %u / %u",
37 len, ad->len);
38 return;
39 }
40
41 data.type = net_buf_simple_pull_u8(ad);
42 data.data_len = len - 1;
43 data.data = ad->data;
44
45 if (!func(&data, user_data)) {
46 return;
47 }
48
49 net_buf_simple_pull(ad, len - 1);
50 }
51 }
52
bt_data_get_len(const struct bt_data data[],size_t data_count)53 size_t bt_data_get_len(const struct bt_data data[], size_t data_count)
54 {
55 size_t total_len = 0;
56
57 for (size_t i = 0; i < data_count; i++) {
58 total_len += sizeof(data[i].data_len) + sizeof(data[i].type) + data[i].data_len;
59 }
60
61 return total_len;
62 }
63
bt_data_serialize(const struct bt_data * input,uint8_t * output)64 size_t bt_data_serialize(const struct bt_data *input, uint8_t *output)
65 {
66 CHECKIF(input == NULL) {
67 LOG_DBG("input is NULL");
68 return 0;
69 }
70
71 CHECKIF(output == NULL) {
72 LOG_DBG("output is NULL");
73 return 0;
74 }
75
76 uint8_t ad_data_len = input->data_len;
77 uint8_t data_len = ad_data_len + 1;
78
79 output[0] = data_len;
80 output[1] = input->type;
81
82 memcpy(&output[2], input->data, ad_data_len);
83
84 return data_len + 1;
85 }
86