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/conn.h>
9 #include <zephyr/bluetooth/gap.h>
10 #include <zephyr/kernel.h>
11 #include <string.h>
12 #include <stdint.h>
13 #include <stdlib.h>
14 #include <zephyr/sys/util.h>
15 #include <zephyr/sys_clock.h>
16 #include <zephyr/types.h>
17
18
19 #include <zephyr/console/console.h>
20 #include <zephyr/bluetooth/bluetooth.h>
21 #include <zephyr/bluetooth/iso.h>
22 #include <zephyr/bluetooth/hci.h>
23 #include <zephyr/sys/byteorder.h>
24
25 #include <zephyr/logging/log.h>
26 LOG_MODULE_REGISTER(iso_connected, LOG_LEVEL_DBG);
27
28 #define DEVICE_NAME CONFIG_BT_DEVICE_NAME
29 #define DEVICE_NAME_LEN (sizeof(DEVICE_NAME))
30
31 enum benchmark_role {
32 ROLE_CENTRAL,
33 ROLE_PERIPHERAL,
34 ROLE_QUIT
35 };
36
37 enum sdu_dir {
38 DIR_C_TO_P,
39 DIR_P_TO_C
40 };
41
42 #define DEFAULT_CIS_RTN 2
43 #define DEFAULT_CIS_INTERVAL_US 7500
44 #define DEFAULT_CIS_LATENCY_MS 40
45 #define DEFAULT_CIS_PHY BT_GAP_LE_PHY_2M
46 #define DEFAULT_CIS_SDU_SIZE CONFIG_BT_ISO_TX_MTU
47 #define DEFAULT_CIS_PACKING 0
48 #define DEFAULT_CIS_FRAMING 0
49 #define DEFAULT_CIS_COUNT 1U
50 #define DEFAULT_CIS_SEC_LEVEL BT_SECURITY_L1
51
52 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
53 #define DEFAULT_CIS_NSE BT_ISO_NSE_MIN
54 #define DEFAULT_CIS_BN BT_ISO_BN_MIN
55 #define DEFAULT_CIS_PDU_SIZE CONFIG_BT_ISO_TX_MTU
56 #define DEFAULT_CIS_FT BT_ISO_FT_MIN
57 #define DEFAULT_CIS_ISO_INTERVAL DEFAULT_CIS_INTERVAL_US / 1250U /* N * 1.25 ms */
58 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
59
60 #define BUFFERS_ENQUEUED 2 /* Number of buffers enqueue for each channel */
61
62 BUILD_ASSERT(BUFFERS_ENQUEUED * CONFIG_BT_ISO_MAX_CHAN <= CONFIG_BT_ISO_TX_BUF_COUNT,
63 "Not enough buffers to enqueue");
64
65 struct iso_recv_stats {
66 uint32_t iso_recv_count;
67 uint32_t iso_lost_count;
68 };
69
70 struct iso_chan_work {
71 struct bt_iso_chan chan;
72 struct k_work_delayable send_work;
73 struct bt_iso_info info;
74 uint16_t seq_num;
75 } iso_chans[CONFIG_BT_ISO_MAX_CHAN];
76
77 static enum benchmark_role role;
78 static struct bt_conn *default_conn;
79 static struct bt_iso_chan *cis[CONFIG_BT_ISO_MAX_CHAN];
80 static bool advertiser_found;
81 static bt_addr_le_t adv_addr;
82 static uint32_t last_received_counter;
83 static struct iso_recv_stats stats_current_conn;
84 static struct iso_recv_stats stats_overall;
85 static int64_t iso_conn_start_time;
86 static size_t total_iso_conn_count;
87 static uint32_t iso_send_count;
88 static struct bt_iso_cig *cig;
89
90 NET_BUF_POOL_FIXED_DEFINE(tx_pool, 1, BT_ISO_SDU_BUF_SIZE(CONFIG_BT_ISO_TX_MTU),
91 CONFIG_BT_CONN_TX_USER_DATA_SIZE, NULL);
92 static uint8_t iso_data[CONFIG_BT_ISO_TX_MTU];
93
94 static K_SEM_DEFINE(sem_adv, 0, 1);
95 static K_SEM_DEFINE(sem_iso_accept, 0, 1);
96 static K_SEM_DEFINE(sem_iso_connected, 0, CONFIG_BT_ISO_MAX_CHAN);
97 static K_SEM_DEFINE(sem_iso_disconnected, 0, CONFIG_BT_ISO_MAX_CHAN);
98 static K_SEM_DEFINE(sem_connected, 0, 1);
99 static K_SEM_DEFINE(sem_disconnected, 0, 1);
100
101 static struct bt_iso_chan_io_qos iso_tx_qos = {
102 .sdu = DEFAULT_CIS_SDU_SIZE, /* bytes */
103 .rtn = DEFAULT_CIS_RTN,
104 .phy = DEFAULT_CIS_PHY,
105 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
106 .max_pdu = DEFAULT_CIS_PDU_SIZE,
107 .burst_number = DEFAULT_CIS_BN,
108 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
109 };
110
111 static struct bt_iso_chan_io_qos iso_rx_qos = {
112 .sdu = DEFAULT_CIS_SDU_SIZE, /* bytes */
113 .rtn = DEFAULT_CIS_RTN,
114 .phy = DEFAULT_CIS_PHY,
115 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
116 .max_pdu = DEFAULT_CIS_PDU_SIZE,
117 .burst_number = DEFAULT_CIS_BN,
118 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
119 };
120
121 static struct bt_iso_chan_qos iso_qos = {
122 .tx = &iso_tx_qos,
123 .rx = &iso_rx_qos,
124 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
125 .num_subevents = DEFAULT_CIS_NSE,
126 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
127 };
128
129 static struct bt_iso_cig_param cig_create_param = {
130 .c_to_p_interval = DEFAULT_CIS_INTERVAL_US, /* in microseconds */
131 .p_to_c_interval = DEFAULT_CIS_INTERVAL_US, /* in microseconds */
132 .c_to_p_latency = DEFAULT_CIS_LATENCY_MS, /* milliseconds */
133 .p_to_c_latency = DEFAULT_CIS_LATENCY_MS, /* milliseconds */
134 .sca = BT_GAP_SCA_UNKNOWN,
135 .packing = DEFAULT_CIS_PACKING,
136 .framing = DEFAULT_CIS_FRAMING,
137 .cis_channels = cis,
138 .num_cis = DEFAULT_CIS_COUNT,
139 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
140 .c_to_p_ft = DEFAULT_CIS_FT,
141 .p_to_c_ft = DEFAULT_CIS_FT,
142 .iso_interval = DEFAULT_CIS_ISO_INTERVAL,
143 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
144 };
145
146 static const struct bt_data sd[] = {
147 BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME, sizeof(CONFIG_BT_DEVICE_NAME) - 1),
148 };
149
device_role_select(void)150 static enum benchmark_role device_role_select(void)
151 {
152 char role_char;
153 const char central_char = 'c';
154 const char peripheral_char = 'p';
155 const char quit_char = 'q';
156
157 while (true) {
158 printk("Choose device role - type %c (central role) or %c (peripheral role), or %c to quit: ",
159 central_char, peripheral_char, quit_char);
160
161 role_char = tolower(console_getchar());
162
163 printk("%c\n", role_char);
164
165 if (role_char == central_char) {
166 printk("Central role\n");
167 return ROLE_CENTRAL;
168 } else if (role_char == peripheral_char) {
169 printk("Peripheral role\n");
170 return ROLE_PERIPHERAL;
171 } else if (role_char == quit_char) {
172 printk("Quitting\n");
173 return ROLE_QUIT;
174 } else if (role_char == '\n' || role_char == '\r') {
175 continue;
176 }
177
178 printk("Invalid role: %c\n", role_char);
179 }
180 }
181
print_stats(char * name,struct iso_recv_stats * stats)182 static void print_stats(char *name, struct iso_recv_stats *stats)
183 {
184 uint32_t total_packets;
185
186 total_packets = stats->iso_recv_count + stats->iso_lost_count;
187
188 LOG_INF("%s: Received %u/%u (%.2f%%) - Total packets lost %u",
189 name, stats->iso_recv_count, total_packets,
190 (double)((float)stats->iso_recv_count * 100 / total_packets),
191 stats->iso_lost_count);
192 }
193
iso_send(struct bt_iso_chan * chan)194 static void iso_send(struct bt_iso_chan *chan)
195 {
196 int ret;
197 struct net_buf *buf;
198 struct iso_chan_work *chan_work;
199 uint32_t interval;
200
201 chan_work = CONTAINER_OF(chan, struct iso_chan_work, chan);
202
203 if (!chan_work->info.can_send) {
204 return;
205 }
206
207 interval = (role == ROLE_CENTRAL) ?
208 cig_create_param.c_to_p_interval : cig_create_param.p_to_c_interval;
209
210 buf = net_buf_alloc(&tx_pool, K_NO_WAIT);
211 if (buf == NULL) {
212 LOG_ERR("Could not allocate buffer");
213 k_work_reschedule(&chan_work->send_work, K_USEC(interval));
214 return;
215 }
216
217 net_buf_reserve(buf, BT_ISO_CHAN_SEND_RESERVE);
218 net_buf_add_mem(buf, iso_data, iso_tx_qos.sdu);
219
220 ret = bt_iso_chan_send(chan, buf, chan_work->seq_num++);
221 if (ret < 0) {
222 LOG_ERR("Unable to send data: %d", ret);
223 net_buf_unref(buf);
224 k_work_reschedule(&chan_work->send_work, K_USEC(interval));
225 return;
226 }
227
228 iso_send_count++;
229
230 if ((iso_send_count % 100) == 0) {
231 LOG_INF("Sending value %u", iso_send_count);
232 }
233 }
234
iso_timer_timeout(struct k_work * work)235 static void iso_timer_timeout(struct k_work *work)
236 {
237 struct bt_iso_chan *chan;
238 struct iso_chan_work *chan_work;
239 struct k_work_delayable *delayable = k_work_delayable_from_work(work);
240
241 chan_work = CONTAINER_OF(delayable, struct iso_chan_work, send_work);
242 chan = &chan_work->chan;
243
244 iso_send(chan);
245 }
246
iso_sent(struct bt_iso_chan * chan)247 static void iso_sent(struct bt_iso_chan *chan)
248 {
249 struct iso_chan_work *chan_work;
250
251 chan_work = CONTAINER_OF(chan, struct iso_chan_work, chan);
252
253 k_work_reschedule(&chan_work->send_work, K_MSEC(0));
254 }
255
iso_recv(struct bt_iso_chan * chan,const struct bt_iso_recv_info * info,struct net_buf * buf)256 static void iso_recv(struct bt_iso_chan *chan,
257 const struct bt_iso_recv_info *info,
258 struct net_buf *buf)
259 {
260 uint32_t total_packets;
261 static bool stats_latest_arr[1000];
262 static size_t stats_latest_arr_pos;
263
264 /* NOTE: The packets received may be on different CISes */
265
266 if (info->flags & BT_ISO_FLAGS_VALID) {
267 stats_current_conn.iso_recv_count++;
268 stats_overall.iso_recv_count++;
269 stats_latest_arr[stats_latest_arr_pos++] = true;
270 } else {
271 stats_current_conn.iso_lost_count++;
272 stats_overall.iso_lost_count++;
273 stats_latest_arr[stats_latest_arr_pos++] = false;
274 }
275
276 if (stats_latest_arr_pos == sizeof(stats_latest_arr)) {
277 stats_latest_arr_pos = 0;
278 }
279
280 total_packets = stats_overall.iso_recv_count + stats_overall.iso_lost_count;
281
282 if ((total_packets % 100) == 0) {
283 struct iso_recv_stats stats_latest = { 0 };
284
285 for (int i = 0; i < ARRAY_SIZE(stats_latest_arr); i++) {
286 /* If we have not yet received 1000 packets, break
287 * early
288 */
289 if (i == total_packets) {
290 break;
291 }
292
293 if (stats_latest_arr[i]) {
294 stats_latest.iso_recv_count++;
295 } else {
296 stats_latest.iso_lost_count++;
297 }
298 }
299
300 print_stats("Overall ", &stats_overall);
301 print_stats("Current Conn", &stats_current_conn);
302 print_stats("Latest 1000 ", &stats_latest);
303 LOG_INF(""); /* Empty line to separate the stats */
304 }
305 }
306
iso_connected(struct bt_iso_chan * chan)307 static void iso_connected(struct bt_iso_chan *chan)
308 {
309 struct iso_chan_work *chan_work;
310 int err;
311
312 LOG_INF("ISO Channel %p connected", chan);
313
314 chan_work = CONTAINER_OF(chan, struct iso_chan_work, chan);
315 err = bt_iso_chan_get_info(chan, &chan_work->info);
316 if (err != 0) {
317 LOG_ERR("Could get info about chan %p: %d", chan, err);
318 }
319
320 /* If multiple CIS was created, this will be the value of the last
321 * created in the CIG
322 */
323 iso_conn_start_time = k_uptime_get();
324
325 chan_work = CONTAINER_OF(chan, struct iso_chan_work, chan);
326 chan_work->seq_num = 0U;
327
328 k_sem_give(&sem_iso_connected);
329 }
330
iso_disconnected(struct bt_iso_chan * chan,uint8_t reason)331 static void iso_disconnected(struct bt_iso_chan *chan, uint8_t reason)
332 {
333 /* Calculate cumulative moving average - Be aware that this may
334 * cause overflow at sufficiently large counts or durations
335 *
336 * This duration is calculated for each CIS disconnected from the time
337 * of the last created CIS.
338 */
339 static int64_t average_duration;
340 uint64_t iso_conn_duration;
341 uint64_t total_duration;
342
343 if (iso_conn_start_time > 0) {
344 iso_conn_duration = k_uptime_get() - iso_conn_start_time;
345 } else {
346 iso_conn_duration = 0;
347 }
348 total_duration = iso_conn_duration + (total_iso_conn_count - 1) * average_duration;
349
350 average_duration = total_duration / total_iso_conn_count;
351
352 LOG_INF("ISO Channel %p disconnected with reason 0x%02x after "
353 "%llu milliseconds (average duration %llu)",
354 chan, reason, iso_conn_duration, average_duration);
355
356 k_sem_give(&sem_iso_disconnected);
357 }
358
359 static struct bt_iso_chan_ops iso_ops = {
360 .recv = iso_recv,
361 .connected = iso_connected,
362 .disconnected = iso_disconnected,
363 .sent = iso_sent,
364 };
365
iso_accept(const struct bt_iso_accept_info * info,struct bt_iso_chan ** chan)366 static int iso_accept(const struct bt_iso_accept_info *info,
367 struct bt_iso_chan **chan)
368 {
369 LOG_INF("Incoming ISO request from %p", (void *)info->acl);
370
371 for (int i = 0; i < ARRAY_SIZE(iso_chans); i++) {
372 if (iso_chans[i].chan.state == BT_ISO_STATE_DISCONNECTED) {
373 LOG_INF("Returning instance %d", i);
374 *chan = &iso_chans[i].chan;
375 cig_create_param.num_cis++;
376
377 k_sem_give(&sem_iso_accept);
378 return 0;
379 }
380 }
381
382 LOG_ERR("Could not accept any more CIS");
383
384 *chan = NULL;
385
386 return -ENOMEM;
387 }
388
389 static struct bt_iso_server iso_server = {
390 #if defined(CONFIG_BT_SMP)
391 .sec_level = DEFAULT_CIS_SEC_LEVEL,
392 #endif /* CONFIG_BT_SMP */
393 .accept = iso_accept,
394 };
395
data_cb(struct bt_data * data,void * user_data)396 static bool data_cb(struct bt_data *data, void *user_data)
397 {
398 char *name = user_data;
399 uint8_t len;
400
401 switch (data->type) {
402 case BT_DATA_NAME_SHORTENED:
403 __fallthrough;
404 case BT_DATA_NAME_COMPLETE:
405 len = MIN(data->data_len, DEVICE_NAME_LEN - 1);
406 memcpy(name, data->data, len);
407 name[len] = '\0';
408 return false;
409 default:
410 return true;
411 }
412 }
413
start_scan(void)414 static int start_scan(void)
415 {
416 int err;
417
418 err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, NULL);
419 if (err != 0) {
420 LOG_ERR("Scan start failed: %d", err);
421 return err;
422 }
423
424 LOG_INF("Scan started");
425
426 return 0;
427 }
428
stop_scan(void)429 static int stop_scan(void)
430 {
431 int err;
432
433 err = bt_le_scan_stop();
434 if (err != 0) {
435 LOG_ERR("Scan stop failed: %d", err);
436 return err;
437 }
438
439 LOG_INF("Scan stopped");
440
441 return 0;
442 }
443
scan_recv(const struct bt_le_scan_recv_info * info,struct net_buf_simple * buf)444 static void scan_recv(const struct bt_le_scan_recv_info *info,
445 struct net_buf_simple *buf)
446 {
447 char le_addr[BT_ADDR_LE_STR_LEN];
448 char name[DEVICE_NAME_LEN];
449
450 if (advertiser_found) {
451 return;
452 }
453
454 (void)memset(name, 0, sizeof(name));
455
456 bt_data_parse(buf, data_cb, name);
457
458 if (strncmp(DEVICE_NAME, name, strlen(DEVICE_NAME))) {
459 return;
460 }
461
462 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
463
464 LOG_INF("Found peripheral with address %s (RSSI %i)",
465 le_addr, info->rssi);
466
467
468 bt_addr_le_copy(&adv_addr, info->addr);
469 advertiser_found = true;
470 k_sem_give(&sem_adv);
471 }
472
473 static struct bt_le_scan_cb scan_callbacks = {
474 .recv = scan_recv,
475 };
476
connected(struct bt_conn * conn,uint8_t err)477 static void connected(struct bt_conn *conn, uint8_t err)
478 {
479 char addr[BT_ADDR_LE_STR_LEN];
480
481 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
482
483 if (err != 0 && role == ROLE_CENTRAL) {
484 LOG_INF("Failed to connect to %s: %u %s", addr, err, bt_hci_err_to_str(err));
485
486 bt_conn_unref(default_conn);
487 default_conn = NULL;
488 return;
489 } else if (role == ROLE_PERIPHERAL) {
490 default_conn = bt_conn_ref(conn);
491 }
492
493 LOG_INF("Connected: %s", addr);
494
495 k_sem_give(&sem_connected);
496 }
497
disconnected(struct bt_conn * conn,uint8_t reason)498 static void disconnected(struct bt_conn *conn, uint8_t reason)
499 {
500 char addr[BT_ADDR_LE_STR_LEN];
501
502 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
503
504 LOG_INF("Disconnected: %s, reason 0x%02x %s", addr, reason, bt_hci_err_to_str(reason));
505
506 bt_conn_unref(default_conn);
507 default_conn = NULL;
508 k_sem_give(&sem_disconnected);
509 }
510
511 static struct bt_conn_cb conn_callbacks = {
512 .connected = connected,
513 .disconnected = disconnected,
514 };
515
get_chars(char * buffer,size_t max_size)516 static size_t get_chars(char *buffer, size_t max_size)
517 {
518 size_t pos = 0;
519
520 while (pos < max_size) {
521 char c = tolower(console_getchar());
522
523 if (c == '\n' || c == '\r') {
524 break;
525 }
526 printk("%c", c);
527 buffer[pos++] = c;
528 }
529 printk("\n");
530 buffer[pos] = '\0';
531
532 return pos;
533 }
534
parse_rtn_arg(struct bt_iso_chan_io_qos * qos)535 static int parse_rtn_arg(struct bt_iso_chan_io_qos *qos)
536 {
537 char buffer[4];
538 size_t char_count;
539 uint64_t rtn;
540
541 printk("Set RTN (current %u, default %u)\n",
542 qos->rtn, DEFAULT_CIS_RTN);
543
544 char_count = get_chars(buffer, sizeof(buffer) - 1);
545 if (char_count == 0) {
546 return DEFAULT_CIS_RTN;
547 }
548
549 rtn = strtoul(buffer, NULL, 0);
550 if (rtn > BT_ISO_CONNECTED_RTN_MAX) {
551 printk("Invalid RTN %llu", rtn);
552 return -EINVAL;
553 }
554
555 return (int)rtn;
556 }
557
parse_interval_arg(enum sdu_dir direction)558 static int parse_interval_arg(enum sdu_dir direction)
559 {
560 char buffer[9];
561 size_t char_count;
562 uint64_t interval;
563
564 interval = (direction == DIR_C_TO_P) ?
565 cig_create_param.c_to_p_interval : cig_create_param.p_to_c_interval;
566
567 printk("Set %s interval (us) (current %llu, default %u)\n",
568 (direction == DIR_C_TO_P) ? "C to P" : "P to C",
569 interval, DEFAULT_CIS_INTERVAL_US);
570
571 char_count = get_chars(buffer, sizeof(buffer) - 1);
572 if (char_count == 0) {
573 return DEFAULT_CIS_INTERVAL_US;
574 }
575
576 interval = strtoul(buffer, NULL, 0);
577 if (interval < BT_ISO_SDU_INTERVAL_MIN || interval > BT_ISO_SDU_INTERVAL_MAX) {
578 printk("Invalid interval %llu", interval);
579 return -EINVAL;
580 }
581
582 return (int)interval;
583 }
584
parse_latency_arg(enum sdu_dir direction)585 static int parse_latency_arg(enum sdu_dir direction)
586 {
587 char buffer[6];
588 size_t char_count;
589 uint64_t latency;
590
591 latency = (direction == DIR_C_TO_P) ?
592 cig_create_param.c_to_p_latency : cig_create_param.p_to_c_latency;
593
594 printk("Set %s latency (ms) (current %llu, default %u)\n",
595 (direction == DIR_C_TO_P) ? "C to P" : "P to C",
596 latency, DEFAULT_CIS_LATENCY_MS);
597
598 char_count = get_chars(buffer, sizeof(buffer) - 1);
599 if (char_count == 0) {
600 return DEFAULT_CIS_LATENCY_MS;
601 }
602
603 latency = strtoul(buffer, NULL, 0);
604 if (latency < BT_ISO_LATENCY_MIN || latency > BT_ISO_LATENCY_MAX) {
605 printk("Invalid latency %llu", latency);
606 return -EINVAL;
607 }
608
609 return (int)latency;
610 }
611
parse_phy_arg(struct bt_iso_chan_io_qos * qos)612 static int parse_phy_arg(struct bt_iso_chan_io_qos *qos)
613 {
614 char buffer[3];
615 size_t char_count;
616 uint64_t phy;
617
618 printk("Set PHY (current %u, default %u) - %u = 1M, %u = 2M, %u = Coded\n",
619 qos->phy, DEFAULT_CIS_PHY, BT_GAP_LE_PHY_1M,
620 BT_GAP_LE_PHY_2M, BT_GAP_LE_PHY_CODED);
621
622 char_count = get_chars(buffer, sizeof(buffer) - 1);
623 if (char_count == 0) {
624 return DEFAULT_CIS_PHY;
625 }
626
627 phy = strtoul(buffer, NULL, 0);
628 if (phy != BT_GAP_LE_PHY_1M &&
629 phy != BT_GAP_LE_PHY_2M &&
630 phy != BT_GAP_LE_PHY_CODED) {
631 printk("Invalid PHY %llu", phy);
632 return -EINVAL;
633 }
634
635 return (int)phy;
636 }
637
parse_sdu_arg(struct bt_iso_chan_io_qos * qos)638 static int parse_sdu_arg(struct bt_iso_chan_io_qos *qos)
639 {
640 char buffer[6];
641 size_t char_count;
642 uint64_t sdu;
643
644 printk("Set SDU (current %u, default %u)\n",
645 qos->sdu, DEFAULT_CIS_SDU_SIZE);
646
647 char_count = get_chars(buffer, sizeof(buffer) - 1);
648 if (char_count == 0) {
649 return DEFAULT_CIS_SDU_SIZE;
650 }
651
652 sdu = strtoul(buffer, NULL, 0);
653 if (sdu > MIN(BT_ISO_MAX_SDU, sizeof(iso_data)) ||
654 sdu < sizeof(uint32_t) /* room for the counter */) {
655 printk("Invalid SDU %llu", sdu);
656 return -EINVAL;
657 }
658
659 return (int)sdu;
660 }
661
662 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
parse_c_to_p_ft_arg(void)663 static int parse_c_to_p_ft_arg(void)
664 {
665 char buffer[4];
666 size_t char_count;
667 uint64_t c_to_p_ft;
668
669 printk("Set central to peripheral flush timeout (current %u, default %u)\n",
670 cig_create_param.c_to_p_ft, DEFAULT_CIS_FT);
671
672 char_count = get_chars(buffer, sizeof(buffer) - 1);
673 if (char_count == 0) {
674 return DEFAULT_CIS_FT;
675 }
676
677 c_to_p_ft = strtoul(buffer, NULL, 0);
678 if (!IN_RANGE(c_to_p_ft, BT_ISO_FT_MIN, BT_ISO_FT_MAX)) {
679 printk("Invalid central to peripheral flush timeout %llu",
680 c_to_p_ft);
681
682 return -EINVAL;
683 }
684
685 return (int)c_to_p_ft;
686 }
687
parse_p_to_c_ft_arg(void)688 static int parse_p_to_c_ft_arg(void)
689 {
690 char buffer[4];
691 size_t char_count;
692 uint64_t p_to_c_ft;
693
694 printk("Set peripheral to central flush timeout (current %u, default %u)\n",
695 cig_create_param.p_to_c_ft, DEFAULT_CIS_FT);
696
697 char_count = get_chars(buffer, sizeof(buffer) - 1);
698 if (char_count == 0) {
699 return DEFAULT_CIS_FT;
700 }
701
702 p_to_c_ft = strtoul(buffer, NULL, 0);
703 if (!IN_RANGE(p_to_c_ft, BT_ISO_FT_MIN, BT_ISO_FT_MAX)) {
704 printk("Invalid peripheral to central flush timeout %llu",
705 p_to_c_ft);
706
707 return -EINVAL;
708 }
709
710 return (int)p_to_c_ft;
711 }
712
parse_iso_interval_arg(void)713 static int parse_iso_interval_arg(void)
714 {
715 char buffer[8];
716 size_t char_count;
717 uint64_t iso_interval;
718
719 printk("Set ISO interval (current %u, default %u)\n",
720 cig_create_param.iso_interval, DEFAULT_CIS_ISO_INTERVAL);
721
722 char_count = get_chars(buffer, sizeof(buffer) - 1);
723 if (char_count == 0) {
724 return DEFAULT_CIS_ISO_INTERVAL;
725 }
726
727 iso_interval = strtoul(buffer, NULL, 0);
728 if (IN_RANGE(iso_interval, BT_ISO_ISO_INTERVAL_MIN, BT_ISO_ISO_INTERVAL_MAX)) {
729 printk("Invalid ISO interval %llu", iso_interval);
730
731 return -EINVAL;
732 }
733
734 return (int)iso_interval;
735 }
736
parse_nse_arg(struct bt_iso_chan_qos * qos)737 static int parse_nse_arg(struct bt_iso_chan_qos *qos)
738 {
739 char buffer[4];
740 size_t char_count;
741 uint64_t nse;
742
743 printk("Set number of subevents (current %u, default %u)\n",
744 qos->num_subevents, DEFAULT_CIS_NSE);
745
746 char_count = get_chars(buffer, sizeof(buffer) - 1);
747 if (char_count == 0) {
748 return DEFAULT_CIS_NSE;
749 }
750
751 nse = strtoul(buffer, NULL, 0);
752 if (IN_RANGE(nse, BT_ISO_NSE_MIN, BT_ISO_NSE_MAX)) {
753 printk("Invalid number of subevents %llu", nse);
754
755 return -EINVAL;
756 }
757
758 return (int)nse;
759 }
760
parse_pdu_arg(const struct bt_iso_chan_io_qos * qos)761 static int parse_pdu_arg(const struct bt_iso_chan_io_qos *qos)
762 {
763 char buffer[6];
764 size_t char_count;
765 uint64_t pdu;
766
767 printk("Set PDU (current %u, default %u)\n",
768 qos->max_pdu, DEFAULT_CIS_PDU_SIZE);
769
770 char_count = get_chars(buffer, sizeof(buffer) - 1);
771 if (char_count == 0) {
772 return DEFAULT_CIS_PDU_SIZE;
773 }
774
775 pdu = strtoul(buffer, NULL, 0);
776 if (pdu > BT_ISO_PDU_MAX) {
777 printk("Invalid PDU %llu", pdu);
778
779 return -EINVAL;
780 }
781
782 return (int)pdu;
783 }
784
parse_bn_arg(const struct bt_iso_chan_io_qos * qos)785 static int parse_bn_arg(const struct bt_iso_chan_io_qos *qos)
786 {
787 char buffer[4];
788 size_t char_count;
789 uint64_t bn;
790
791 printk("Set burst number (current %u, default %u)\n",
792 qos->burst_number, DEFAULT_CIS_BN);
793
794 char_count = get_chars(buffer, sizeof(buffer) - 1);
795 if (char_count == 0) {
796 return DEFAULT_CIS_PDU_SIZE;
797 }
798
799 bn = strtoul(buffer, NULL, 0);
800 if (!IN_RANGE(bn, BT_ISO_BN_MIN, BT_ISO_BN_MAX)) {
801 printk("Invalid burst number %llu", bn);
802
803 return -EINVAL;
804 }
805
806 return (int)bn;
807 }
808 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
809
parse_cis_count_arg(void)810 static int parse_cis_count_arg(void)
811 {
812 char buffer[4];
813 size_t char_count;
814 uint64_t cis_count;
815
816 printk("Set CIS count (current %u, default %u)\n",
817 cig_create_param.num_cis, DEFAULT_CIS_COUNT);
818
819 char_count = get_chars(buffer, sizeof(buffer) - 1);
820 if (char_count == 0) {
821 return DEFAULT_CIS_COUNT;
822 }
823
824 cis_count = strtoul(buffer, NULL, 0);
825 if (cis_count > MAX(BT_ISO_MAX_GROUP_ISO_COUNT, CONFIG_BT_ISO_MAX_CHAN)) {
826 printk("Invalid CIS count %llu", cis_count);
827 return -EINVAL;
828 }
829
830 return (int)cis_count;
831 }
832
parse_cig_args(void)833 static int parse_cig_args(void)
834 {
835 int c_to_p_interval;
836 int p_to_c_interval;
837 int c_to_p_latency;
838 int p_to_c_latency;
839 int cis_count;
840 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
841 int c_to_p_ft;
842 int p_to_c_ft;
843 int iso_interval;
844 int num_subevents;
845 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
846
847 printk("Follow the prompts. Press enter to use default values.\n");
848
849 cis_count = parse_cis_count_arg();
850 if (cis_count < 0) {
851 return -EINVAL;
852 }
853
854 c_to_p_interval = parse_interval_arg(DIR_C_TO_P);
855 if (c_to_p_interval < 0) {
856 return -EINVAL;
857 }
858
859 p_to_c_interval = parse_interval_arg(DIR_P_TO_C);
860 if (p_to_c_interval < 0) {
861 return -EINVAL;
862 }
863
864 c_to_p_latency = parse_latency_arg(DIR_C_TO_P);
865 if (c_to_p_latency < 0) {
866 return -EINVAL;
867 }
868
869 p_to_c_latency = parse_latency_arg(DIR_P_TO_C);
870 if (p_to_c_latency < 0) {
871 return -EINVAL;
872 }
873
874 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
875 c_to_p_ft = parse_c_to_p_ft_arg();
876 if (c_to_p_ft < 0) {
877 return -EINVAL;
878 }
879
880 p_to_c_ft = parse_p_to_c_ft_arg();
881 if (p_to_c_ft < 0) {
882 return -EINVAL;
883 }
884
885 iso_interval = parse_iso_interval_arg();
886 if (iso_interval < 0) {
887 return -EINVAL;
888 }
889
890 num_subevents = parse_nse_arg(&iso_qos);
891 if (num_subevents < 0) {
892 return -EINVAL;
893 }
894 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
895
896 cig_create_param.c_to_p_interval = c_to_p_interval;
897 cig_create_param.p_to_c_interval = p_to_c_interval;
898 cig_create_param.c_to_p_latency = c_to_p_latency;
899 cig_create_param.p_to_c_latency = p_to_c_latency;
900 cig_create_param.num_cis = cis_count;
901 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
902 cig_create_param.c_to_p_ft = c_to_p_ft;
903 cig_create_param.p_to_c_ft = p_to_c_ft;
904 cig_create_param.iso_interval = iso_interval;
905 iso_qos.num_subevents = num_subevents;
906 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
907
908 return 0;
909 }
910
parse_cis_args(struct bt_iso_chan_io_qos * qos)911 static int parse_cis_args(struct bt_iso_chan_io_qos *qos)
912 {
913 int rtn;
914 int phy;
915 int sdu;
916 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
917 int max_pdu;
918 int burst_number;
919 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
920
921 printk("Follow the prompts. Press enter to use default values.\n");
922
923 rtn = parse_rtn_arg(qos);
924 if (rtn < 0) {
925 return -EINVAL;
926 }
927
928 phy = parse_phy_arg(qos);
929 if (phy < 0) {
930 return -EINVAL;
931 }
932
933 sdu = parse_sdu_arg(qos);
934 if (sdu < 0) {
935 return -EINVAL;
936 }
937
938 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
939 max_pdu = parse_pdu_arg(qos);
940 if (max_pdu < 0) {
941 return -EINVAL;
942 }
943
944 burst_number = parse_bn_arg(qos);
945 if (burst_number < 0) {
946 return -EINVAL;
947 }
948 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
949
950 qos->rtn = rtn;
951 qos->phy = phy;
952 qos->sdu = sdu;
953 #if defined(CONFIG_BT_ISO_TEST_PARAMS)
954 qos->max_pdu = max_pdu;
955 qos->burst_number = burst_number;
956 #endif /* CONFIG_BT_ISO_TEST_PARAMS */
957
958 return 0;
959 }
960
change_central_settings(void)961 static int change_central_settings(void)
962 {
963 char c;
964 int err;
965
966 printk("Change CIG settings (y/N)? (Current settings: cis_count=%u, "
967 "C to P interval=%u, P to C interval=%u "
968 "C to P latency=%u, P to C latency=%u)\n",
969 cig_create_param.num_cis, cig_create_param.c_to_p_interval,
970 cig_create_param.p_to_c_interval, cig_create_param.c_to_p_latency,
971 cig_create_param.p_to_c_latency);
972
973 c = tolower(console_getchar());
974 if (c == 'y') {
975 err = parse_cig_args();
976 if (err != 0) {
977 return err;
978 }
979
980 printk("New settings: cis_count=%u, C to P interval=%u, "
981 "P TO C interval=%u, C to P latency=%u "
982 "P TO C latency=%u\n",
983 cig_create_param.num_cis, cig_create_param.c_to_p_interval,
984 cig_create_param.p_to_c_interval, cig_create_param.c_to_p_latency,
985 cig_create_param.p_to_c_latency);
986 }
987
988 printk("Change TX settings (y/N)? (Current settings: rtn=%u, "
989 "phy=%u, sdu=%u)\n",
990 iso_tx_qos.rtn, iso_tx_qos.phy, iso_tx_qos.sdu);
991
992 c = tolower(console_getchar());
993 if (c == 'y') {
994 printk("Disable TX (y/N)?\n");
995 c = tolower(console_getchar());
996 if (c == 'y') {
997 iso_qos.tx = NULL;
998 printk("TX disabled\n");
999 } else {
1000 iso_qos.tx = &iso_tx_qos;
1001
1002 err = parse_cis_args(&iso_tx_qos);
1003 if (err != 0) {
1004 return err;
1005 }
1006
1007 printk("New settings: rtn=%u, phy=%u, sdu=%u\n",
1008 iso_tx_qos.rtn, iso_tx_qos.phy, iso_tx_qos.sdu);
1009 }
1010 }
1011
1012 printk("Change RX settings (y/N)? (Current settings: rtn=%u, "
1013 "phy=%u, sdu=%u)\n",
1014 iso_rx_qos.rtn, iso_rx_qos.phy, iso_rx_qos.sdu);
1015
1016 c = tolower(console_getchar());
1017 if (c == 'y') {
1018 printk("Disable RX (y/N)?\n");
1019 c = tolower(console_getchar());
1020 if (c == 'y') {
1021 if (iso_qos.tx == NULL) {
1022 LOG_ERR("Cannot disable both TX and RX\n");
1023 return -EINVAL;
1024 }
1025
1026 iso_qos.rx = NULL;
1027 printk("RX disabled\n");
1028 } else {
1029
1030 printk("Set RX settings to TX settings (Y/n)?\n");
1031
1032 c = tolower(console_getchar());
1033 if (c == 'n') {
1034 err = parse_cis_args(&iso_rx_qos);
1035 if (err != 0) {
1036 return err;
1037 }
1038
1039 printk("New settings: rtn=%u, phy=%u, sdu=%u\n",
1040 iso_rx_qos.rtn, iso_rx_qos.phy,
1041 iso_rx_qos.sdu);
1042 } else {
1043 (void)memcpy(&iso_rx_qos, &iso_tx_qos,
1044 sizeof(iso_rx_qos));
1045 }
1046 }
1047 }
1048
1049 return 0;
1050 }
1051
central_create_connection(void)1052 static int central_create_connection(void)
1053 {
1054 /* Give the controller a large range of intervals to pick from. In this benchmark sample we
1055 * want to prioritize ISO over ACL, but will leave the actual choice up to the controller.
1056 */
1057 const struct bt_le_conn_param *conn_param =
1058 BT_LE_CONN_PARAM(BT_GAP_INIT_CONN_INT_MIN, BT_GAP_MS_TO_CONN_INTERVAL(500U), 0,
1059 BT_GAP_MS_TO_CONN_TIMEOUT(4000));
1060 int err;
1061
1062 advertiser_found = false;
1063
1064 err = start_scan();
1065 if (err != 0) {
1066 LOG_ERR("Could not start scan: %d", err);
1067 return err;
1068 }
1069
1070 LOG_INF("Waiting for advertiser");
1071 err = k_sem_take(&sem_adv, K_FOREVER);
1072 if (err != 0) {
1073 LOG_ERR("failed to take sem_adv: %d", err);
1074 return err;
1075 }
1076
1077 LOG_INF("Stopping scan");
1078 err = stop_scan();
1079 if (err != 0) {
1080 LOG_ERR("Could not stop scan: %d", err);
1081 return err;
1082 }
1083
1084 LOG_INF("Connecting");
1085 err = bt_conn_le_create(&adv_addr, BT_CONN_LE_CREATE_CONN, conn_param, &default_conn);
1086 if (err != 0) {
1087 LOG_ERR("Create connection failed: %d", err);
1088 return err;
1089 }
1090
1091 err = k_sem_take(&sem_connected, K_FOREVER);
1092 if (err != 0) {
1093 LOG_ERR("failed to take sem_connected: %d", err);
1094 return err;
1095 }
1096
1097 return 0;
1098 }
1099
central_create_cig(void)1100 static int central_create_cig(void)
1101 {
1102 int err;
1103
1104 iso_conn_start_time = 0;
1105
1106 LOG_INF("Creating CIG");
1107
1108 err = bt_iso_cig_create(&cig_create_param, &cig);
1109 if (err != 0) {
1110 LOG_ERR("Failed to create CIG: %d", err);
1111 return err;
1112 }
1113
1114 return 0;
1115 }
1116
central_connect_cis(void)1117 static int central_connect_cis(void)
1118 {
1119 struct bt_iso_connect_param connect_param[CONFIG_BT_ISO_MAX_CHAN];
1120 int err;
1121
1122 iso_conn_start_time = 0;
1123
1124 LOG_INF("Connecting ISO channels");
1125
1126 for (int i = 0; i < cig_create_param.num_cis; i++) {
1127 connect_param[i].acl = default_conn;
1128 connect_param[i].iso_chan = &iso_chans[i].chan;
1129 }
1130
1131 err = bt_iso_chan_connect(connect_param, cig_create_param.num_cis);
1132 if (err != 0) {
1133 LOG_ERR("Failed to connect iso: %d", err);
1134 return err;
1135 }
1136 total_iso_conn_count++;
1137
1138 for (int i = 0; i < cig_create_param.num_cis; i++) {
1139 err = k_sem_take(&sem_iso_connected, K_FOREVER);
1140 if (err != 0) {
1141 LOG_ERR("failed to take sem_iso_connected: %d", err);
1142 return err;
1143 }
1144 }
1145
1146 return 0;
1147 }
1148
reset_sems(void)1149 static void reset_sems(void)
1150 {
1151 (void)k_sem_reset(&sem_adv);
1152 (void)k_sem_reset(&sem_iso_accept);
1153 (void)k_sem_reset(&sem_iso_connected);
1154 (void)k_sem_reset(&sem_iso_disconnected);
1155 (void)k_sem_reset(&sem_connected);
1156 (void)k_sem_reset(&sem_disconnected);
1157 }
1158
cleanup(void)1159 static int cleanup(void)
1160 {
1161 int err;
1162
1163 for (size_t i = 0; i < cig_create_param.num_cis; i++) {
1164 (void)k_work_cancel_delayable(&iso_chans[i].send_work);
1165 }
1166
1167 err = k_sem_take(&sem_disconnected, K_NO_WAIT);
1168 if (err != 0) {
1169 for (int i = 0; i < cig_create_param.num_cis; i++) {
1170 err = k_sem_take(&sem_iso_disconnected, K_NO_WAIT);
1171 if (err == 0) {
1172 err = bt_iso_chan_disconnect(&iso_chans[i].chan);
1173 if (err != 0) {
1174 LOG_ERR("Could not disconnect ISO[%d]: %d",
1175 i, err);
1176 break;
1177 }
1178 } /* else ISO already disconnected */
1179 }
1180
1181 err = bt_conn_disconnect(default_conn,
1182 BT_HCI_ERR_REMOTE_USER_TERM_CONN);
1183 if (err != 0) {
1184 LOG_ERR("Could not disconnect ACL: %d", err);
1185 return err;
1186 }
1187
1188 err = k_sem_take(&sem_disconnected, K_FOREVER);
1189 if (err != 0) {
1190 LOG_ERR("failed to take sem_disconnected: %d", err);
1191 return err;
1192 }
1193 } /* else ACL already disconnected */
1194
1195 if (cig) {
1196 err = bt_iso_cig_terminate(cig);
1197 if (err != 0) {
1198 LOG_ERR("Could not terminate CIG: %d", err);
1199 return err;
1200 }
1201 cig = NULL;
1202 }
1203
1204 return err;
1205 }
1206
run_central(void)1207 static int run_central(void)
1208 {
1209 int err;
1210 char c;
1211
1212 iso_conn_start_time = 0;
1213 last_received_counter = 0;
1214 memset(&stats_current_conn, 0, sizeof(stats_current_conn));
1215 reset_sems();
1216
1217 printk("Change ISO settings (y/N)?\n");
1218 c = tolower(console_getchar());
1219 if (c == 'y') {
1220 err = change_central_settings();
1221 if (err != 0) {
1222 LOG_ERR("Failed to set parameters: %d", err);
1223 return err;
1224 }
1225 }
1226
1227 /* Creating the CIG before connecting verified that it's possible before establishing a
1228 * connection, while also providing the controller information about our use case before
1229 * creating the connection, which should provide additional information to the controller
1230 * about which connection interval to use
1231 */
1232 err = central_create_cig();
1233 if (err != 0) {
1234 LOG_ERR("Failed to create CIG: %d", err);
1235 return err;
1236 }
1237
1238 err = central_create_connection();
1239 if (err != 0) {
1240 LOG_ERR("Failed to create connection: %d", err);
1241 return err;
1242 }
1243
1244 err = central_connect_cis();
1245 if (err != 0) {
1246 LOG_ERR("Failed to connect CISes: %d", err);
1247 return err;
1248 }
1249
1250 for (size_t i = 0; i < cig_create_param.num_cis; i++) {
1251 struct k_work_delayable *work = &iso_chans[i].send_work;
1252
1253 k_work_init_delayable(work, iso_timer_timeout);
1254
1255 for (int j = 0; j < BUFFERS_ENQUEUED; j++) {
1256 iso_send(&iso_chans[i].chan);
1257 }
1258 }
1259
1260 err = k_sem_take(&sem_disconnected, K_FOREVER);
1261 if (err != 0) {
1262 LOG_ERR("failed to take sem_disconnected: %d", err);
1263 return err;
1264 }
1265
1266 LOG_INF("Disconnected - Cleaning up");
1267 for (size_t i = 0; i < cig_create_param.num_cis; i++) {
1268 (void)k_work_cancel_delayable(&iso_chans[i].send_work);
1269 }
1270
1271 for (int i = 0; i < cig_create_param.num_cis; i++) {
1272 err = k_sem_take(&sem_iso_disconnected, K_FOREVER);
1273 if (err != 0) {
1274 LOG_ERR("failed to take sem_iso_disconnected: %d", err);
1275 return err;
1276 }
1277 }
1278
1279 err = bt_iso_cig_terminate(cig);
1280 if (err != 0) {
1281 LOG_ERR("Could not terminate CIG: %d", err);
1282 return err;
1283 }
1284 cig = NULL;
1285
1286 return 0;
1287 }
1288
run_peripheral(void)1289 static int run_peripheral(void)
1290 {
1291 int err;
1292 static bool initialized;
1293
1294 /* Reset */
1295 cig_create_param.num_cis = 0;
1296 iso_conn_start_time = 0;
1297 last_received_counter = 0;
1298 memset(&stats_current_conn, 0, sizeof(stats_current_conn));
1299 reset_sems();
1300
1301 if (!initialized) {
1302 LOG_INF("Registering ISO server");
1303 err = bt_iso_server_register(&iso_server);
1304 if (err != 0) {
1305 LOG_ERR("ISO server register failed: %d", err);
1306 return err;
1307 }
1308 initialized = true;
1309 }
1310
1311 LOG_INF("Starting advertising");
1312 err = bt_le_adv_start(BT_LE_ADV_CONN_FAST_1, NULL, 0, sd, ARRAY_SIZE(sd));
1313 if (err != 0) {
1314 LOG_ERR("Advertising failed to start: %d", err);
1315 return err;
1316 }
1317
1318 LOG_INF("Waiting for ACL connection");
1319 err = k_sem_take(&sem_connected, K_FOREVER);
1320 if (err != 0) {
1321 LOG_ERR("failed to take sem_connected: %d", err);
1322 return err;
1323 }
1324
1325 err = bt_le_adv_stop();
1326 if (err != 0) {
1327 LOG_ERR("Advertising failed to stop: %d", err);
1328 return err;
1329 }
1330
1331 LOG_INF("Waiting for ISO connection");
1332
1333 err = k_sem_take(&sem_iso_accept, K_SECONDS(2));
1334 if (err != 0) {
1335 return err;
1336 }
1337
1338 for (int i = 0; i < cig_create_param.num_cis; i++) {
1339 err = k_sem_take(&sem_iso_connected, K_FOREVER);
1340 if (err != 0) {
1341 LOG_ERR("failed to take sem_iso_connected: %d", err);
1342 return err;
1343 }
1344 }
1345 total_iso_conn_count++;
1346
1347 for (size_t i = 0; i < cig_create_param.num_cis; i++) {
1348 struct k_work_delayable *work = &iso_chans[i].send_work;
1349
1350 k_work_init_delayable(work, iso_timer_timeout);
1351
1352 for (int j = 0; j < BUFFERS_ENQUEUED; j++) {
1353 iso_send(&iso_chans[i].chan);
1354 }
1355 }
1356
1357 /* Wait for disconnect */
1358 err = k_sem_take(&sem_disconnected, K_FOREVER);
1359 if (err != 0) {
1360 LOG_ERR("failed to take sem_disconnected: %d", err);
1361 return err;
1362 }
1363
1364 for (int i = 0; i < cig_create_param.num_cis; i++) {
1365 err = k_sem_take(&sem_iso_disconnected, K_FOREVER);
1366 if (err != 0) {
1367 LOG_ERR("failed to take sem_iso_disconnected: %d", err);
1368 return err;
1369 }
1370 }
1371
1372 LOG_INF("Disconnected - Cleaning up");
1373 for (size_t i = 0; i < cig_create_param.num_cis; i++) {
1374 (void)k_work_cancel_delayable(&iso_chans[i].send_work);
1375 }
1376
1377 return 0;
1378 }
1379
main(void)1380 int main(void)
1381 {
1382 int err;
1383
1384 LOG_INF("Starting Bluetooth Throughput example");
1385
1386 err = bt_enable(NULL);
1387 if (err != 0) {
1388 LOG_ERR("Bluetooth init failed: %d", err);
1389 return 0;
1390 }
1391
1392 bt_conn_cb_register(&conn_callbacks);
1393 bt_le_scan_cb_register(&scan_callbacks);
1394
1395 err = console_init();
1396 if (err != 0) {
1397 LOG_ERR("Console init failed: %d", err);
1398 return 0;
1399 }
1400
1401 LOG_INF("Bluetooth initialized");
1402
1403 for (int i = 0; i < ARRAY_SIZE(iso_chans); i++) {
1404 iso_chans[i].chan.ops = &iso_ops;
1405 iso_chans[i].chan.qos = &iso_qos;
1406 cis[i] = &iso_chans[i].chan;
1407 }
1408
1409 /* Init data */
1410 for (int i = 0; i < iso_tx_qos.sdu; i++) {
1411 if (i < sizeof(iso_send_count)) {
1412 continue;
1413 }
1414 iso_data[i] = (uint8_t)i;
1415 }
1416
1417 while (true) {
1418 int cleanup_err;
1419
1420 role = device_role_select();
1421
1422 if (role == ROLE_CENTRAL) {
1423 err = run_central();
1424 } else if (role == ROLE_PERIPHERAL) {
1425 err = run_peripheral();
1426 } else {
1427 if (role != ROLE_QUIT) {
1428 LOG_INF("Invalid role %u", role);
1429 continue;
1430 } else {
1431 err = 0;
1432 break;
1433 }
1434 }
1435
1436 if (err != 0) {
1437 cleanup_err = cleanup();
1438 if (cleanup_err != 0) {
1439 LOG_ERR("Could not clean up: %d", err);
1440 }
1441 }
1442
1443 LOG_INF("Test complete: %d", err);
1444 }
1445
1446 LOG_INF("Exiting");
1447 return 0;
1448 }
1449
