1 /*
2 * Copyright (c) 2022 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/types.h>
8 #include <stddef.h>
9 #include <errno.h>
10 #include <zephyr/kernel.h>
11 #include <zephyr/sys/printk.h>
12 #include <zephyr/sys/byteorder.h>
13
14 #include <zephyr/bluetooth/bluetooth.h>
15 #include <zephyr/bluetooth/hci.h>
16 #include <zephyr/bluetooth/direction.h>
17 #include <zephyr/bluetooth/conn.h>
18 #include <zephyr/bluetooth/uuid.h>
19 #include <zephyr/bluetooth/gatt.h>
20 #include <zephyr/bluetooth/hci_vs.h>
21
22 /* Latency set to zero, to enforce PDU exchange every connection event */
23 #define CONN_LATENCY 0U
24 /* Arbitrary selected timeout value */
25 #define CONN_TIMEOUT 400U
26 /* Interval used to run CTE request procedure periodically.
27 * Value is a number of connection events.
28 */
29 #define CTE_REQ_INTERVAL (CONN_LATENCY + 10U)
30 /* Length of CTE in unit of 8 us */
31 #define CTE_LEN (0x14U)
32
33 #define DF_FEAT_ENABLED BIT64(BT_LE_FEAT_BIT_CONN_CTE_RESP)
34
35 static struct bt_conn *default_conn;
36 static const struct bt_le_conn_param conn_params = BT_LE_CONN_PARAM_INIT(
37 BT_GAP_INIT_CONN_INT_MIN, BT_GAP_INIT_CONN_INT_MAX, CONN_LATENCY, CONN_TIMEOUT);
38
39 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
40 static const uint8_t ant_patterns[] = { 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8, 0x9, 0xA };
41 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
42
43 static void start_scan(void);
44
cte_type2str(uint8_t type)45 static const char *cte_type2str(uint8_t type)
46 {
47 switch (type) {
48 case BT_DF_CTE_TYPE_AOA:
49 return "AOA";
50 case BT_DF_CTE_TYPE_AOD_1US:
51 return "AOD 1 [us]";
52 case BT_DF_CTE_TYPE_AOD_2US:
53 return "AOD 2 [us]";
54 default:
55 return "Unknown";
56 }
57 }
58
sample_type2str(enum bt_df_iq_sample type)59 static const char *sample_type2str(enum bt_df_iq_sample type)
60 {
61 switch (type) {
62 case BT_DF_IQ_SAMPLE_8_BITS_INT:
63 return "8 bits int";
64 case BT_DF_IQ_SAMPLE_16_BITS_INT:
65 return "16 bits int";
66 default:
67 return "Unknown";
68 }
69 }
70
packet_status2str(uint8_t status)71 static const char *packet_status2str(uint8_t status)
72 {
73 switch (status) {
74 case BT_DF_CTE_CRC_OK:
75 return "CRC OK";
76 case BT_DF_CTE_CRC_ERR_CTE_BASED_TIME:
77 return "CRC not OK, CTE Info OK";
78 case BT_DF_CTE_CRC_ERR_CTE_BASED_OTHER:
79 return "CRC not OK, Sampled other way";
80 case BT_DF_CTE_INSUFFICIENT_RESOURCES:
81 return "No resources";
82 default:
83 return "Unknown";
84 }
85 }
86
eir_found(struct bt_data * data,void * user_data)87 static bool eir_found(struct bt_data *data, void *user_data)
88 {
89 bt_addr_le_t *addr = user_data;
90 uint64_t u64 = 0U;
91 int err;
92
93 printk("[AD]: %u data_len %u\n", data->type, data->data_len);
94
95 switch (data->type) {
96 case BT_DATA_LE_SUPPORTED_FEATURES:
97 if (data->data_len > sizeof(u64)) {
98 return true;
99 }
100
101 (void)memcpy(&u64, data->data, data->data_len);
102
103 u64 = sys_le64_to_cpu(u64);
104
105 if (!(u64 & DF_FEAT_ENABLED)) {
106 return true;
107 }
108
109 err = bt_le_scan_stop();
110 if (err) {
111 printk("Stop LE scan failed (err %d)\n", err);
112 return true;
113 }
114
115 err = bt_conn_le_create(addr, BT_CONN_LE_CREATE_CONN, &conn_params, &default_conn);
116 if (err) {
117 printk("Create conn failed (err %d)\n", err);
118 start_scan();
119 }
120 return false;
121 }
122
123 return true;
124 }
125
device_found(const bt_addr_le_t * addr,int8_t rssi,uint8_t type,struct net_buf_simple * ad)126 static void device_found(const bt_addr_le_t *addr, int8_t rssi, uint8_t type,
127 struct net_buf_simple *ad)
128 {
129 char dev[BT_ADDR_LE_STR_LEN];
130
131 bt_addr_le_to_str(addr, dev, sizeof(dev));
132 printk("[DEVICE]: %s, AD evt type %u, AD data len %u, RSSI %i\n", dev, type, ad->len, rssi);
133
134 /* We're only interested in connectable events */
135 if (type == BT_GAP_ADV_TYPE_ADV_IND || type == BT_GAP_ADV_TYPE_ADV_DIRECT_IND) {
136 bt_data_parse(ad, eir_found, (void *)addr);
137 }
138 }
139
enable_cte_reqest(void)140 static void enable_cte_reqest(void)
141 {
142 int err;
143
144 const struct bt_df_conn_cte_rx_param cte_rx_params = {
145 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
146 .cte_types = BT_DF_CTE_TYPE_ALL,
147 .slot_durations = 0x2,
148 .num_ant_ids = ARRAY_SIZE(ant_patterns),
149 .ant_ids = ant_patterns,
150 #else
151 .cte_types = BT_DF_CTE_TYPE_AOD_1US | BT_DF_CTE_TYPE_AOD_2US,
152 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
153 };
154
155 const struct bt_df_conn_cte_req_params cte_req_params = {
156 .interval = CTE_REQ_INTERVAL,
157 .cte_length = CTE_LEN,
158 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
159 .cte_type = BT_DF_CTE_TYPE_AOA,
160 #else
161 .cte_type = BT_DF_CTE_TYPE_AOD_2US,
162 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
163 };
164
165 printk("Enable receiving of CTE...\n");
166 err = bt_df_conn_cte_rx_enable(default_conn, &cte_rx_params);
167 if (err) {
168 printk("failed (err %d)\n", err);
169 return;
170 }
171 printk("success. CTE receive enabled.\n");
172
173 printk("Request CTE from peer device...\n");
174 err = bt_df_conn_cte_req_enable(default_conn, &cte_req_params);
175 if (err) {
176 printk("failed (err %d)\n", err);
177 return;
178 }
179 printk("success. CTE request enabled.\n");
180 }
181
start_scan(void)182 static void start_scan(void)
183 {
184 int err;
185
186 /* Use active scanning and disable duplicate filtering to handle any
187 * devices that might update their advertising data at runtime.
188 */
189 struct bt_le_scan_param scan_param = {
190 .type = BT_LE_SCAN_TYPE_ACTIVE,
191 .options = BT_LE_SCAN_OPT_NONE,
192 .interval = BT_GAP_SCAN_FAST_INTERVAL,
193 .window = BT_GAP_SCAN_FAST_WINDOW,
194 };
195
196 err = bt_le_scan_start(&scan_param, device_found);
197 if (err) {
198 printk("Scanning failed to start (err %d)\n", err);
199 return;
200 }
201
202 printk("Scanning successfully started\n");
203 }
204
connected(struct bt_conn * conn,uint8_t conn_err)205 static void connected(struct bt_conn *conn, uint8_t conn_err)
206 {
207 char addr[BT_ADDR_LE_STR_LEN];
208
209 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
210
211 if (conn_err) {
212 printk("Failed to connect to %s (%u)\n", addr, conn_err);
213
214 bt_conn_unref(default_conn);
215 default_conn = NULL;
216
217 start_scan();
218 return;
219 }
220
221 printk("Connected: %s\n", addr);
222
223 if (conn == default_conn) {
224 enable_cte_reqest();
225 }
226 }
227
disconnected(struct bt_conn * conn,uint8_t reason)228 static void disconnected(struct bt_conn *conn, uint8_t reason)
229 {
230 char addr[BT_ADDR_LE_STR_LEN];
231
232 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
233
234 printk("Disconnected: %s, reason 0x%02x %s\n", addr, reason, bt_hci_err_to_str(reason));
235
236 if (default_conn != conn) {
237 return;
238 }
239
240 bt_conn_unref(default_conn);
241 default_conn = NULL;
242
243 start_scan();
244 }
245
cte_recv_cb(struct bt_conn * conn,struct bt_df_conn_iq_samples_report const * report)246 static void cte_recv_cb(struct bt_conn *conn, struct bt_df_conn_iq_samples_report const *report)
247 {
248 char addr[BT_ADDR_LE_STR_LEN];
249
250 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
251
252 if (report->err == BT_DF_IQ_REPORT_ERR_SUCCESS) {
253 printk("CTE[%s]: samples type: %s, samples count %d, cte type %s, slot durations: "
254 "%u [us], packet status %s, RSSI %i\n", addr,
255 sample_type2str(report->sample_type), report->sample_count,
256 cte_type2str(report->cte_type), report->slot_durations,
257 packet_status2str(report->packet_status), report->rssi);
258
259 if (IS_ENABLED(CONFIG_DF_CENTRAL_APP_IQ_REPORT_PRINT_IQ_SAMPLES)) {
260 for (uint8_t idx = 0; idx < report->sample_count; idx++) {
261 if (report->sample_type == BT_DF_IQ_SAMPLE_8_BITS_INT) {
262 printk(" IQ[%d]: %d, %d\n", idx, report->sample[idx].i,
263 report->sample[idx].q);
264 } else if (IS_ENABLED(
265 CONFIG_BT_DF_VS_CONN_IQ_REPORT_16_BITS_IQ_SAMPLES)) {
266 printk(" IQ[%" PRIu8 "]: %d, %d\n", idx,
267 report->sample16[idx].i, report->sample16[idx].q);
268 } else {
269 printk("Unhandled vendor specific IQ samples type\n");
270 break;
271 }
272 }
273 }
274 } else {
275 printk("CTE[%s]: request failed, err %u\n", addr, report->err);
276 }
277 }
278
279 BT_CONN_CB_DEFINE(conn_callbacks) = {
280 .connected = connected,
281 .disconnected = disconnected,
282 .cte_report_cb = cte_recv_cb,
283 };
284
main(void)285 int main(void)
286 {
287 int err;
288
289 err = bt_enable(NULL);
290 if (err) {
291 printk("Bluetooth init failed (err %d)\n", err);
292 return 0;
293 }
294
295 printk("Bluetooth initialized\n");
296
297 start_scan();
298 return 0;
299 }
300
