1 /*
2 * Copyright (c) 2021 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/device.h>
8 #include <zephyr/devicetree.h>
9 #include <zephyr/drivers/gpio.h>
10 #include <zephyr/bluetooth/bluetooth.h>
11 #include <zephyr/bluetooth/conn.h>
12 #include <zephyr/bluetooth/iso.h>
13 #include <zephyr/sys/byteorder.h>
14
15 #define TIMEOUT_SYNC_CREATE K_SECONDS(10)
16 #define NAME_LEN 30
17
18 #define BT_LE_SCAN_CUSTOM BT_LE_SCAN_PARAM(BT_LE_SCAN_TYPE_ACTIVE, \
19 BT_LE_SCAN_OPT_NONE, \
20 BT_GAP_SCAN_FAST_INTERVAL, \
21 BT_GAP_SCAN_FAST_WINDOW)
22
23 #define PA_RETRY_COUNT 6
24
25 #define BIS_ISO_CHAN_COUNT MIN(2U, CONFIG_BT_ISO_MAX_CHAN)
26
27 static bool per_adv_found;
28 static bool per_adv_lost;
29 static bt_addr_le_t per_addr;
30 static uint8_t per_sid;
31 static uint32_t per_interval_us;
32
33 static uint32_t iso_recv_count;
34
35 static K_SEM_DEFINE(sem_per_adv, 0, 1);
36 static K_SEM_DEFINE(sem_per_sync, 0, 1);
37 static K_SEM_DEFINE(sem_per_sync_lost, 0, 1);
38 static K_SEM_DEFINE(sem_per_big_info, 0, 1);
39 static K_SEM_DEFINE(sem_big_sync, 0, BIS_ISO_CHAN_COUNT);
40 static K_SEM_DEFINE(sem_big_sync_lost, 0, BIS_ISO_CHAN_COUNT);
41
42 /* The devicetree node identifier for the "led0" alias. */
43 #define LED0_NODE DT_ALIAS(led0)
44
45 #ifdef CONFIG_ISO_BLINK_LED0
46 static const struct gpio_dt_spec led_gpio = GPIO_DT_SPEC_GET(LED0_NODE, gpios);
47 #define BLINK_ONOFF K_MSEC(500)
48
49 static struct k_work_delayable blink_work;
50 static bool led_is_on;
51 static bool blink;
52
blink_timeout(struct k_work * work)53 static void blink_timeout(struct k_work *work)
54 {
55 if (!blink) {
56 return;
57 }
58
59 led_is_on = !led_is_on;
60 gpio_pin_set_dt(&led_gpio, (int)led_is_on);
61
62 k_work_schedule(&blink_work, BLINK_ONOFF);
63 }
64 #endif
65
data_cb(struct bt_data * data,void * user_data)66 static bool data_cb(struct bt_data *data, void *user_data)
67 {
68 char *name = user_data;
69 uint8_t len;
70
71 switch (data->type) {
72 case BT_DATA_NAME_SHORTENED:
73 case BT_DATA_NAME_COMPLETE:
74 len = MIN(data->data_len, NAME_LEN - 1);
75 memcpy(name, data->data, len);
76 name[len] = '\0';
77 return false;
78 default:
79 return true;
80 }
81 }
82
phy2str(uint8_t phy)83 static const char *phy2str(uint8_t phy)
84 {
85 switch (phy) {
86 case 0: return "No packets";
87 case BT_GAP_LE_PHY_1M: return "LE 1M";
88 case BT_GAP_LE_PHY_2M: return "LE 2M";
89 case BT_GAP_LE_PHY_CODED: return "LE Coded";
90 default: return "Unknown";
91 }
92 }
93
scan_recv(const struct bt_le_scan_recv_info * info,struct net_buf_simple * buf)94 static void scan_recv(const struct bt_le_scan_recv_info *info,
95 struct net_buf_simple *buf)
96 {
97 char le_addr[BT_ADDR_LE_STR_LEN];
98 char name[NAME_LEN];
99
100 (void)memset(name, 0, sizeof(name));
101
102 bt_data_parse(buf, data_cb, name);
103
104 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
105 printk("[DEVICE]: %s, AD evt type %u, Tx Pwr: %i, RSSI %i %s "
106 "C:%u S:%u D:%u SR:%u E:%u Prim: %s, Secn: %s, "
107 "Interval: 0x%04x (%u us), SID: %u\n",
108 le_addr, info->adv_type, info->tx_power, info->rssi, name,
109 (info->adv_props & BT_GAP_ADV_PROP_CONNECTABLE) != 0,
110 (info->adv_props & BT_GAP_ADV_PROP_SCANNABLE) != 0,
111 (info->adv_props & BT_GAP_ADV_PROP_DIRECTED) != 0,
112 (info->adv_props & BT_GAP_ADV_PROP_SCAN_RESPONSE) != 0,
113 (info->adv_props & BT_GAP_ADV_PROP_EXT_ADV) != 0,
114 phy2str(info->primary_phy), phy2str(info->secondary_phy),
115 info->interval, BT_CONN_INTERVAL_TO_US(info->interval), info->sid);
116
117 if (!per_adv_found && info->interval) {
118 per_adv_found = true;
119
120 per_sid = info->sid;
121 per_interval_us = BT_CONN_INTERVAL_TO_US(info->interval);
122 bt_addr_le_copy(&per_addr, info->addr);
123
124 k_sem_give(&sem_per_adv);
125 }
126 }
127
128 static struct bt_le_scan_cb scan_callbacks = {
129 .recv = scan_recv,
130 };
131
sync_cb(struct bt_le_per_adv_sync * sync,struct bt_le_per_adv_sync_synced_info * info)132 static void sync_cb(struct bt_le_per_adv_sync *sync,
133 struct bt_le_per_adv_sync_synced_info *info)
134 {
135 char le_addr[BT_ADDR_LE_STR_LEN];
136
137 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
138
139 printk("PER_ADV_SYNC[%u]: [DEVICE]: %s synced, "
140 "Interval 0x%04x (%u ms), PHY %s\n",
141 bt_le_per_adv_sync_get_index(sync), le_addr,
142 info->interval, info->interval * 5 / 4, phy2str(info->phy));
143
144 k_sem_give(&sem_per_sync);
145 }
146
term_cb(struct bt_le_per_adv_sync * sync,const struct bt_le_per_adv_sync_term_info * info)147 static void term_cb(struct bt_le_per_adv_sync *sync,
148 const struct bt_le_per_adv_sync_term_info *info)
149 {
150 char le_addr[BT_ADDR_LE_STR_LEN];
151
152 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
153
154 printk("PER_ADV_SYNC[%u]: [DEVICE]: %s sync terminated\n",
155 bt_le_per_adv_sync_get_index(sync), le_addr);
156
157 per_adv_lost = true;
158 k_sem_give(&sem_per_sync_lost);
159 }
160
recv_cb(struct bt_le_per_adv_sync * sync,const struct bt_le_per_adv_sync_recv_info * info,struct net_buf_simple * buf)161 static void recv_cb(struct bt_le_per_adv_sync *sync,
162 const struct bt_le_per_adv_sync_recv_info *info,
163 struct net_buf_simple *buf)
164 {
165 char le_addr[BT_ADDR_LE_STR_LEN];
166 char data_str[129];
167
168 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
169 bin2hex(buf->data, buf->len, data_str, sizeof(data_str));
170
171 printk("PER_ADV_SYNC[%u]: [DEVICE]: %s, tx_power %i, "
172 "RSSI %i, CTE %u, data length %u, data: %s\n",
173 bt_le_per_adv_sync_get_index(sync), le_addr, info->tx_power,
174 info->rssi, info->cte_type, buf->len, data_str);
175 }
176
biginfo_cb(struct bt_le_per_adv_sync * sync,const struct bt_iso_biginfo * biginfo)177 static void biginfo_cb(struct bt_le_per_adv_sync *sync,
178 const struct bt_iso_biginfo *biginfo)
179 {
180 char le_addr[BT_ADDR_LE_STR_LEN];
181
182 bt_addr_le_to_str(biginfo->addr, le_addr, sizeof(le_addr));
183
184 printk("BIG INFO[%u]: [DEVICE]: %s, sid 0x%02x, "
185 "num_bis %u, nse %u, interval 0x%04x (%u ms), "
186 "bn %u, pto %u, irc %u, max_pdu %u, "
187 "sdu_interval %u us, max_sdu %u, phy %s, "
188 "%s framing, %sencrypted\n",
189 bt_le_per_adv_sync_get_index(sync), le_addr, biginfo->sid,
190 biginfo->num_bis, biginfo->sub_evt_count,
191 biginfo->iso_interval,
192 (biginfo->iso_interval * 5 / 4),
193 biginfo->burst_number, biginfo->offset,
194 biginfo->rep_count, biginfo->max_pdu, biginfo->sdu_interval,
195 biginfo->max_sdu, phy2str(biginfo->phy),
196 biginfo->framing ? "with" : "without",
197 biginfo->encryption ? "" : "not ");
198
199
200 k_sem_give(&sem_per_big_info);
201 }
202
203 static struct bt_le_per_adv_sync_cb sync_callbacks = {
204 .synced = sync_cb,
205 .term = term_cb,
206 .recv = recv_cb,
207 .biginfo = biginfo_cb,
208 };
209
iso_recv(struct bt_iso_chan * chan,const struct bt_iso_recv_info * info,struct net_buf * buf)210 static void iso_recv(struct bt_iso_chan *chan, const struct bt_iso_recv_info *info,
211 struct net_buf *buf)
212 {
213 char data_str[128];
214 size_t str_len;
215 uint32_t count = 0; /* only valid if the data is a counter */
216
217 if (buf->len == sizeof(count)) {
218 count = sys_get_le32(buf->data);
219 if (IS_ENABLED(CONFIG_ISO_ALIGN_PRINT_INTERVALS)) {
220 iso_recv_count = count;
221 }
222 }
223
224 if ((iso_recv_count % CONFIG_ISO_PRINT_INTERVAL) == 0) {
225 str_len = bin2hex(buf->data, buf->len, data_str, sizeof(data_str));
226 printk("Incoming data channel %p flags 0x%x seq_num %u ts %u len %u: "
227 "%s (counter value %u)\n", chan, info->flags, info->seq_num,
228 info->ts, buf->len, data_str, count);
229 }
230
231 iso_recv_count++;
232 }
233
iso_connected(struct bt_iso_chan * chan)234 static void iso_connected(struct bt_iso_chan *chan)
235 {
236 printk("ISO Channel %p connected\n", chan);
237 k_sem_give(&sem_big_sync);
238 }
239
iso_disconnected(struct bt_iso_chan * chan,uint8_t reason)240 static void iso_disconnected(struct bt_iso_chan *chan, uint8_t reason)
241 {
242 printk("ISO Channel %p disconnected with reason 0x%02x\n",
243 chan, reason);
244
245 if (reason != BT_HCI_ERR_OP_CANCELLED_BY_HOST) {
246 k_sem_give(&sem_big_sync_lost);
247 }
248 }
249
250 static struct bt_iso_chan_ops iso_ops = {
251 .recv = iso_recv,
252 .connected = iso_connected,
253 .disconnected = iso_disconnected,
254 };
255
256 static struct bt_iso_chan_io_qos iso_rx_qos[BIS_ISO_CHAN_COUNT];
257
258 static struct bt_iso_chan_qos bis_iso_qos[] = {
259 { .rx = &iso_rx_qos[0], },
260 { .rx = &iso_rx_qos[1], },
261 };
262
263 static struct bt_iso_chan bis_iso_chan[] = {
264 { .ops = &iso_ops,
265 .qos = &bis_iso_qos[0], },
266 { .ops = &iso_ops,
267 .qos = &bis_iso_qos[1], },
268 };
269
270 static struct bt_iso_chan *bis[] = {
271 &bis_iso_chan[0],
272 &bis_iso_chan[1],
273 };
274
275 static struct bt_iso_big_sync_param big_sync_param = {
276 .bis_channels = bis,
277 .num_bis = BIS_ISO_CHAN_COUNT,
278 .bis_bitfield = (BIT_MASK(BIS_ISO_CHAN_COUNT)),
279 .mse = BT_ISO_SYNC_MSE_ANY, /* any number of subevents */
280 .sync_timeout = 100, /* in 10 ms units */
281 };
282
reset_semaphores(void)283 static void reset_semaphores(void)
284 {
285 k_sem_reset(&sem_per_adv);
286 k_sem_reset(&sem_per_sync);
287 k_sem_reset(&sem_per_sync_lost);
288 k_sem_reset(&sem_per_big_info);
289 k_sem_reset(&sem_big_sync);
290 k_sem_reset(&sem_big_sync_lost);
291 }
292
main(void)293 int main(void)
294 {
295 struct bt_le_per_adv_sync_param sync_create_param;
296 struct bt_le_per_adv_sync *sync;
297 struct bt_iso_big *big;
298 uint32_t sem_timeout_us;
299 int err;
300
301 iso_recv_count = 0;
302
303 printk("Starting Synchronized Receiver Demo\n");
304
305 #ifdef CONFIG_ISO_BLINK_LED0
306 printk("Get reference to LED device...");
307
308 if (!gpio_is_ready_dt(&led_gpio)) {
309 printk("LED gpio device not ready.\n");
310 return 0;
311 }
312 printk("done.\n");
313
314 printk("Configure GPIO pin...");
315 err = gpio_pin_configure_dt(&led_gpio, GPIO_OUTPUT_ACTIVE);
316 if (err) {
317 return 0;
318 }
319 printk("done.\n");
320
321 k_work_init_delayable(&blink_work, blink_timeout);
322 #endif /* CONFIG_ISO_BLINK_LED0 */
323
324 /* Initialize the Bluetooth Subsystem */
325 err = bt_enable(NULL);
326 if (err) {
327 printk("Bluetooth init failed (err %d)\n", err);
328 return 0;
329 }
330
331 printk("Scan callbacks register...");
332 bt_le_scan_cb_register(&scan_callbacks);
333 printk("success.\n");
334
335 printk("Periodic Advertising callbacks register...");
336 bt_le_per_adv_sync_cb_register(&sync_callbacks);
337 printk("Success.\n");
338
339 do {
340 reset_semaphores();
341 per_adv_lost = false;
342
343 printk("Start scanning...");
344 err = bt_le_scan_start(BT_LE_SCAN_CUSTOM, NULL);
345 if (err) {
346 printk("failed (err %d)\n", err);
347 return 0;
348 }
349 printk("success.\n");
350
351 #ifdef CONFIG_ISO_BLINK_LED0
352 printk("Start blinking LED...\n");
353 led_is_on = false;
354 blink = true;
355 gpio_pin_set_dt(&led_gpio, (int)led_is_on);
356 k_work_reschedule(&blink_work, BLINK_ONOFF);
357 #endif /* CONFIG_ISO_BLINK_LED0 */
358
359 printk("Waiting for periodic advertising...\n");
360 per_adv_found = false;
361 err = k_sem_take(&sem_per_adv, K_FOREVER);
362 if (err) {
363 printk("failed (err %d)\n", err);
364 return 0;
365 }
366 printk("Found periodic advertising.\n");
367
368 printk("Stop scanning...");
369 err = bt_le_scan_stop();
370 if (err) {
371 printk("failed (err %d)\n", err);
372 return 0;
373 }
374 printk("success.\n");
375
376 printk("Creating Periodic Advertising Sync...");
377 bt_addr_le_copy(&sync_create_param.addr, &per_addr);
378 sync_create_param.options = 0;
379 sync_create_param.sid = per_sid;
380 sync_create_param.skip = 0;
381 /* Multiple PA interval with retry count and convert to unit of 10 ms */
382 sync_create_param.timeout = (per_interval_us * PA_RETRY_COUNT) /
383 (10 * USEC_PER_MSEC);
384 sem_timeout_us = per_interval_us * PA_RETRY_COUNT;
385 err = bt_le_per_adv_sync_create(&sync_create_param, &sync);
386 if (err) {
387 printk("failed (err %d)\n", err);
388 return 0;
389 }
390 printk("success.\n");
391
392 printk("Waiting for periodic sync...\n");
393 err = k_sem_take(&sem_per_sync, K_USEC(sem_timeout_us));
394 if (err) {
395 printk("failed (err %d)\n", err);
396
397 printk("Deleting Periodic Advertising Sync...");
398 err = bt_le_per_adv_sync_delete(sync);
399 if (err) {
400 printk("failed (err %d)\n", err);
401 return 0;
402 }
403 continue;
404 }
405 printk("Periodic sync established.\n");
406
407 printk("Waiting for BIG info...\n");
408 err = k_sem_take(&sem_per_big_info, K_USEC(sem_timeout_us));
409 if (err) {
410 printk("failed (err %d)\n", err);
411
412 if (per_adv_lost) {
413 continue;
414 }
415
416 printk("Deleting Periodic Advertising Sync...");
417 err = bt_le_per_adv_sync_delete(sync);
418 if (err) {
419 printk("failed (err %d)\n", err);
420 return 0;
421 }
422 continue;
423 }
424 printk("Periodic sync established.\n");
425
426 big_sync_create:
427 printk("Create BIG Sync...\n");
428 err = bt_iso_big_sync(sync, &big_sync_param, &big);
429 if (err) {
430 printk("failed (err %d)\n", err);
431 return 0;
432 }
433 printk("success.\n");
434
435 for (uint8_t chan = 0U; chan < BIS_ISO_CHAN_COUNT; chan++) {
436 printk("Waiting for BIG sync chan %u...\n", chan);
437 err = k_sem_take(&sem_big_sync, TIMEOUT_SYNC_CREATE);
438 if (err) {
439 break;
440 }
441 printk("BIG sync chan %u successful.\n", chan);
442 }
443 if (err) {
444 printk("failed (err %d)\n", err);
445
446 printk("BIG Sync Terminate...");
447 err = bt_iso_big_terminate(big);
448 if (err) {
449 printk("failed (err %d)\n", err);
450 return 0;
451 }
452 printk("done.\n");
453
454 goto per_sync_lost_check;
455 }
456 printk("BIG sync established.\n");
457
458 #ifdef CONFIG_ISO_BLINK_LED0
459 printk("Stop blinking LED.\n");
460 blink = false;
461 /* If this fails, we'll exit early in the handler because blink
462 * is false.
463 */
464 k_work_cancel_delayable(&blink_work);
465
466 /* Keep LED on */
467 led_is_on = true;
468 gpio_pin_set_dt(&led_gpio, (int)led_is_on);
469 #endif /* CONFIG_ISO_BLINK_LED0 */
470
471 for (uint8_t chan = 0U; chan < BIS_ISO_CHAN_COUNT; chan++) {
472 printk("Waiting for BIG sync lost chan %u...\n", chan);
473 err = k_sem_take(&sem_big_sync_lost, K_FOREVER);
474 if (err) {
475 printk("failed (err %d)\n", err);
476 return 0;
477 }
478 printk("BIG sync lost chan %u.\n", chan);
479 }
480 printk("BIG sync lost.\n");
481
482 per_sync_lost_check:
483 printk("Check for periodic sync lost...\n");
484 err = k_sem_take(&sem_per_sync_lost, K_NO_WAIT);
485 if (err) {
486 /* Periodic Sync active, go back to creating BIG Sync */
487 goto big_sync_create;
488 }
489 printk("Periodic sync lost.\n");
490 } while (true);
491 }
492
