1 /*
2 * Copyright (c) 2021 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <ctype.h>
8 #include <zephyr/bluetooth/bluetooth.h>
9 #include <zephyr/bluetooth/conn.h>
10 #include <zephyr/bluetooth/iso.h>
11 #include <zephyr/sys/byteorder.h>
12 #include <zephyr/console/console.h>
13
14 #include <zephyr/logging/log.h>
15 LOG_MODULE_REGISTER(iso_broadcast_receiver, LOG_LEVEL_DBG);
16
17 #define DEVICE_NAME CONFIG_BT_DEVICE_NAME
18 #define DEVICE_NAME_LEN (sizeof(DEVICE_NAME))
19
20 #define PA_RETRY_COUNT 6
21 #define ISO_RETRY_COUNT 10
22
23 struct iso_recv_stats {
24 uint32_t iso_recv_count;
25 uint32_t iso_lost_count;
26 };
27
28 static bool broadcaster_found;
29 static bool per_adv_lost;
30 static bool big_sync_lost;
31 static bool biginfo_received;
32 static bt_addr_le_t per_addr;
33 static uint8_t per_sid;
34 static uint32_t per_interval_us;
35 static uint32_t iso_interval_us;
36 static uint8_t bis_count;
37 static uint32_t last_received_counter;
38 static int64_t big_sync_start_time;
39 static size_t big_sync_count;
40
41 static struct iso_recv_stats stats_current_sync;
42 static struct iso_recv_stats stats_overall;
43
44 static K_SEM_DEFINE(sem_per_adv, 0, 1);
45 static K_SEM_DEFINE(sem_per_sync, 0, 1);
46 static K_SEM_DEFINE(sem_per_sync_lost, 0, 1);
47 static K_SEM_DEFINE(sem_per_big_info, 0, 1);
48 static K_SEM_DEFINE(sem_big_sync, 0, 1);
49 static K_SEM_DEFINE(sem_big_sync_lost, 0, 1);
50
phy2str(uint8_t phy)51 static const char *phy2str(uint8_t phy)
52 {
53 switch (phy) {
54 case 0: return "No packets";
55 case BT_GAP_LE_PHY_1M: return "LE 1M";
56 case BT_GAP_LE_PHY_2M: return "LE 2M";
57 case BT_GAP_LE_PHY_CODED: return "LE Coded";
58 default: return "Unknown";
59 }
60 }
61
data_cb(struct bt_data * data,void * user_data)62 static bool data_cb(struct bt_data *data, void *user_data)
63 {
64 char *name = user_data;
65 uint8_t len;
66
67 switch (data->type) {
68 case BT_DATA_NAME_SHORTENED:
69 case BT_DATA_NAME_COMPLETE:
70 len = MIN(data->data_len, DEVICE_NAME_LEN - 1);
71 memcpy(name, data->data, len);
72 name[len] = '\0';
73 return false;
74 default:
75 return true;
76 }
77 }
78
scan_recv(const struct bt_le_scan_recv_info * info,struct net_buf_simple * buf)79 static void scan_recv(const struct bt_le_scan_recv_info *info,
80 struct net_buf_simple *buf)
81 {
82 char le_addr[BT_ADDR_LE_STR_LEN];
83 char name[DEVICE_NAME_LEN];
84
85 if (broadcaster_found) {
86 return;
87 }
88
89 if (info->interval == 0U) {
90 /* Not broadcast periodic advertising - Ignore */
91 return;
92 }
93
94 (void)memset(name, 0, sizeof(name));
95
96 bt_data_parse(buf, data_cb, name);
97
98 if (strncmp(DEVICE_NAME, name, strlen(DEVICE_NAME))) {
99 return;
100 }
101
102 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
103
104 LOG_INF("Found broadcaster with address %s (RSSI %i)",
105 le_addr, info->rssi);
106
107 broadcaster_found = true;
108
109 per_sid = info->sid;
110 per_interval_us = BT_CONN_INTERVAL_TO_US(info->interval);
111 bt_addr_le_copy(&per_addr, info->addr);
112
113 k_sem_give(&sem_per_adv);
114 }
115
116 static struct bt_le_scan_cb scan_callbacks = {
117 .recv = scan_recv,
118 };
119
sync_cb(struct bt_le_per_adv_sync * sync,struct bt_le_per_adv_sync_synced_info * info)120 static void sync_cb(struct bt_le_per_adv_sync *sync,
121 struct bt_le_per_adv_sync_synced_info *info)
122 {
123 LOG_INF("Periodic advertisement synced");
124
125 k_sem_give(&sem_per_sync);
126 }
127
term_cb(struct bt_le_per_adv_sync * sync,const struct bt_le_per_adv_sync_term_info * info)128 static void term_cb(struct bt_le_per_adv_sync *sync,
129 const struct bt_le_per_adv_sync_term_info *info)
130 {
131 LOG_INF("Periodic advertisement sync terminated");
132
133 per_adv_lost = true;
134 k_sem_give(&sem_per_sync_lost);
135 }
136
biginfo_cb(struct bt_le_per_adv_sync * sync,const struct bt_iso_biginfo * biginfo)137 static void biginfo_cb(struct bt_le_per_adv_sync *sync,
138 const struct bt_iso_biginfo *biginfo)
139 {
140 if (biginfo_received) {
141 return;
142 }
143
144 LOG_INF("BIGinfo received: num_bis %u, nse %u, interval %u us, "
145 "bn %u, pto %u, irc %u, max_pdu %u, sdu_interval %u us, "
146 "max_sdu %u, phy %s, %s framing, %sencrypted",
147 biginfo->num_bis, biginfo->sub_evt_count,
148 BT_CONN_INTERVAL_TO_US(biginfo->iso_interval),
149 biginfo->burst_number, biginfo->offset, biginfo->rep_count,
150 biginfo->max_pdu, biginfo->sdu_interval, biginfo->max_sdu,
151 phy2str(biginfo->phy), biginfo->framing ? "with" : "without",
152 biginfo->encryption ? "" : "not ");
153
154 iso_interval_us = BT_CONN_INTERVAL_TO_US(biginfo->iso_interval);
155 bis_count = MIN(biginfo->num_bis, CONFIG_BT_ISO_MAX_CHAN);
156 biginfo_received = true;
157 k_sem_give(&sem_per_big_info);
158 }
159
160 static struct bt_le_per_adv_sync_cb sync_callbacks = {
161 .synced = sync_cb,
162 .term = term_cb,
163 .biginfo = biginfo_cb,
164 };
165
print_stats(char * name,struct iso_recv_stats * stats)166 static void print_stats(char *name, struct iso_recv_stats *stats)
167 {
168 uint32_t total_packets;
169
170 total_packets = stats->iso_recv_count + stats->iso_lost_count;
171
172 LOG_INF("%s: Received %u/%u (%.2f%%) - Total packets lost %u",
173 name, stats->iso_recv_count, total_packets,
174 (float)stats->iso_recv_count * 100 / total_packets,
175 stats->iso_lost_count);
176
177 }
178
iso_recv(struct bt_iso_chan * chan,const struct bt_iso_recv_info * info,struct net_buf * buf)179 static void iso_recv(struct bt_iso_chan *chan,
180 const struct bt_iso_recv_info *info,
181 struct net_buf *buf)
182 {
183 uint32_t total_packets;
184 static bool stats_latest_arr[1000];
185 static size_t stats_latest_arr_pos;
186
187 /* NOTE: The packets received may be on different BISes */
188
189 if (info->flags & BT_ISO_FLAGS_VALID) {
190 stats_current_sync.iso_recv_count++;
191 stats_overall.iso_recv_count++;
192 stats_latest_arr[stats_latest_arr_pos++] = true;
193 } else {
194 stats_current_sync.iso_lost_count++;
195 stats_overall.iso_lost_count++;
196 stats_latest_arr[stats_latest_arr_pos++] = false;
197 }
198
199 if (stats_latest_arr_pos == sizeof(stats_latest_arr)) {
200 stats_latest_arr_pos = 0;
201 }
202
203 total_packets = stats_overall.iso_recv_count + stats_overall.iso_lost_count;
204
205 if ((total_packets % 100) == 0) {
206 struct iso_recv_stats stats_latest = { 0 };
207
208 for (int i = 0; i < ARRAY_SIZE(stats_latest_arr); i++) {
209 /* If we have not yet received 1000 packets, break
210 * early
211 */
212 if (i == total_packets) {
213 break;
214 }
215
216 if (stats_latest_arr[i]) {
217 stats_latest.iso_recv_count++;
218 } else {
219 stats_latest.iso_lost_count++;
220 }
221 }
222
223 print_stats("Overall ", &stats_overall);
224 print_stats("Current Sync", &stats_current_sync);
225 print_stats("Latest 1000 ", &stats_latest);
226 LOG_INF(""); /* Empty line to separate the stats */
227 }
228 }
229
iso_connected(struct bt_iso_chan * chan)230 static void iso_connected(struct bt_iso_chan *chan)
231 {
232 LOG_INF("ISO Channel %p connected", chan);
233
234 big_sync_start_time = k_uptime_get();
235
236 k_sem_give(&sem_big_sync);
237 }
238
iso_disconnected(struct bt_iso_chan * chan,uint8_t reason)239 static void iso_disconnected(struct bt_iso_chan *chan, uint8_t reason)
240 {
241 /* Calculate cumulative moving average - Be aware that this may
242 * cause overflow at sufficiently large counts or durations
243 */
244 static int64_t average_duration;
245 uint64_t big_sync_duration = k_uptime_get() - big_sync_start_time;
246 uint64_t total_duration = big_sync_duration + (big_sync_count - 1) * average_duration;
247
248 average_duration = total_duration / big_sync_count;
249
250 LOG_INF("ISO Channel %p disconnected with reason 0x%02x after "
251 "%llu milliseconds (average duration %llu)",
252 chan, reason, big_sync_duration, average_duration);
253
254 big_sync_lost = true;
255
256 k_sem_give(&sem_big_sync_lost);
257 }
258
259 static struct bt_iso_chan_ops iso_ops = {
260 .recv = iso_recv,
261 .connected = iso_connected,
262 .disconnected = iso_disconnected,
263 };
264
265 static struct bt_iso_chan_io_qos iso_rx_qos;
266
267 static struct bt_iso_chan_qos bis_iso_qos = {
268 .rx = &iso_rx_qos,
269 };
270
271
start_scan(void)272 static int start_scan(void)
273 {
274 int err;
275
276 err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, NULL);
277 if (err != 0) {
278 LOG_ERR("Scan start failed (err %d)", err);
279 return err;
280 }
281
282 LOG_INF("Scan started");
283
284 return 0;
285 }
286
stop_scan(void)287 static int stop_scan(void)
288 {
289 int err;
290
291 err = bt_le_scan_stop();
292 if (err != 0) {
293 LOG_ERR("Scan stop failed (err %d)", err);
294 return err;
295 }
296 LOG_INF("Scan stopped");
297
298 return 0;
299 }
300
create_pa_sync(struct bt_le_per_adv_sync ** sync)301 static int create_pa_sync(struct bt_le_per_adv_sync **sync)
302 {
303 struct bt_le_per_adv_sync_param sync_create_param;
304 int err;
305 uint32_t sem_timeout_us;
306
307 LOG_INF("Creating Periodic Advertising Sync");
308 bt_addr_le_copy(&sync_create_param.addr, &per_addr);
309 sync_create_param.options = 0;
310 sync_create_param.sid = per_sid;
311 sync_create_param.skip = 0;
312 /* Multiple PA interval with retry count and convert to unit of 10 ms */
313 sync_create_param.timeout = (per_interval_us * PA_RETRY_COUNT) / (10 * USEC_PER_MSEC);
314 sem_timeout_us = per_interval_us * PA_RETRY_COUNT;
315 err = bt_le_per_adv_sync_create(&sync_create_param, sync);
316 if (err != 0) {
317 LOG_ERR("Periodic advertisement sync create failed (err %d)",
318 err);
319 return err;
320 }
321
322 LOG_INF("Waiting for periodic sync");
323 err = k_sem_take(&sem_per_sync, K_USEC(sem_timeout_us));
324 if (err != 0) {
325 LOG_INF("failed to take sem_per_sync (err %d)", err);
326
327 LOG_INF("Deleting Periodic Advertising Sync");
328 err = bt_le_per_adv_sync_delete(*sync);
329 if (err != 0) {
330 LOG_ERR("Failed to delete Periodic advertisement sync (err %d)",
331 err);
332
333 return err;
334 }
335 }
336 LOG_INF("Periodic sync established");
337
338 return 0;
339 }
340
create_big_sync(struct bt_iso_big ** big,struct bt_le_per_adv_sync * sync)341 static int create_big_sync(struct bt_iso_big **big, struct bt_le_per_adv_sync *sync)
342 {
343 int err;
344 uint32_t sem_timeout_us = per_interval_us * PA_RETRY_COUNT;
345 uint32_t sync_timeout_us;
346 static struct bt_iso_chan bis_iso_chan[CONFIG_BT_ISO_MAX_CHAN];
347 struct bt_iso_chan *bis[CONFIG_BT_ISO_MAX_CHAN];
348 struct bt_iso_big_sync_param big_sync_param = {
349 .bis_channels = bis,
350 .num_bis = 0,
351 .bis_bitfield = 0,
352 .mse = 0,
353 .sync_timeout = 0
354 };
355
356 for (int i = 0; i < ARRAY_SIZE(bis_iso_chan); i++) {
357 bis_iso_chan[i].ops = &iso_ops;
358 bis_iso_chan[i].qos = &bis_iso_qos;
359 bis[i] = &bis_iso_chan[i];
360 }
361
362 LOG_INF("Waiting for BIG info");
363 err = k_sem_take(&sem_per_big_info, K_USEC(sem_timeout_us));
364 if (err != 0) {
365 LOG_ERR("failed to take sem_per_big_info (err %d)", err);
366 return err;
367 }
368
369 sync_timeout_us = iso_interval_us * ISO_RETRY_COUNT;
370 /* timeout is in 10 ms units */
371 big_sync_param.sync_timeout = CLAMP(sync_timeout_us / (10 * USEC_PER_MSEC),
372 BT_ISO_SYNC_TIMEOUT_MIN,
373 BT_ISO_SYNC_TIMEOUT_MAX);
374 big_sync_param.num_bis = bis_count;
375 /* BIS indexes start from 0x01, so add one to `i` */
376 for (int i = 1; i <= big_sync_param.num_bis; i++) {
377 big_sync_param.bis_bitfield |= BIT(i);
378 }
379
380 LOG_INF("Syncing to BIG");
381 err = bt_iso_big_sync(sync, &big_sync_param, big);
382 if (err != 0) {
383 LOG_ERR("BIG sync failed (err %d)", err);
384 return err;
385 }
386
387 LOG_INF("Waiting for BIG sync");
388 err = k_sem_take(&sem_big_sync, K_USEC(sem_timeout_us));
389 if (err != 0) {
390 LOG_ERR("failed to take sem_big_sync (err %d)", err);
391 return err;
392 }
393 LOG_INF("BIG sync established");
394
395 big_sync_count++;
396
397 return 0;
398 }
399
cleanup(struct bt_le_per_adv_sync * sync,struct bt_iso_big * big)400 static int cleanup(struct bt_le_per_adv_sync *sync, struct bt_iso_big *big)
401 {
402 int pa_err = 0;
403 int big_err = 0;
404
405 if (!per_adv_lost && sync) {
406 LOG_INF("Deleting Periodic advertisement Sync");
407 pa_err = bt_le_per_adv_sync_delete(sync);
408 if (pa_err != 0) {
409 LOG_ERR("Failed to delete Periodic advertisement sync (err %d)",
410 pa_err);
411 }
412 }
413
414 if (!big_sync_lost && big) {
415 LOG_INF("Terminating BIG Sync");
416 big_err = bt_iso_big_terminate(big);
417 if (big_err != 0) {
418 LOG_ERR("BIG terminate failed (err %d)", big_err);
419 }
420 }
421
422 if (pa_err != 0 || big_err != 0) {
423 LOG_ERR("Cleanup failed (%d), recommend restart application to "
424 "avoid any potential leftovers",
425 pa_err != 0 ? pa_err : big_err);
426 }
427
428 return pa_err != 0 ? pa_err : big_err;
429 }
430
reset_sems(void)431 static void reset_sems(void)
432 {
433 (void)k_sem_reset(&sem_per_adv);
434 (void)k_sem_reset(&sem_per_sync);
435 (void)k_sem_reset(&sem_per_sync_lost);
436 (void)k_sem_reset(&sem_per_big_info);
437 (void)k_sem_reset(&sem_big_sync);
438 (void)k_sem_reset(&sem_big_sync_lost);
439 }
440
test_run_receiver(void)441 int test_run_receiver(void)
442 {
443 struct bt_le_per_adv_sync *sync = NULL;
444 struct bt_iso_big *big = NULL;
445 int err;
446 static bool callbacks_registered;
447
448 broadcaster_found = false;
449 per_adv_lost = false;
450 big_sync_lost = false;
451 biginfo_received = false;
452 reset_sems();
453
454 if (!callbacks_registered) {
455 bt_le_per_adv_sync_cb_register(&sync_callbacks);
456 bt_le_scan_cb_register(&scan_callbacks);
457 callbacks_registered = true;
458 }
459
460 err = start_scan();
461 if (err != 0) {
462 return err;
463 }
464
465 LOG_INF("Waiting for periodic advertiser");
466 err = k_sem_take(&sem_per_adv, K_FOREVER);
467 if (err != 0) {
468 LOG_ERR("failed to take sem_per_adv (err %d)", err);
469 return err;
470 }
471 LOG_INF("Periodic advertiser found");
472
473 err = stop_scan();
474 if (err != 0) {
475 return err;
476 }
477
478 err = create_pa_sync(&sync);
479 if (err != 0) {
480 return err;
481 }
482
483 last_received_counter = 0;
484 memset(&stats_current_sync, 0, sizeof(stats_current_sync));
485 big_sync_start_time = 0;
486
487 err = create_big_sync(&big, sync);
488 if (err != 0) {
489 (void)cleanup(sync, big);
490 return err;
491 }
492
493 err = k_sem_take(&sem_big_sync_lost, K_FOREVER);
494 if (err != 0) {
495 LOG_ERR("failed to take sem_big_sync_lost (err %d)", err);
496 return err;
497 }
498 LOG_INF("BIG sync lost, returning");
499 /* TODO: Add support to cancel the test early */
500
501 return cleanup(sync, big);
502 }
503
