1 /*
2 * Copyright (c) 2021-2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stddef.h>
8 #include <stdint.h>
9 #include <zephyr/kernel.h>
10
11 #include <zephyr/sys/printk.h>
12 #include <zephyr/sys/byteorder.h>
13 #include <zephyr/sys/util.h>
14
15 #include <zephyr/bluetooth/bluetooth.h>
16 #include <zephyr/bluetooth/gap.h>
17 #include <zephyr/bluetooth/direction.h>
18 #include <zephyr/bluetooth/hci.h>
19 #include <zephyr/bluetooth/hci_vs.h>
20
21 #define DEVICE_NAME CONFIG_BT_DEVICE_NAME
22 #define DEVICE_NAME_LEN (sizeof(DEVICE_NAME) - 1)
23 #define NAME_LEN 30
24 #define PEER_NAME_LEN_MAX 30
25 /* BT Core 5.3 Vol 6, Part B section 4.4.5.1 Periodic Advertising Trains allows controller to wait
26 * 6 periodic advertising events for synchronization establishment, hence timeout must be longer
27 * than that.
28 */
29 #define SYNC_CREATE_TIMEOUT_INTERVAL_NUM 7
30 /* Maximum length of advertising data represented in hexadecimal format */
31 #define ADV_DATA_HEX_STR_LEN_MAX (BT_GAP_ADV_MAX_EXT_ADV_DATA_LEN * 2 + 1)
32
33 static struct bt_le_per_adv_sync_param sync_create_param;
34 static struct bt_le_per_adv_sync *adv_sync;
35 static bt_addr_le_t per_addr;
36 static bool per_adv_found;
37 static bool scan_enabled;
38 static bool sync_wait;
39 static bool sync_terminated;
40 static uint8_t per_sid;
41 static uint16_t sync_create_timeout;
42
43 static K_SEM_DEFINE(sem_per_adv, 0, 1);
44 static K_SEM_DEFINE(sem_per_sync, 0, 1);
45 static K_SEM_DEFINE(sem_per_sync_lost, 0, 1);
46
47 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
48 const static uint8_t ant_patterns[] = { 0x1, 0x2, 0x3, 0x4, 0x5,
49 0x6, 0x7, 0x8, 0x9, 0xA };
50 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
51
52 static bool data_cb(struct bt_data *data, void *user_data);
53 static void create_sync(void);
54 static void scan_recv(const struct bt_le_scan_recv_info *info,
55 struct net_buf_simple *buf);
56
57 static void sync_cb(struct bt_le_per_adv_sync *sync,
58 struct bt_le_per_adv_sync_synced_info *info);
59 static void term_cb(struct bt_le_per_adv_sync *sync,
60 const struct bt_le_per_adv_sync_term_info *info);
61 static void recv_cb(struct bt_le_per_adv_sync *sync,
62 const struct bt_le_per_adv_sync_recv_info *info,
63 struct net_buf_simple *buf);
64 static void scan_recv(const struct bt_le_scan_recv_info *info,
65 struct net_buf_simple *buf);
66 static void scan_disable(void);
67 static void cte_recv_cb(struct bt_le_per_adv_sync *sync,
68 struct bt_df_per_adv_sync_iq_samples_report const *report);
69
70 static struct bt_le_per_adv_sync_cb sync_callbacks = {
71 .synced = sync_cb,
72 .term = term_cb,
73 .recv = recv_cb,
74 .cte_report_cb = cte_recv_cb,
75 };
76
77 static struct bt_le_scan_cb scan_callbacks = {
78 .recv = scan_recv,
79 };
80
sync_create_timeout_get(uint16_t interval)81 static uint16_t sync_create_timeout_get(uint16_t interval)
82 {
83 uint32_t interval_us;
84 uint32_t timeout;
85
86 /* Add retries and convert to unit in 10's of ms */
87 interval_us = BT_GAP_PER_ADV_INTERVAL_TO_US(interval);
88 timeout = BT_GAP_US_TO_PER_ADV_SYNC_TIMEOUT(interval_us) * SYNC_CREATE_TIMEOUT_INTERVAL_NUM;
89
90 /* Enforce restraints */
91 timeout = CLAMP(timeout, BT_GAP_PER_ADV_MIN_TIMEOUT, BT_GAP_PER_ADV_MAX_TIMEOUT);
92
93 return (uint16_t)timeout;
94 }
95
phy2str(uint8_t phy)96 static const char *phy2str(uint8_t phy)
97 {
98 switch (phy) {
99 case 0: return "No packets";
100 case BT_GAP_LE_PHY_1M: return "LE 1M";
101 case BT_GAP_LE_PHY_2M: return "LE 2M";
102 case BT_GAP_LE_PHY_CODED: return "LE Coded";
103 default: return "Unknown";
104 }
105 }
106
cte_type2str(uint8_t type)107 static const char *cte_type2str(uint8_t type)
108 {
109 switch (type) {
110 case BT_DF_CTE_TYPE_AOA: return "AOA";
111 case BT_DF_CTE_TYPE_AOD_1US: return "AOD 1 [us]";
112 case BT_DF_CTE_TYPE_AOD_2US: return "AOD 2 [us]";
113 case BT_DF_CTE_TYPE_NONE: return "";
114 default: return "Unknown";
115 }
116 }
117
sample_type2str(enum bt_df_iq_sample type)118 static const char *sample_type2str(enum bt_df_iq_sample type)
119 {
120 switch (type) {
121 case BT_DF_IQ_SAMPLE_8_BITS_INT:
122 return "8 bits int";
123 case BT_DF_IQ_SAMPLE_16_BITS_INT:
124 return "16 bits int";
125 default:
126 return "Unknown";
127 }
128 }
129
pocket_status2str(uint8_t status)130 static const char *pocket_status2str(uint8_t status)
131 {
132 switch (status) {
133 case BT_DF_CTE_CRC_OK: return "CRC OK";
134 case BT_DF_CTE_CRC_ERR_CTE_BASED_TIME: return "CRC not OK, CTE Info OK";
135 case BT_DF_CTE_CRC_ERR_CTE_BASED_OTHER: return "CRC not OK, Sampled other way";
136 case BT_DF_CTE_INSUFFICIENT_RESOURCES: return "No resources";
137 default: return "Unknown";
138 }
139 }
140
data_cb(struct bt_data * data,void * user_data)141 static bool data_cb(struct bt_data *data, void *user_data)
142 {
143 char *name = user_data;
144 uint8_t len;
145
146 switch (data->type) {
147 case BT_DATA_NAME_SHORTENED:
148 case BT_DATA_NAME_COMPLETE:
149 len = MIN(data->data_len, NAME_LEN - 1);
150 memcpy(name, data->data, len);
151 name[len] = '\0';
152 return false;
153 default:
154 return true;
155 }
156 }
157
sync_cb(struct bt_le_per_adv_sync * sync,struct bt_le_per_adv_sync_synced_info * info)158 static void sync_cb(struct bt_le_per_adv_sync *sync,
159 struct bt_le_per_adv_sync_synced_info *info)
160 {
161 char le_addr[BT_ADDR_LE_STR_LEN];
162
163 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
164
165 printk("PER_ADV_SYNC[%u]: [DEVICE]: %s synced, "
166 "Interval 0x%04x (%u ms), PHY %s\n",
167 bt_le_per_adv_sync_get_index(sync), le_addr,
168 info->interval, info->interval * 5 / 4, phy2str(info->phy));
169
170 k_sem_give(&sem_per_sync);
171 }
172
term_cb(struct bt_le_per_adv_sync * sync,const struct bt_le_per_adv_sync_term_info * info)173 static void term_cb(struct bt_le_per_adv_sync *sync,
174 const struct bt_le_per_adv_sync_term_info *info)
175 {
176 char le_addr[BT_ADDR_LE_STR_LEN];
177
178 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
179
180 printk("PER_ADV_SYNC[%u]: [DEVICE]: %s sync terminated\n",
181 bt_le_per_adv_sync_get_index(sync), le_addr);
182
183 if (sync_wait) {
184 sync_terminated = true;
185 k_sem_give(&sem_per_sync);
186 } else {
187 k_sem_give(&sem_per_sync_lost);
188 }
189 }
190
recv_cb(struct bt_le_per_adv_sync * sync,const struct bt_le_per_adv_sync_recv_info * info,struct net_buf_simple * buf)191 static void recv_cb(struct bt_le_per_adv_sync *sync,
192 const struct bt_le_per_adv_sync_recv_info *info,
193 struct net_buf_simple *buf)
194 {
195 static char data_str[ADV_DATA_HEX_STR_LEN_MAX];
196 char le_addr[BT_ADDR_LE_STR_LEN];
197
198 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
199 bin2hex(buf->data, buf->len, data_str, sizeof(data_str));
200
201 printk("PER_ADV_SYNC[%u]: [DEVICE]: %s, tx_power %i, "
202 "RSSI %i, CTE %s, data length %u, data: %s\n",
203 bt_le_per_adv_sync_get_index(sync), le_addr, info->tx_power,
204 info->rssi, cte_type2str(info->cte_type), buf->len, data_str);
205 }
206
cte_recv_cb(struct bt_le_per_adv_sync * sync,struct bt_df_per_adv_sync_iq_samples_report const * report)207 static void cte_recv_cb(struct bt_le_per_adv_sync *sync,
208 struct bt_df_per_adv_sync_iq_samples_report const *report)
209 {
210 printk("CTE[%u]: samples type: %s, samples count %d, cte type %s, slot durations: %u [us], "
211 "packet status %s, RSSI %i\n",
212 bt_le_per_adv_sync_get_index(sync), sample_type2str(report->sample_type),
213 report->sample_count, cte_type2str(report->cte_type), report->slot_durations,
214 pocket_status2str(report->packet_status), report->rssi);
215
216 if (IS_ENABLED(CONFIG_DF_LOCATOR_APP_IQ_REPORT_PRINT_IQ_SAMPLES)) {
217 for (uint8_t idx = 0; idx < report->sample_count; idx++) {
218 if (report->sample_type == BT_DF_IQ_SAMPLE_8_BITS_INT) {
219 printk(" IQ[%d]: %d, %d\n", idx, report->sample[idx].i,
220 report->sample[idx].q);
221 } else if (IS_ENABLED(CONFIG_BT_DF_VS_CL_IQ_REPORT_16_BITS_IQ_SAMPLES)) {
222 printk(" IQ[%" PRIu8 "]: %d, %d\n", idx, report->sample16[idx].i,
223 report->sample16[idx].q);
224 } else {
225 printk("Unhandled vendor specific IQ samples type\n");
226 break;
227 }
228 }
229 }
230 }
231
scan_recv(const struct bt_le_scan_recv_info * info,struct net_buf_simple * buf)232 static void scan_recv(const struct bt_le_scan_recv_info *info,
233 struct net_buf_simple *buf)
234 {
235 char le_addr[BT_ADDR_LE_STR_LEN];
236 char name[NAME_LEN];
237
238 (void)memset(name, 0, sizeof(name));
239
240 bt_data_parse(buf, data_cb, name);
241
242 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
243
244 printk("[DEVICE]: %s, AD evt type %u, Tx Pwr: %i, RSSI %i %s C:%u S:%u "
245 "D:%u SR:%u E:%u Prim: %s, Secn: %s, Interval: 0x%04x (%u ms), "
246 "SID: %u\n",
247 le_addr, info->adv_type, info->tx_power, info->rssi, name,
248 (info->adv_props & BT_GAP_ADV_PROP_CONNECTABLE) != 0,
249 (info->adv_props & BT_GAP_ADV_PROP_SCANNABLE) != 0,
250 (info->adv_props & BT_GAP_ADV_PROP_DIRECTED) != 0,
251 (info->adv_props & BT_GAP_ADV_PROP_SCAN_RESPONSE) != 0,
252 (info->adv_props & BT_GAP_ADV_PROP_EXT_ADV) != 0,
253 phy2str(info->primary_phy), phy2str(info->secondary_phy),
254 info->interval, info->interval * 5 / 4, info->sid);
255
256 if (!per_adv_found && info->interval != 0) {
257 sync_create_timeout = sync_create_timeout_get(info->interval);
258 per_adv_found = true;
259 per_sid = info->sid;
260 bt_addr_le_copy(&per_addr, info->addr);
261
262 k_sem_give(&sem_per_adv);
263 }
264 }
265
create_sync(void)266 static void create_sync(void)
267 {
268 int err;
269
270 printk("Creating Periodic Advertising Sync...");
271 bt_addr_le_copy(&sync_create_param.addr, &per_addr);
272
273 sync_create_param.options = BT_LE_PER_ADV_SYNC_OPT_SYNC_ONLY_CONST_TONE_EXT;
274 sync_create_param.sid = per_sid;
275 sync_create_param.skip = 0;
276 sync_create_param.timeout = sync_create_timeout;
277 err = bt_le_per_adv_sync_create(&sync_create_param, &adv_sync);
278 if (err != 0) {
279 printk("failed (err %d)\n", err);
280 return;
281 }
282 printk("success.\n");
283 }
284
delete_sync(void)285 static int delete_sync(void)
286 {
287 int err;
288
289 printk("Deleting Periodic Advertising Sync...");
290 err = bt_le_per_adv_sync_delete(adv_sync);
291 if (err != 0) {
292 printk("failed (err %d)\n", err);
293 return err;
294 }
295 printk("success\n");
296
297 return 0;
298 }
299
enable_cte_rx(void)300 static void enable_cte_rx(void)
301 {
302 int err;
303
304 const struct bt_df_per_adv_sync_cte_rx_param cte_rx_params = {
305 .max_cte_count = 5,
306 #if defined(CONFIG_BT_DF_CTE_RX_AOA)
307 .cte_types = BT_DF_CTE_TYPE_ALL,
308 .slot_durations = 0x2,
309 .num_ant_ids = ARRAY_SIZE(ant_patterns),
310 .ant_ids = ant_patterns,
311 #else
312 .cte_types = BT_DF_CTE_TYPE_AOD_1US | BT_DF_CTE_TYPE_AOD_2US,
313 #endif /* CONFIG_BT_DF_CTE_RX_AOA */
314 };
315
316 printk("Enable receiving of CTE...\n");
317 err = bt_df_per_adv_sync_cte_rx_enable(adv_sync, &cte_rx_params);
318 if (err != 0) {
319 printk("failed (err %d)\n", err);
320 return;
321 }
322 printk("success. CTE receive enabled.\n");
323 }
324
scan_init(void)325 static int scan_init(void)
326 {
327 printk("Scan callbacks register...");
328 bt_le_scan_cb_register(&scan_callbacks);
329 printk("success.\n");
330
331 printk("Periodic Advertising callbacks register...");
332 bt_le_per_adv_sync_cb_register(&sync_callbacks);
333 printk("success.\n");
334
335 return 0;
336 }
337
scan_enable(void)338 static int scan_enable(void)
339 {
340 struct bt_le_scan_param param = {
341 .type = BT_LE_SCAN_TYPE_ACTIVE,
342 .options = BT_LE_SCAN_OPT_FILTER_DUPLICATE,
343 .interval = BT_GAP_SCAN_FAST_INTERVAL,
344 .window = BT_GAP_SCAN_FAST_WINDOW,
345 .timeout = 0U, };
346 int err;
347
348 if (!scan_enabled) {
349 printk("Start scanning...");
350 err = bt_le_scan_start(¶m, NULL);
351 if (err != 0) {
352 printk("failed (err %d)\n", err);
353 return err;
354 }
355 printk("success\n");
356 scan_enabled = true;
357 }
358
359 return 0;
360 }
361
scan_disable(void)362 static void scan_disable(void)
363 {
364 int err;
365
366 printk("Scan disable...");
367 err = bt_le_scan_stop();
368 if (err != 0) {
369 printk("failed (err %d)\n", err);
370 return;
371 }
372 printk("Success.\n");
373
374 scan_enabled = false;
375 }
376
main(void)377 int main(void)
378 {
379 int err;
380
381 printk("Starting Connectionless Locator Demo\n");
382
383 printk("Bluetooth initialization...");
384 err = bt_enable(NULL);
385 if (err != 0) {
386 printk("failed (err %d)\n", err);
387 }
388 printk("success\n");
389
390 scan_init();
391
392 scan_enabled = false;
393 do {
394 scan_enable();
395
396 printk("Waiting for periodic advertising...");
397 per_adv_found = false;
398 err = k_sem_take(&sem_per_adv, K_FOREVER);
399 if (err != 0) {
400 printk("failed (err %d)\n", err);
401 return 0;
402 }
403 printk("success. Found periodic advertising.\n");
404
405 sync_wait = true;
406 sync_terminated = false;
407
408 create_sync();
409
410 printk("Waiting for periodic sync...\n");
411 err = k_sem_take(&sem_per_sync, K_MSEC(sync_create_timeout));
412 if (err != 0 || sync_terminated) {
413 if (err != 0) {
414 printk("failed (err %d)\n", err);
415 } else {
416 printk("terminated\n");
417 }
418
419 sync_wait = false;
420
421 err = delete_sync();
422 if (err != 0) {
423 return 0;
424 }
425
426 continue;
427 }
428 printk("success. Periodic sync established.\n");
429 sync_wait = false;
430
431 enable_cte_rx();
432
433 /* Disable scan to cleanup output */
434 scan_disable();
435
436 printk("Waiting for periodic sync lost...\n");
437 err = k_sem_take(&sem_per_sync_lost, K_FOREVER);
438 if (err != 0) {
439 printk("failed (err %d)\n", err);
440 return 0;
441 }
442 printk("Periodic sync lost.\n");
443 } while (true);
444 return 0;
445 }
446
