1 /*
2 * Copyright (c) 2022-2024 Nordic Semiconductor ASA
3 * Copyright (c) 2024 Demant A/S
4 *
5 * SPDX-License-Identifier: Apache-2.0
6 */
7
8 #include <errno.h>
9 #include <stdint.h>
10 #include <string.h>
11 #include <strings.h>
12
13 #include <zephyr/autoconf.h>
14 #include <zephyr/bluetooth/addr.h>
15 #include <zephyr/bluetooth/audio/lc3.h>
16 #include <zephyr/bluetooth/bluetooth.h>
17 #include <zephyr/bluetooth/audio/audio.h>
18 #include <zephyr/bluetooth/audio/bap.h>
19 #include <zephyr/bluetooth/audio/pacs.h>
20 #include <zephyr/bluetooth/conn.h>
21 #include <zephyr/bluetooth/gap.h>
22 #include <zephyr/bluetooth/hci.h>
23 #include <zephyr/bluetooth/hci_types.h>
24 #include <zephyr/bluetooth/iso.h>
25 #include <zephyr/bluetooth/uuid.h>
26 #include <zephyr/kernel.h>
27 #include <zephyr/net_buf.h>
28 #include <zephyr/sys/byteorder.h>
29 #include <zephyr/sys/printk.h>
30 #include <zephyr/sys/util.h>
31 #include <zephyr/sys/util_macro.h>
32 #include <zephyr/sys_clock.h>
33 #include <zephyr/toolchain.h>
34
35 #if defined(CONFIG_LIBLC3)
36 #include "lc3.h"
37 #endif /* defined(CONFIG_LIBLC3) */
38 #if defined(CONFIG_USB_DEVICE_AUDIO)
39 #include <zephyr/device.h>
40 #include <zephyr/devicetree.h>
41 #include <zephyr/usb/usb_device.h>
42 #include <zephyr/usb/class/usb_audio.h>
43 #include <zephyr/sys/ring_buffer.h>
44 #endif /* defined(CONFIG_USB_DEVICE_AUDIO) */
45
46
47 BUILD_ASSERT(IS_ENABLED(CONFIG_SCAN_SELF) || IS_ENABLED(CONFIG_SCAN_OFFLOAD),
48 "Either SCAN_SELF or SCAN_OFFLOAD must be enabled");
49
50 #define SEM_TIMEOUT K_SECONDS(60)
51 #define BROADCAST_ASSISTANT_TIMEOUT K_SECONDS(120) /* 2 minutes */
52
53 #define LOG_INTERVAL 1000U
54
55 #if defined(CONFIG_SCAN_SELF)
56 #define ADV_TIMEOUT K_SECONDS(CONFIG_SCAN_DELAY)
57 #else /* !CONFIG_SCAN_SELF */
58 #define ADV_TIMEOUT K_FOREVER
59 #endif /* CONFIG_SCAN_SELF */
60
61 #define PA_SYNC_INTERVAL_TO_TIMEOUT_RATIO 5 /* Set the timeout relative to interval */
62 #define PA_SYNC_SKIP 5
63 #define NAME_LEN sizeof(CONFIG_TARGET_BROADCAST_NAME) + 1
64 #define BROADCAST_DATA_ELEMENT_SIZE sizeof(int16_t)
65
66 #if defined(CONFIG_LIBLC3)
67 #define LC3_MAX_SAMPLE_RATE 48000U
68 #define LC3_MAX_FRAME_DURATION_US 10000U
69 #define LC3_MAX_NUM_SAMPLES_MONO ((LC3_MAX_FRAME_DURATION_US * LC3_MAX_SAMPLE_RATE) \
70 / USEC_PER_SEC)
71 #define LC3_MAX_NUM_SAMPLES_STEREO (LC3_MAX_NUM_SAMPLES_MONO * 2)
72
73 #define LC3_ENCODER_STACK_SIZE 4096
74 #define LC3_ENCODER_PRIORITY 5
75 #endif /* defined(CONFIG_LIBLC3) */
76
77 #if defined(CONFIG_USB_DEVICE_AUDIO)
78 #define USB_ENQUEUE_COUNT 10U
79 #define USB_SAMPLE_RATE 48000U
80 #define USB_FRAME_DURATION_US 1000U
81 #define USB_MONO_SAMPLE_SIZE \
82 ((USB_FRAME_DURATION_US * USB_SAMPLE_RATE * BROADCAST_DATA_ELEMENT_SIZE) / USEC_PER_SEC)
83 #define USB_STEREO_SAMPLE_SIZE (USB_MONO_SAMPLE_SIZE * 2)
84 #define USB_RING_BUF_SIZE (5 * LC3_MAX_NUM_SAMPLES_STEREO) /* 5 SDUs*/
85 #endif /* defined(CONFIG_USB_DEVICE_AUDIO) */
86
87 static K_SEM_DEFINE(sem_broadcast_sink_stopped, 0U, 1U);
88 static K_SEM_DEFINE(sem_connected, 0U, 1U);
89 static K_SEM_DEFINE(sem_disconnected, 0U, 1U);
90 static K_SEM_DEFINE(sem_broadcaster_found, 0U, 1U);
91 static K_SEM_DEFINE(sem_pa_synced, 0U, 1U);
92 static K_SEM_DEFINE(sem_base_received, 0U, 1U);
93 static K_SEM_DEFINE(sem_syncable, 0U, 1U);
94 static K_SEM_DEFINE(sem_pa_sync_lost, 0U, 1U);
95 static K_SEM_DEFINE(sem_broadcast_code_received, 0U, 1U);
96 static K_SEM_DEFINE(sem_pa_request, 0U, 1U);
97 static K_SEM_DEFINE(sem_past_request, 0U, 1U);
98 static K_SEM_DEFINE(sem_bis_sync_requested, 0U, 1U);
99 static K_SEM_DEFINE(sem_stream_connected, 0U, CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT);
100 static K_SEM_DEFINE(sem_stream_started, 0U, CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT);
101 static K_SEM_DEFINE(sem_big_synced, 0U, 1U);
102
103 /* Sample assumes that we only have a single Scan Delegator receive state */
104 static const struct bt_bap_scan_delegator_recv_state *req_recv_state;
105 static struct bt_bap_broadcast_sink *broadcast_sink;
106 static struct bt_le_scan_recv_info broadcaster_info;
107 static bt_addr_le_t broadcaster_addr;
108 static struct bt_le_per_adv_sync *pa_sync;
109 static uint32_t broadcaster_broadcast_id;
110 static struct broadcast_sink_stream {
111 struct bt_bap_stream stream;
112 size_t recv_cnt;
113 size_t loss_cnt;
114 size_t error_cnt;
115 size_t valid_cnt;
116 #if defined(CONFIG_LIBLC3)
117 struct net_buf *in_buf;
118 struct k_work_delayable lc3_decode_work;
119
120 /* LC3 config values */
121 enum bt_audio_location chan_allocation;
122 uint16_t lc3_octets_per_frame;
123 uint8_t lc3_frames_blocks_per_sdu;
124
125 /* Internal lock for protecting net_buf from multiple access */
126 struct k_mutex lc3_decoder_mutex;
127 lc3_decoder_t lc3_decoder;
128 lc3_decoder_mem_48k_t lc3_decoder_mem;
129 #endif /* defined(CONFIG_LIBLC3) */
130 } streams[CONFIG_BT_BAP_BROADCAST_SNK_STREAM_COUNT];
131
132 static struct bt_bap_stream *streams_p[ARRAY_SIZE(streams)];
133 static volatile bool big_synced;
134 static volatile bool base_received;
135 static struct bt_conn *broadcast_assistant_conn;
136 static struct bt_le_ext_adv *ext_adv;
137 static volatile uint8_t stream_count;
138
139 static const struct bt_audio_codec_cap codec_cap = BT_AUDIO_CODEC_CAP_LC3(
140 BT_AUDIO_CODEC_CAP_FREQ_16KHZ | BT_AUDIO_CODEC_CAP_FREQ_24KHZ,
141 BT_AUDIO_CODEC_CAP_DURATION_10, BT_AUDIO_CODEC_CAP_CHAN_COUNT_SUPPORT(1), 40u, 60u,
142 CONFIG_MAX_CODEC_FRAMES_PER_SDU,
143 (BT_AUDIO_CONTEXT_TYPE_CONVERSATIONAL | BT_AUDIO_CONTEXT_TYPE_MEDIA));
144
145 /* Create a mask for the maximum BIS we can sync to using the number of streams
146 * we have. Bit 0 is BIS index 1.
147 */
148 static const uint32_t bis_index_mask = BIT_MASK(ARRAY_SIZE(streams));
149 static uint32_t requested_bis_sync;
150 static uint32_t bis_index_bitfield;
151 static uint8_t sink_broadcast_code[BT_ISO_BROADCAST_CODE_SIZE];
152
153 uint64_t total_rx_iso_packet_count; /* This value is exposed to test code */
154
155 static int stop_adv(void);
156
157 #if defined(CONFIG_USB_DEVICE_AUDIO)
158 RING_BUF_DECLARE(usb_ring_buf, USB_RING_BUF_SIZE);
159 NET_BUF_POOL_DEFINE(usb_tx_buf_pool, USB_ENQUEUE_COUNT, USB_STEREO_SAMPLE_SIZE, 0, net_buf_destroy);
160
161 static void add_to_usb_ring_buf(const int16_t audio_buf[LC3_MAX_NUM_SAMPLES_STEREO]);
162 #endif /* defined(CONFIG_USB_DEVICE_AUDIO) */
163
164 #if defined(CONFIG_LIBLC3)
165 static K_SEM_DEFINE(lc3_decoder_sem, 0, 1);
166
167 static void do_lc3_decode(lc3_decoder_t decoder, const void *in_data, uint8_t octets_per_frame,
168 int16_t out_data[LC3_MAX_NUM_SAMPLES_MONO]);
169 static void lc3_decoder_thread(void *arg1, void *arg2, void *arg3);
170 K_THREAD_DEFINE(decoder_tid, LC3_ENCODER_STACK_SIZE, lc3_decoder_thread,
171 NULL, NULL, NULL, LC3_ENCODER_PRIORITY, 0, -1);
172
173 /* Consumer thread of the decoded stream data */
lc3_decoder_thread(void * arg1,void * arg2,void * arg3)174 static void lc3_decoder_thread(void *arg1, void *arg2, void *arg3)
175 {
176 while (true) {
177 #if defined(CONFIG_USB_DEVICE_AUDIO)
178 static int16_t right_frames[CONFIG_MAX_CODEC_FRAMES_PER_SDU]
179 [LC3_MAX_NUM_SAMPLES_MONO];
180 static int16_t left_frames[CONFIG_MAX_CODEC_FRAMES_PER_SDU]
181 [LC3_MAX_NUM_SAMPLES_MONO];
182 size_t right_frames_cnt = 0;
183 size_t left_frames_cnt = 0;
184
185 memset(right_frames, 0, sizeof(right_frames));
186 memset(left_frames, 0, sizeof(left_frames));
187 #else
188 static int16_t lc3_audio_buf[LC3_MAX_NUM_SAMPLES_MONO];
189 #endif /* CONFIG_USB_DEVICE_AUDIO */
190
191 k_sem_take(&lc3_decoder_sem, K_FOREVER);
192
193 for (size_t i = 0; i < ARRAY_SIZE(streams); i++) {
194 struct broadcast_sink_stream *stream = &streams[i];
195 const uint8_t frames_blocks_per_sdu = stream->lc3_frames_blocks_per_sdu;
196 const uint16_t octets_per_frame = stream->lc3_octets_per_frame;
197 uint16_t frames_per_block;
198 struct net_buf *buf;
199
200 k_mutex_lock(&stream->lc3_decoder_mutex, K_FOREVER);
201
202 if (stream->in_buf == NULL) {
203 k_mutex_unlock(&stream->lc3_decoder_mutex);
204
205 continue;
206 }
207
208 buf = net_buf_ref(stream->in_buf);
209 net_buf_unref(stream->in_buf);
210 stream->in_buf = NULL;
211 k_mutex_unlock(&stream->lc3_decoder_mutex);
212
213 frames_per_block = bt_audio_get_chan_count(stream->chan_allocation);
214 if (buf->len !=
215 (frames_per_block * octets_per_frame * frames_blocks_per_sdu)) {
216 printk("Expected %u frame blocks with %u frames of size %u, but "
217 "length is %u\n",
218 frames_blocks_per_sdu, frames_per_block, octets_per_frame,
219 buf->len);
220
221 net_buf_unref(buf);
222
223 continue;
224 }
225
226 #if defined(CONFIG_USB_DEVICE_AUDIO)
227 const bool has_left =
228 (stream->chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
229 const bool has_right =
230 (stream->chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
231 const bool is_mono =
232 stream->chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
233
234 /* Split the SDU into frames*/
235 for (uint8_t i = 0U; i < frames_blocks_per_sdu; i++) {
236 for (uint16_t j = 0U; j < frames_per_block; j++) {
237 const bool is_left = j == 0 && has_left;
238 const bool is_right =
239 has_right && (j == 0 || (j == 1 && has_left));
240 const void *data = net_buf_pull_mem(buf, octets_per_frame);
241 int16_t *out_frame;
242
243 if (is_left) {
244 out_frame = left_frames[left_frames_cnt++];
245 } else if (is_right) {
246 out_frame = right_frames[right_frames_cnt++];
247 } else if (is_mono) {
248 /* Use left as mono*/
249 out_frame = left_frames[left_frames_cnt++];
250 } else {
251 /* unused channel */
252 break;
253 }
254
255 do_lc3_decode(stream->lc3_decoder, data, octets_per_frame,
256 out_frame);
257 }
258 }
259 #else
260 /* Dummy behavior: Decode and discard data */
261 for (uint8_t i = 0U; i < frames_blocks_per_sdu; i++) {
262 for (uint16_t j = 0U; j < frames_per_block; j++) {
263 const void *data = net_buf_pull_mem(buf, octets_per_frame);
264
265 do_lc3_decode(stream->lc3_decoder, data, octets_per_frame,
266 lc3_audio_buf);
267 }
268 }
269 #endif /* CONFIG_USB_DEVICE_AUDIO */
270
271 net_buf_unref(buf);
272 }
273
274 #if defined(CONFIG_USB_DEVICE_AUDIO)
275 const bool is_left_only = right_frames_cnt == 0U;
276 const bool is_right_only = left_frames_cnt == 0U;
277
278 if (!is_left_only && !is_right_only && left_frames_cnt != right_frames_cnt) {
279 printk("Mismatch between number of left (%zu) and right (%zu) frames, "
280 "discard SDU",
281 left_frames_cnt, right_frames_cnt);
282 continue;
283 }
284
285 /* Send frames to USB - If we only have a single channel we mix it to stereo */
286 for (size_t i = 0U; i < MAX(left_frames_cnt, right_frames_cnt); i++) {
287 const bool is_single_channel = is_left_only || is_right_only;
288 static int16_t stereo_frame[LC3_MAX_NUM_SAMPLES_STEREO];
289 int16_t *right_frame = right_frames[i];
290 int16_t *left_frame = left_frames[i];
291
292 /* Not enough space to store data */
293 if (ring_buf_space_get(&usb_ring_buf) < sizeof(stereo_frame)) {
294 break;
295 }
296
297 memset(stereo_frame, 0, sizeof(stereo_frame));
298
299 /* Generate the stereo frame
300 *
301 * If we only have single channel then that is always stored in the
302 * left_frame, and we mix that to stereo
303 */
304 for (int j = 0; j < LC3_MAX_NUM_SAMPLES_MONO; j++) {
305 if (is_single_channel) {
306 /* Mix to stereo */
307 if (is_left_only) {
308 stereo_frame[j * 2] = left_frame[j];
309 stereo_frame[j * 2 + 1] = left_frame[j];
310 } else if (is_right_only) {
311 stereo_frame[j * 2] = right_frame[j];
312 stereo_frame[j * 2 + 1] = right_frame[j];
313 }
314 } else {
315 stereo_frame[j * 2] = left_frame[j];
316 stereo_frame[j * 2 + 1] = right_frame[j];
317 }
318 }
319
320 add_to_usb_ring_buf(stereo_frame);
321 }
322 #endif /* CONFIG_USB_DEVICE_AUDIO */
323 }
324 }
325
326 /** Decode LC3 data on a stream and returns true if successful */
do_lc3_decode(lc3_decoder_t decoder,const void * in_data,uint8_t octets_per_frame,int16_t out_data[LC3_MAX_NUM_SAMPLES_MONO])327 static void do_lc3_decode(lc3_decoder_t decoder, const void *in_data, uint8_t octets_per_frame,
328 int16_t out_data[LC3_MAX_NUM_SAMPLES_MONO])
329 {
330 int err;
331
332 err = lc3_decode(decoder, in_data, octets_per_frame, LC3_PCM_FORMAT_S16, out_data, 1);
333 if (err == 1) {
334 printk(" decoder performed PLC\n");
335 } else if (err < 0) {
336 printk(" decoder failed - wrong parameters? (err = %d)\n", err);
337 }
338 }
339
lc3_enable(struct broadcast_sink_stream * sink_stream)340 static int lc3_enable(struct broadcast_sink_stream *sink_stream)
341 {
342 size_t chan_alloc_bit_cnt;
343 size_t sdu_size_required;
344 int frame_duration_us;
345 int freq_hz;
346 int ret;
347
348 printk("Enable: stream with codec %p\n", sink_stream->stream.codec_cfg);
349
350 ret = bt_audio_codec_cfg_get_freq(sink_stream->stream.codec_cfg);
351 if (ret > 0) {
352 freq_hz = bt_audio_codec_cfg_freq_to_freq_hz(ret);
353 } else {
354 printk("Error: Codec frequency not set, cannot start codec.");
355 return -1;
356 }
357
358 ret = bt_audio_codec_cfg_get_frame_dur(sink_stream->stream.codec_cfg);
359 if (ret > 0) {
360 frame_duration_us = bt_audio_codec_cfg_frame_dur_to_frame_dur_us(ret);
361 } else {
362 printk("Error: Frame duration not set, cannot start codec.");
363 return ret;
364 }
365
366 ret = bt_audio_codec_cfg_get_chan_allocation(sink_stream->stream.codec_cfg,
367 &sink_stream->chan_allocation, true);
368 if (ret != 0) {
369 printk("Error: Channel allocation not set, invalid configuration for LC3");
370 return ret;
371 }
372
373 ret = bt_audio_codec_cfg_get_octets_per_frame(sink_stream->stream.codec_cfg);
374 if (ret > 0) {
375 sink_stream->lc3_octets_per_frame = (uint16_t)ret;
376 } else {
377 printk("Error: Octets per frame not set, invalid configuration for LC3");
378 return ret;
379 }
380
381 ret = bt_audio_codec_cfg_get_frame_blocks_per_sdu(sink_stream->stream.codec_cfg, true);
382 if (ret > 0) {
383 sink_stream->lc3_frames_blocks_per_sdu = (uint8_t)ret;
384 } else {
385 printk("Error: Frame blocks per SDU not set, invalid configuration for LC3");
386 return ret;
387 }
388
389 /* An SDU can consist of X frame blocks, each with Y frames (one per channel) of size Z in
390 * them. The minimum SDU size required for this is X * Y * Z.
391 */
392 chan_alloc_bit_cnt = bt_audio_get_chan_count(sink_stream->chan_allocation);
393 sdu_size_required = chan_alloc_bit_cnt * sink_stream->lc3_octets_per_frame *
394 sink_stream->lc3_frames_blocks_per_sdu;
395 if (sdu_size_required > sink_stream->stream.qos->sdu) {
396 printk("With %zu channels and %u octets per frame and %u frames per block, SDUs "
397 "shall be at minimum %zu, but the stream has been configured for %u",
398 chan_alloc_bit_cnt, sink_stream->lc3_octets_per_frame,
399 sink_stream->lc3_frames_blocks_per_sdu, sdu_size_required,
400 sink_stream->stream.qos->sdu);
401
402 return -EINVAL;
403 }
404
405 printk("Enabling LC3 decoder with frame duration %uus, frequency %uHz and with channel "
406 "allocation 0x%08X, %u octets per frame and %u frame blocks per SDU\n",
407 frame_duration_us, freq_hz, sink_stream->chan_allocation,
408 sink_stream->lc3_octets_per_frame, sink_stream->lc3_frames_blocks_per_sdu);
409
410 #if defined(CONFIG_USB_DEVICE_AUDIO)
411 sink_stream->lc3_decoder = lc3_setup_decoder(frame_duration_us, freq_hz, USB_SAMPLE_RATE,
412 &sink_stream->lc3_decoder_mem);
413 #else
414 sink_stream->lc3_decoder = lc3_setup_decoder(frame_duration_us, freq_hz, 0,
415 &sink_stream->lc3_decoder_mem);
416 #endif /* defined(CONFIG_USB_DEVICE_AUDIO) */
417
418 if (sink_stream->lc3_decoder == NULL) {
419 printk("ERROR: Failed to setup LC3 decoder - wrong parameters?\n");
420 return -1;
421 }
422
423 k_thread_start(decoder_tid);
424
425 return 0;
426 }
427 #endif /* defined(CONFIG_LIBLC3) */
428
429 #if defined(CONFIG_USB_DEVICE_AUDIO)
430 /* Move the LC3 data to the USB ring buffer */
add_to_usb_ring_buf(const int16_t audio_buf[LC3_MAX_NUM_SAMPLES_STEREO])431 static void add_to_usb_ring_buf(const int16_t audio_buf[LC3_MAX_NUM_SAMPLES_STEREO])
432 {
433 uint32_t size;
434
435 size = ring_buf_put(&usb_ring_buf, (uint8_t *)audio_buf,
436 LC3_MAX_NUM_SAMPLES_STEREO * sizeof(int16_t));
437 if (size != LC3_MAX_NUM_SAMPLES_STEREO) {
438 static int rb_put_failures;
439
440 rb_put_failures++;
441 if (rb_put_failures == LOG_INTERVAL) {
442 printk("%s: Failure to add to usb_ring_buf %d, %u\n", __func__,
443 rb_put_failures, size);
444 }
445 }
446 }
447
448 /* USB consumer callback, called every 1ms, consumes data from ring-buffer */
usb_data_request_cb(const struct device * dev)449 static void usb_data_request_cb(const struct device *dev)
450 {
451 uint8_t usb_audio_data[USB_STEREO_SAMPLE_SIZE] = {0};
452 static struct net_buf *pcm_buf;
453 static size_t cnt;
454 int err;
455
456 ring_buf_get(&usb_ring_buf, (uint8_t *)usb_audio_data, sizeof(usb_audio_data));
457 /* Ignore ring_buf_get() return value, if size is 0 we send empty PCM frames to
458 * not starve USB audio interface, if size is lower than USB_STEREO_SAMPLE_SIZE
459 * we send frames padded with 0's as usb_audio_data is 0-initialized
460 */
461
462 pcm_buf = net_buf_alloc(&usb_tx_buf_pool, K_NO_WAIT);
463 if (pcm_buf == NULL) {
464 printk("Could not allocate pcm_buf\n");
465 return;
466 }
467
468 net_buf_add_mem(pcm_buf, usb_audio_data, sizeof(usb_audio_data));
469
470 if (cnt % LOG_INTERVAL == 0) {
471 printk("Sending USB audio (count = %zu)\n", cnt);
472 }
473
474 err = usb_audio_send(dev, pcm_buf, USB_STEREO_SAMPLE_SIZE);
475 if (err) {
476 printk("Failed to send USB audio: %d\n", err);
477 net_buf_unref(pcm_buf);
478 }
479
480 cnt++;
481 }
482
usb_data_written_cb(const struct device * dev,struct net_buf * buf,size_t size)483 static void usb_data_written_cb(const struct device *dev, struct net_buf *buf, size_t size)
484 {
485 /* Unreference the buffer now that the USB is done with it */
486 net_buf_unref(buf);
487 }
488 #endif /* defined(CONFIG_USB_DEVICE_AUDIO) */
489
stream_connected_cb(struct bt_bap_stream * stream)490 static void stream_connected_cb(struct bt_bap_stream *stream)
491 {
492 printk("Stream %p connected\n", stream);
493
494 k_sem_give(&sem_stream_connected);
495 }
496
stream_disconnected_cb(struct bt_bap_stream * stream,uint8_t reason)497 static void stream_disconnected_cb(struct bt_bap_stream *stream, uint8_t reason)
498 {
499 int err;
500
501 printk("Stream %p disconnected with reason 0x%02X\n", stream, reason);
502
503 err = k_sem_take(&sem_stream_connected, K_NO_WAIT);
504 if (err != 0) {
505 printk("Failed to take sem_stream_connected: %d\n", err);
506 }
507 }
508
stream_started_cb(struct bt_bap_stream * stream)509 static void stream_started_cb(struct bt_bap_stream *stream)
510 {
511 struct broadcast_sink_stream *sink_stream =
512 CONTAINER_OF(stream, struct broadcast_sink_stream, stream);
513
514 printk("Stream %p started\n", stream);
515
516 total_rx_iso_packet_count = 0U;
517 sink_stream->recv_cnt = 0U;
518 sink_stream->loss_cnt = 0U;
519 sink_stream->valid_cnt = 0U;
520 sink_stream->error_cnt = 0U;
521
522 #if defined(CONFIG_LIBLC3)
523 int err;
524
525 if (stream->codec_cfg != 0 && stream->codec_cfg->id != BT_HCI_CODING_FORMAT_LC3) {
526 /* No subgroups with LC3 was found */
527 printk("Did not parse an LC3 codec\n");
528 return;
529 }
530
531 err = lc3_enable(sink_stream);
532 if (err < 0) {
533 printk("Error: cannot enable LC3 codec: %d", err);
534 return;
535 }
536 #endif /* CONFIG_LIBLC3 */
537
538 k_sem_give(&sem_stream_started);
539 }
540
stream_stopped_cb(struct bt_bap_stream * stream,uint8_t reason)541 static void stream_stopped_cb(struct bt_bap_stream *stream, uint8_t reason)
542 {
543 int err;
544
545 printk("Stream %p stopped with reason 0x%02X\n", stream, reason);
546
547 err = k_sem_take(&sem_stream_started, K_NO_WAIT);
548 if (err != 0) {
549 printk("Failed to take sem_stream_started: %d\n", err);
550 }
551 }
552
stream_recv_cb(struct bt_bap_stream * stream,const struct bt_iso_recv_info * info,struct net_buf * buf)553 static void stream_recv_cb(struct bt_bap_stream *stream, const struct bt_iso_recv_info *info,
554 struct net_buf *buf)
555 {
556 struct broadcast_sink_stream *sink_stream =
557 CONTAINER_OF(stream, struct broadcast_sink_stream, stream);
558
559 if (info->flags & BT_ISO_FLAGS_ERROR) {
560 sink_stream->error_cnt++;
561 }
562
563 if (info->flags & BT_ISO_FLAGS_LOST) {
564 sink_stream->loss_cnt++;
565 }
566
567 if (info->flags & BT_ISO_FLAGS_VALID) {
568 sink_stream->valid_cnt++;
569 #if defined(CONFIG_LIBLC3)
570 k_mutex_lock(&sink_stream->lc3_decoder_mutex, K_FOREVER);
571 if (sink_stream->in_buf != NULL) {
572 net_buf_unref(sink_stream->in_buf);
573 sink_stream->in_buf = NULL;
574 }
575
576 sink_stream->in_buf = net_buf_ref(buf);
577 k_mutex_unlock(&sink_stream->lc3_decoder_mutex);
578 k_sem_give(&lc3_decoder_sem);
579 #endif /* defined(CONFIG_LIBLC3) */
580 }
581
582 total_rx_iso_packet_count++;
583 sink_stream->recv_cnt++;
584 if ((sink_stream->recv_cnt % LOG_INTERVAL) == 0U) {
585 printk("Stream %p: received %u total ISO packets: Valid %u | Error %u | Loss %u\n",
586 &sink_stream->stream, sink_stream->recv_cnt, sink_stream->valid_cnt,
587 sink_stream->error_cnt, sink_stream->loss_cnt);
588 }
589 }
590
591 static struct bt_bap_stream_ops stream_ops = {
592 .connected = stream_connected_cb,
593 .disconnected = stream_disconnected_cb,
594 .started = stream_started_cb,
595 .stopped = stream_stopped_cb,
596 .recv = stream_recv_cb,
597 };
598
599 #if defined(CONFIG_TARGET_BROADCAST_CHANNEL)
600 struct bis_channel_allocation_data {
601 struct {
602 bool chan_allocation_available;
603 uint8_t index;
604 enum bt_audio_location chan_allocation;
605 } bis[BT_ISO_BIS_INDEX_MAX];
606
607 uint8_t cnt;
608 };
609
610 /**
611 * This is called for each BIS in a subgroup
612 *
613 * Gets BIS channel allocation (if exists).
614 * Always returns `true` to continue to next BIS
615 */
bis_get_channel_allocation_cb(const struct bt_bap_base_subgroup_bis * bis,void * user_data)616 static bool bis_get_channel_allocation_cb(const struct bt_bap_base_subgroup_bis *bis,
617 void *user_data)
618 {
619 struct bis_channel_allocation_data *data = user_data;
620 struct bt_audio_codec_cfg codec_cfg;
621 int err, idx;
622
623 idx = data->cnt++;
624 data->bis[idx].index = bis->index;
625 data->bis[idx].chan_allocation_available = false;
626
627 err = bt_bap_base_subgroup_bis_codec_to_codec_cfg(bis, &codec_cfg);
628 if (err != 0) {
629 printk("Could not get codec configuration for BIS: %d\n", err);
630
631 return true; /* continue to next BIS */
632 }
633
634 err = bt_audio_codec_cfg_get_chan_allocation(&codec_cfg, &data->bis[idx].chan_allocation,
635 false);
636 if (err != 0) {
637 printk("Could not find channel allocation for BIS: %d\n", err);
638
639 return true; /* continue to next BIS */
640 }
641
642 /* Channel allocation data available for this bis */
643 data->bis[idx].chan_allocation_available = true;
644
645 return true; /* continue to next BIS */
646 }
647
648 /**
649 * Called for each subgroup in the BASE. Will populate the 32-bit bitfield of BIS indexes if the
650 * subgroup contains it.
651 *
652 * The channel allocation may
653 * - Not exist at all, implicitly meaning BT_AUDIO_LOCATION_MONO_AUDIO
654 * - Exist only in the subgroup codec configuration
655 * - Exist only in the BIS codec configuration
656 * - Exist in both the subgroup and BIS codec configuration, in which case, the BIS codec
657 * configuration overwrites the subgroup values
658 *
659 * This function returns `true` if the subgroup does not support the channels in
660 * CONFIG_TARGET_BROADCAST_CHANNEL which makes it iterate over the next subgroup, and
661 * returns `false` if this subgroup satisfies our CONFIG_TARGET_BROADCAST_CHANNEL.
662 */
subgroup_get_valid_bis_indexes_cb(const struct bt_bap_base_subgroup * subgroup,void * user_data)663 static bool subgroup_get_valid_bis_indexes_cb(const struct bt_bap_base_subgroup *subgroup,
664 void *user_data)
665 {
666 enum bt_audio_location subgroup_chan_allocation;
667 enum bt_audio_location chan_allocation = BT_AUDIO_LOCATION_MONO_AUDIO;
668 bool subgroup_chan_allocation_available = false;
669 struct bt_audio_codec_cfg codec_cfg;
670 struct bis_channel_allocation_data data = {
671 .cnt = 0,
672 };
673 uint32_t bis_indexes = 0;
674 int err;
675
676 err = bt_bap_base_subgroup_codec_to_codec_cfg(subgroup, &codec_cfg);
677 if (err != 0) {
678 printk("Could not get codec configuration: %d\n", err);
679
680 return true; /* continue to next subgroup */
681 }
682
683 if (codec_cfg.id != BT_HCI_CODING_FORMAT_LC3) {
684 /* Only LC3 codec supported */
685 return false; /* abort */
686 }
687
688 /* Get channel allocation at subgroup level */
689 err = bt_audio_codec_cfg_get_chan_allocation(&codec_cfg, &subgroup_chan_allocation, true);
690 if (err == 0) {
691 printk("Channel allocation (subgroup level) 0x%x\n", subgroup_chan_allocation);
692 subgroup_chan_allocation_available = true;
693 } else {
694 /* subgroup error */
695 return false; /* abort */
696 }
697
698 /* Get channel allocation at BIS level */
699 err = bt_bap_base_subgroup_foreach_bis(subgroup, bis_get_channel_allocation_cb, &data);
700 if (err != 0) {
701 printk("Get channel allocation error %d\n", err);
702
703 return true; /* continue to next subgroup */
704 }
705
706 /* If no BIS channel allocation available use subgroup channel allocation instead if
707 * exists (otherwise mono assumed)
708 */
709 for (uint8_t i = 0U; i < data.cnt; i++) {
710 if (!data.bis[i].chan_allocation_available) {
711 data.bis[i].chan_allocation = subgroup_chan_allocation_available
712 ? subgroup_chan_allocation
713 : BT_AUDIO_LOCATION_MONO_AUDIO;
714 }
715 }
716
717 /* Get the BIS indexes */
718 for (uint8_t i = 0U; i < data.cnt; i++) {
719 if ((data.bis[i].chan_allocation == CONFIG_TARGET_BROADCAST_CHANNEL) ||
720 ((data.bis[i].chan_allocation & CONFIG_TARGET_BROADCAST_CHANNEL) ==
721 CONFIG_TARGET_BROADCAST_CHANNEL)) {
722 /* Exact match */
723 bis_indexes = BT_ISO_BIS_INDEX_BIT(data.bis[i].index);
724
725 printk("Channel allocation match. BIS index bitfield 0x%x\n", bis_indexes);
726 *(uint32_t *)user_data = bis_indexes;
727
728 return false; /* bis index found */
729 } else if ((data.bis[i].chan_allocation & CONFIG_TARGET_BROADCAST_CHANNEL) != 0) {
730 /* Partial match */
731 chan_allocation |= data.bis[i].chan_allocation;
732 bis_indexes |= BT_ISO_BIS_INDEX_BIT(data.bis[i].index);
733
734 if ((chan_allocation & CONFIG_TARGET_BROADCAST_CHANNEL) ==
735 CONFIG_TARGET_BROADCAST_CHANNEL) {
736 printk("Channel allocation match. BIS index bitfield 0x%x\n",
737 bis_indexes);
738 *(uint32_t *)user_data = bis_indexes;
739
740 return false; /* bis indexes found */
741 }
742 }
743 }
744 return true; /* continue to next subgroup */
745 }
746 #endif /* CONFIG_TARGET_BROADCAST_CHANNEL */
747
base_recv_cb(struct bt_bap_broadcast_sink * sink,const struct bt_bap_base * base,size_t base_size)748 static void base_recv_cb(struct bt_bap_broadcast_sink *sink, const struct bt_bap_base *base,
749 size_t base_size)
750 {
751 uint32_t base_bis_index_bitfield = 0U;
752 int err;
753
754 if (base_received) {
755 return;
756 }
757
758 printk("Received BASE with %d subgroups from broadcast sink %p\n",
759 bt_bap_base_get_subgroup_count(base), sink);
760
761 #if defined(CONFIG_TARGET_BROADCAST_CHANNEL)
762 /**
763 * Get a 32-bit bitfield of BIS indexes that cover the channel allocation values in
764 * CONFIG_TARGET_BROADCAST_CHANNEL.
765 */
766 printk("Target channel location: 0x%x\n", CONFIG_TARGET_BROADCAST_CHANNEL);
767 err = bt_bap_base_foreach_subgroup(base, subgroup_get_valid_bis_indexes_cb,
768 &base_bis_index_bitfield);
769 if ((err != 0 && err != -ECANCELED) ||
770 (err == -ECANCELED && base_bis_index_bitfield == 0)) {
771 printk("Failed to get valid BIS indexes: %d\n", err);
772
773 return;
774 }
775 #else
776 err = bt_bap_base_get_bis_indexes(base, &base_bis_index_bitfield);
777 if (err != 0) {
778 printk("Failed to get BIS indexes: %d\n", err);
779
780 return;
781 }
782
783 #endif /* CONFIG_TARGET_BROADCAST_CHANNEL */
784
785 bis_index_bitfield = base_bis_index_bitfield & bis_index_mask;
786
787 printk("bis_index_bitfield = 0x%08x\n", bis_index_bitfield);
788
789 if (broadcast_assistant_conn == NULL) {
790 /* No broadcast assistant requesting anything */
791 requested_bis_sync = BT_BAP_BIS_SYNC_NO_PREF;
792 k_sem_give(&sem_bis_sync_requested);
793 }
794
795 base_received = true;
796 k_sem_give(&sem_base_received);
797 }
798
syncable_cb(struct bt_bap_broadcast_sink * sink,const struct bt_iso_biginfo * biginfo)799 static void syncable_cb(struct bt_bap_broadcast_sink *sink, const struct bt_iso_biginfo *biginfo)
800 {
801 printk("Broadcast sink (%p) is syncable, BIG %s\n", (void *)sink,
802 biginfo->encryption ? "encrypted" : "not encrypted");
803
804 k_sem_give(&sem_syncable);
805
806 if (!biginfo->encryption) {
807 k_sem_give(&sem_broadcast_code_received);
808 }
809 }
810
broadcast_sink_started_cb(struct bt_bap_broadcast_sink * sink)811 static void broadcast_sink_started_cb(struct bt_bap_broadcast_sink *sink)
812 {
813 printk("Broadcast sink %p started\n", sink);
814
815 big_synced = true;
816 k_sem_give(&sem_big_synced);
817 }
818
broadcast_sink_stopped_cb(struct bt_bap_broadcast_sink * sink,uint8_t reason)819 static void broadcast_sink_stopped_cb(struct bt_bap_broadcast_sink *sink, uint8_t reason)
820 {
821 printk("Broadcast sink %p stopped with reason 0x%02X\n", sink, reason);
822
823 big_synced = false;
824 k_sem_give(&sem_broadcast_sink_stopped);
825 }
826
827 static struct bt_bap_broadcast_sink_cb broadcast_sink_cbs = {
828 .base_recv = base_recv_cb,
829 .syncable = syncable_cb,
830 .started = broadcast_sink_started_cb,
831 .stopped = broadcast_sink_stopped_cb,
832 };
833
pa_timer_handler(struct k_work * work)834 static void pa_timer_handler(struct k_work *work)
835 {
836 if (req_recv_state != NULL) {
837 enum bt_bap_pa_state pa_state;
838
839 if (req_recv_state->pa_sync_state == BT_BAP_PA_STATE_INFO_REQ) {
840 pa_state = BT_BAP_PA_STATE_NO_PAST;
841 } else {
842 pa_state = BT_BAP_PA_STATE_FAILED;
843 }
844
845 bt_bap_scan_delegator_set_pa_state(req_recv_state->src_id,
846 pa_state);
847 }
848
849 printk("PA timeout\n");
850 }
851
852 static K_WORK_DELAYABLE_DEFINE(pa_timer, pa_timer_handler);
853
interval_to_sync_timeout(uint16_t pa_interval)854 static uint16_t interval_to_sync_timeout(uint16_t pa_interval)
855 {
856 uint16_t pa_timeout;
857
858 if (pa_interval == BT_BAP_PA_INTERVAL_UNKNOWN) {
859 /* Use maximum value to maximize chance of success */
860 pa_timeout = BT_GAP_PER_ADV_MAX_TIMEOUT;
861 } else {
862 uint32_t interval_us;
863 uint32_t timeout;
864
865 /* Add retries and convert to unit in 10's of ms */
866 interval_us = BT_GAP_PER_ADV_INTERVAL_TO_US(pa_interval);
867 timeout = BT_GAP_US_TO_PER_ADV_SYNC_TIMEOUT(interval_us) *
868 PA_SYNC_INTERVAL_TO_TIMEOUT_RATIO;
869
870 /* Enforce restraints */
871 pa_timeout = CLAMP(timeout, BT_GAP_PER_ADV_MIN_TIMEOUT, BT_GAP_PER_ADV_MAX_TIMEOUT);
872 }
873
874 return pa_timeout;
875 }
876
pa_sync_past(struct bt_conn * conn,uint16_t pa_interval)877 static int pa_sync_past(struct bt_conn *conn, uint16_t pa_interval)
878 {
879 struct bt_le_per_adv_sync_transfer_param param = { 0 };
880 int err;
881
882 param.skip = PA_SYNC_SKIP;
883 param.timeout = interval_to_sync_timeout(pa_interval);
884
885 err = bt_le_per_adv_sync_transfer_subscribe(conn, ¶m);
886 if (err != 0) {
887 printk("Could not do PAST subscribe: %d\n", err);
888 } else {
889 printk("Syncing with PAST\n");
890 (void)k_work_reschedule(&pa_timer, K_MSEC(param.timeout * 10));
891 }
892
893 return err;
894 }
895
recv_state_updated_cb(struct bt_conn * conn,const struct bt_bap_scan_delegator_recv_state * recv_state)896 static void recv_state_updated_cb(struct bt_conn *conn,
897 const struct bt_bap_scan_delegator_recv_state *recv_state)
898 {
899 printk("Receive state updated, pa sync state: %u, encrypt_state %u\n",
900 recv_state->pa_sync_state, recv_state->encrypt_state);
901
902 for (uint8_t i = 0; i < recv_state->num_subgroups; i++) {
903 printk("subgroup %d bis_sync: 0x%08x\n", i, recv_state->subgroups[i].bis_sync);
904 }
905
906 req_recv_state = recv_state;
907 }
908
pa_sync_req_cb(struct bt_conn * conn,const struct bt_bap_scan_delegator_recv_state * recv_state,bool past_avail,uint16_t pa_interval)909 static int pa_sync_req_cb(struct bt_conn *conn,
910 const struct bt_bap_scan_delegator_recv_state *recv_state,
911 bool past_avail, uint16_t pa_interval)
912 {
913
914 printk("Received request to sync to PA (PAST %savailble): %u\n", past_avail ? "" : "not ",
915 recv_state->pa_sync_state);
916
917 req_recv_state = recv_state;
918
919 if (recv_state->pa_sync_state == BT_BAP_PA_STATE_SYNCED ||
920 recv_state->pa_sync_state == BT_BAP_PA_STATE_INFO_REQ) {
921 /* Already syncing */
922 /* TODO: Terminate existing sync and then sync to new?*/
923 return -1;
924 }
925
926 if (IS_ENABLED(CONFIG_BT_PER_ADV_SYNC_TRANSFER_RECEIVER) && past_avail) {
927 int err;
928
929 err = pa_sync_past(conn, pa_interval);
930 if (err != 0) {
931 printk("Failed to subscribe to PAST: %d\n", err);
932
933 return err;
934 }
935
936 k_sem_give(&sem_past_request);
937
938 err = bt_bap_scan_delegator_set_pa_state(recv_state->src_id,
939 BT_BAP_PA_STATE_INFO_REQ);
940 if (err != 0) {
941 printk("Failed to set PA state to BT_BAP_PA_STATE_INFO_REQ: %d\n", err);
942
943 return err;
944 }
945 }
946
947 k_sem_give(&sem_pa_request);
948
949 return 0;
950 }
951
pa_sync_term_req_cb(struct bt_conn * conn,const struct bt_bap_scan_delegator_recv_state * recv_state)952 static int pa_sync_term_req_cb(struct bt_conn *conn,
953 const struct bt_bap_scan_delegator_recv_state *recv_state)
954 {
955 int err;
956
957 printk("PA sync termination req, pa sync state: %u\n", recv_state->pa_sync_state);
958
959 for (uint8_t i = 0; i < recv_state->num_subgroups; i++) {
960 printk("subgroup %d bis_sync: 0x%08x\n", i, recv_state->subgroups[i].bis_sync);
961 }
962
963 req_recv_state = recv_state;
964
965 printk("Delete periodic advertising sync\n");
966 err = bt_le_per_adv_sync_delete(pa_sync);
967 if (err != 0) {
968 printk("Could not delete per adv sync: %d\n", err);
969
970 return err;
971 }
972
973 return 0;
974 }
975
broadcast_code_cb(struct bt_conn * conn,const struct bt_bap_scan_delegator_recv_state * recv_state,const uint8_t broadcast_code[BT_ISO_BROADCAST_CODE_SIZE])976 static void broadcast_code_cb(struct bt_conn *conn,
977 const struct bt_bap_scan_delegator_recv_state *recv_state,
978 const uint8_t broadcast_code[BT_ISO_BROADCAST_CODE_SIZE])
979 {
980 printk("Broadcast code received for %p\n", recv_state);
981
982 req_recv_state = recv_state;
983
984 (void)memcpy(sink_broadcast_code, broadcast_code, BT_ISO_BROADCAST_CODE_SIZE);
985
986 k_sem_give(&sem_broadcast_code_received);
987 }
988
bis_sync_req_cb(struct bt_conn * conn,const struct bt_bap_scan_delegator_recv_state * recv_state,const uint32_t bis_sync_req[CONFIG_BT_BAP_BASS_MAX_SUBGROUPS])989 static int bis_sync_req_cb(struct bt_conn *conn,
990 const struct bt_bap_scan_delegator_recv_state *recv_state,
991 const uint32_t bis_sync_req[CONFIG_BT_BAP_BASS_MAX_SUBGROUPS])
992 {
993 /* Bit field indicating from which subgroup(s) BIS sync is requested */
994 uint32_t requested_subgroup_sync = 0; /* currently only used for printout */
995
996 requested_bis_sync = 0;
997
998 for (uint8_t subgroup = 0; subgroup < recv_state->num_subgroups; subgroup++) {
999 if (bis_sync_req[subgroup] != 0) {
1000 if (requested_bis_sync == 0) {
1001 requested_bis_sync = bis_sync_req[subgroup];
1002 } else {
1003 if (requested_bis_sync != BT_BAP_BIS_SYNC_NO_PREF &&
1004 bis_sync_req[subgroup] != BT_BAP_BIS_SYNC_NO_PREF) {
1005 /* Spec a little bit unclear. Here we choose to say that
1006 * BIS sync request from more than 1 subgroup is not
1007 * possible unless sync value is 0 or
1008 * BT_BAP_BIS_SYNC_NO_PREF
1009 */
1010 printk("Unsupported BIS sync request from more than 1 "
1011 "subgroup\n");
1012 return -EINVAL;
1013 }
1014 }
1015 requested_subgroup_sync |= BIT(subgroup);
1016 }
1017 }
1018
1019 printk("BIS sync req for %p: BIS indexes 0x%08x (subgroup indexes 0x%08x), "
1020 "broadcast id: 0x%06x, (%s)\n",
1021 recv_state, requested_bis_sync, requested_subgroup_sync, recv_state->broadcast_id,
1022 big_synced ? "BIG synced" : "BIG not synced");
1023
1024 if (big_synced && requested_bis_sync == 0) {
1025 int err;
1026
1027 /* The stream stopped callback will be called as part of this,
1028 * and we do not need to wait for any events from the
1029 * controller. Thus, when this returns, the `big_synced`
1030 * is back to false.
1031 */
1032 err = bt_bap_broadcast_sink_stop(broadcast_sink);
1033 if (err != 0) {
1034 printk("Failed to stop Broadcast Sink: %d\n", err);
1035
1036 return err;
1037 }
1038 }
1039
1040 broadcaster_broadcast_id = recv_state->broadcast_id;
1041 if (requested_bis_sync != 0) {
1042 k_sem_give(&sem_bis_sync_requested);
1043 }
1044
1045 return 0;
1046 }
1047
1048 static struct bt_bap_scan_delegator_cb scan_delegator_cbs = {
1049 .recv_state_updated = recv_state_updated_cb,
1050 .pa_sync_req = pa_sync_req_cb,
1051 .pa_sync_term_req = pa_sync_term_req_cb,
1052 .broadcast_code = broadcast_code_cb,
1053 .bis_sync_req = bis_sync_req_cb,
1054 };
1055
connected(struct bt_conn * conn,uint8_t err)1056 static void connected(struct bt_conn *conn, uint8_t err)
1057 {
1058 char addr[BT_ADDR_LE_STR_LEN];
1059
1060 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
1061
1062 if (err != 0U) {
1063 printk("Failed to connect to %s %u %s\n", addr, err, bt_hci_err_to_str(err));
1064
1065 broadcast_assistant_conn = NULL;
1066 return;
1067 }
1068
1069 printk("Connected: %s\n", addr);
1070 broadcast_assistant_conn = bt_conn_ref(conn);
1071
1072 k_sem_give(&sem_connected);
1073 }
1074
disconnected(struct bt_conn * conn,uint8_t reason)1075 static void disconnected(struct bt_conn *conn, uint8_t reason)
1076 {
1077 char addr[BT_ADDR_LE_STR_LEN];
1078
1079 if (conn != broadcast_assistant_conn) {
1080 return;
1081 }
1082
1083 bt_addr_le_to_str(bt_conn_get_dst(conn), addr, sizeof(addr));
1084
1085 printk("Disconnected: %s, reason 0x%02x %s\n", addr, reason, bt_hci_err_to_str(reason));
1086
1087 bt_conn_unref(broadcast_assistant_conn);
1088 broadcast_assistant_conn = NULL;
1089
1090 k_sem_give(&sem_disconnected);
1091 }
1092
1093 BT_CONN_CB_DEFINE(conn_callbacks) = {
1094 .connected = connected,
1095 .disconnected = disconnected,
1096 };
1097
1098 static struct bt_pacs_cap cap = {
1099 .codec_cap = &codec_cap,
1100 };
1101
scan_check_and_sync_broadcast(struct bt_data * data,void * user_data)1102 static bool scan_check_and_sync_broadcast(struct bt_data *data, void *user_data)
1103 {
1104 const struct bt_le_scan_recv_info *info = user_data;
1105 char le_addr[BT_ADDR_LE_STR_LEN];
1106 struct bt_uuid_16 adv_uuid;
1107 uint32_t broadcast_id;
1108
1109 if (data->type != BT_DATA_SVC_DATA16) {
1110 return true;
1111 }
1112
1113 if (data->data_len < BT_UUID_SIZE_16 + BT_AUDIO_BROADCAST_ID_SIZE) {
1114 return true;
1115 }
1116
1117 if (!bt_uuid_create(&adv_uuid.uuid, data->data, BT_UUID_SIZE_16)) {
1118 return true;
1119 }
1120
1121 if (bt_uuid_cmp(&adv_uuid.uuid, BT_UUID_BROADCAST_AUDIO)) {
1122 return true;
1123 }
1124
1125 broadcast_id = sys_get_le24(data->data + BT_UUID_SIZE_16);
1126
1127 bt_addr_le_to_str(info->addr, le_addr, sizeof(le_addr));
1128
1129 printk("Found broadcaster with ID 0x%06X and addr %s and sid 0x%02X\n", broadcast_id,
1130 le_addr, info->sid);
1131
1132 if (broadcast_assistant_conn == NULL /* Not requested by Broadcast Assistant */ ||
1133 (req_recv_state != NULL && bt_addr_le_eq(info->addr, &req_recv_state->addr) &&
1134 info->sid == req_recv_state->adv_sid &&
1135 broadcast_id == req_recv_state->broadcast_id)) {
1136
1137 /* Store info for PA sync parameters */
1138 memcpy(&broadcaster_info, info, sizeof(broadcaster_info));
1139 bt_addr_le_copy(&broadcaster_addr, info->addr);
1140 broadcaster_broadcast_id = broadcast_id;
1141 printk("broadcaster_broadcast_id = 0x%06X\n", broadcaster_broadcast_id);
1142 k_sem_give(&sem_broadcaster_found);
1143 }
1144
1145 /* Stop parsing */
1146 return false;
1147 }
1148
is_substring(const char * substr,const char * str)1149 static bool is_substring(const char *substr, const char *str)
1150 {
1151 const size_t str_len = strlen(str);
1152 const size_t sub_str_len = strlen(substr);
1153
1154 if (sub_str_len > str_len) {
1155 return false;
1156 }
1157
1158 for (size_t pos = 0; pos < str_len; pos++) {
1159 if (pos + sub_str_len > str_len) {
1160 return false;
1161 }
1162
1163 if (strncasecmp(substr, &str[pos], sub_str_len) == 0) {
1164 return true;
1165 }
1166 }
1167
1168 return false;
1169 }
1170
data_cb(struct bt_data * data,void * user_data)1171 static bool data_cb(struct bt_data *data, void *user_data)
1172 {
1173 bool *device_found = user_data;
1174 char name[NAME_LEN] = {0};
1175
1176 switch (data->type) {
1177 case BT_DATA_NAME_SHORTENED:
1178 case BT_DATA_NAME_COMPLETE:
1179 case BT_DATA_BROADCAST_NAME:
1180 memcpy(name, data->data, MIN(data->data_len, NAME_LEN - 1));
1181
1182 if (is_substring(CONFIG_TARGET_BROADCAST_NAME, name)) {
1183 /* Device found */
1184 *device_found = true;
1185 return false;
1186 }
1187 return true;
1188 default:
1189 return true;
1190 }
1191 }
1192
broadcast_scan_recv(const struct bt_le_scan_recv_info * info,struct net_buf_simple * ad)1193 static void broadcast_scan_recv(const struct bt_le_scan_recv_info *info, struct net_buf_simple *ad)
1194 {
1195 if (info->interval != 0U) {
1196 /* call to bt_data_parse consumes netbufs so shallow clone for verbose output */
1197
1198 /* If req_recv_state is not NULL then we have been requested by a broadcast
1199 * assistant to sync to a specific broadcast source. In that case we do not apply
1200 * our own broadcast name filter.
1201 */
1202 if (req_recv_state == NULL && strlen(CONFIG_TARGET_BROADCAST_NAME) > 0U) {
1203 bool device_found = false;
1204 struct net_buf_simple buf_copy;
1205
1206 net_buf_simple_clone(ad, &buf_copy);
1207 bt_data_parse(&buf_copy, data_cb, &device_found);
1208
1209 if (!device_found) {
1210 return;
1211 }
1212 }
1213 bt_data_parse(ad, scan_check_and_sync_broadcast, (void *)info);
1214 }
1215 }
1216
1217 static struct bt_le_scan_cb bap_scan_cb = {
1218 .recv = broadcast_scan_recv,
1219 };
1220
bap_pa_sync_synced_cb(struct bt_le_per_adv_sync * sync,struct bt_le_per_adv_sync_synced_info * info)1221 static void bap_pa_sync_synced_cb(struct bt_le_per_adv_sync *sync,
1222 struct bt_le_per_adv_sync_synced_info *info)
1223 {
1224 if (sync == pa_sync ||
1225 (req_recv_state != NULL && bt_addr_le_eq(info->addr, &req_recv_state->addr) &&
1226 info->sid == req_recv_state->adv_sid)) {
1227 printk("PA sync %p synced for broadcast sink with broadcast ID 0x%06X\n", sync,
1228 broadcaster_broadcast_id);
1229
1230 if (pa_sync == NULL) {
1231 pa_sync = sync;
1232 }
1233
1234 k_work_cancel_delayable(&pa_timer);
1235 k_sem_give(&sem_pa_synced);
1236 }
1237 }
1238
bap_pa_sync_terminated_cb(struct bt_le_per_adv_sync * sync,const struct bt_le_per_adv_sync_term_info * info)1239 static void bap_pa_sync_terminated_cb(struct bt_le_per_adv_sync *sync,
1240 const struct bt_le_per_adv_sync_term_info *info)
1241 {
1242 if (sync == pa_sync) {
1243 printk("PA sync %p lost with reason 0x%02X\n", sync, info->reason);
1244 pa_sync = NULL;
1245
1246 k_sem_give(&sem_pa_sync_lost);
1247
1248 if (info->reason != BT_HCI_ERR_LOCALHOST_TERM_CONN && req_recv_state != NULL) {
1249 int err;
1250
1251 if (big_synced) {
1252 err = bt_bap_broadcast_sink_stop(broadcast_sink);
1253 if (err != 0) {
1254 printk("Failed to stop Broadcast Sink: %d\n", err);
1255
1256 return;
1257 }
1258 }
1259
1260 err = bt_bap_scan_delegator_rem_src(req_recv_state->src_id);
1261 if (err != 0) {
1262 printk("Failed to remove source: %d\n", err);
1263
1264 return;
1265 }
1266 }
1267 }
1268 }
1269
1270 static struct bt_le_per_adv_sync_cb bap_pa_sync_cb = {
1271 .synced = bap_pa_sync_synced_cb,
1272 .term = bap_pa_sync_terminated_cb,
1273 };
1274
init(void)1275 static int init(void)
1276 {
1277 int err;
1278
1279 err = bt_enable(NULL);
1280 if (err) {
1281 printk("Bluetooth enable failed (err %d)\n", err);
1282 return err;
1283 }
1284
1285 printk("Bluetooth initialized\n");
1286
1287 err = bt_pacs_cap_register(BT_AUDIO_DIR_SINK, &cap);
1288 if (err) {
1289 printk("Capability register failed (err %d)\n", err);
1290 return err;
1291 }
1292
1293 err = bt_bap_scan_delegator_register(&scan_delegator_cbs);
1294 if (err) {
1295 printk("Scan delegator register failed (err %d)\n", err);
1296 return err;
1297 }
1298
1299 bt_bap_broadcast_sink_register_cb(&broadcast_sink_cbs);
1300 bt_le_per_adv_sync_cb_register(&bap_pa_sync_cb);
1301 bt_le_scan_cb_register(&bap_scan_cb);
1302
1303 for (size_t i = 0U; i < ARRAY_SIZE(streams); i++) {
1304 streams[i].stream.ops = &stream_ops;
1305 }
1306
1307 /* Initialize ring buffers and USB */
1308 #if defined(CONFIG_USB_DEVICE_AUDIO)
1309 const struct device *hs_dev = DEVICE_DT_GET(DT_NODELABEL(hs_0));
1310 static const struct usb_audio_ops usb_ops = {
1311 .data_request_cb = usb_data_request_cb,
1312 .data_written_cb = usb_data_written_cb,
1313 };
1314
1315 if (!device_is_ready(hs_dev)) {
1316 printk("Cannot get USB Headset Device\n");
1317 return -EIO;
1318 }
1319
1320 usb_audio_register(hs_dev, &usb_ops);
1321 err = usb_enable(NULL);
1322 if (err && err != -EALREADY) {
1323 printk("Failed to enable USB\n");
1324 return err;
1325 }
1326 #endif /* defined(CONFIG_USB_DEVICE_AUDIO) */
1327
1328 return 0;
1329 }
1330
reset(void)1331 static int reset(void)
1332 {
1333 int err;
1334
1335 printk("Reset\n");
1336
1337 bis_index_bitfield = 0U;
1338 requested_bis_sync = 0U;
1339 req_recv_state = NULL;
1340 big_synced = false;
1341 base_received = false;
1342 stream_count = 0U;
1343 (void)memset(sink_broadcast_code, 0, sizeof(sink_broadcast_code));
1344 (void)memset(&broadcaster_info, 0, sizeof(broadcaster_info));
1345 (void)memset(&broadcaster_addr, 0, sizeof(broadcaster_addr));
1346 broadcaster_broadcast_id = BT_BAP_INVALID_BROADCAST_ID;
1347
1348 if (broadcast_sink != NULL) {
1349 err = bt_bap_broadcast_sink_delete(broadcast_sink);
1350 if (err) {
1351 printk("Deleting broadcast sink failed (err %d)\n", err);
1352
1353 return err;
1354 }
1355
1356 broadcast_sink = NULL;
1357 }
1358
1359 if (pa_sync != NULL) {
1360 bt_le_per_adv_sync_delete(pa_sync);
1361 if (err) {
1362 printk("Deleting PA sync failed (err %d)\n", err);
1363
1364 return err;
1365 }
1366
1367 pa_sync = NULL;
1368 }
1369
1370 k_sem_reset(&sem_broadcaster_found);
1371 k_sem_reset(&sem_pa_synced);
1372 k_sem_reset(&sem_base_received);
1373 k_sem_reset(&sem_syncable);
1374 k_sem_reset(&sem_pa_sync_lost);
1375 k_sem_reset(&sem_broadcast_code_received);
1376 k_sem_reset(&sem_bis_sync_requested);
1377 k_sem_reset(&sem_stream_connected);
1378 k_sem_reset(&sem_stream_started);
1379 k_sem_reset(&sem_broadcast_sink_stopped);
1380
1381 return 0;
1382 }
1383
start_adv(void)1384 static int start_adv(void)
1385 {
1386 const struct bt_data ad[] = {
1387 BT_DATA_BYTES(BT_DATA_FLAGS, (BT_LE_AD_GENERAL | BT_LE_AD_NO_BREDR)),
1388 BT_DATA_BYTES(BT_DATA_UUID16_ALL,
1389 BT_UUID_16_ENCODE(BT_UUID_BASS_VAL),
1390 BT_UUID_16_ENCODE(BT_UUID_PACS_VAL)),
1391 BT_DATA_BYTES(BT_DATA_SVC_DATA16, BT_UUID_16_ENCODE(BT_UUID_BASS_VAL)),
1392 BT_DATA(BT_DATA_NAME_COMPLETE, CONFIG_BT_DEVICE_NAME,
1393 sizeof(CONFIG_BT_DEVICE_NAME) - 1),
1394 };
1395 int err;
1396
1397 /* Create a connectable advertising set */
1398 err = bt_le_ext_adv_create(BT_LE_EXT_ADV_CONN, NULL, &ext_adv);
1399 if (err != 0) {
1400 printk("Failed to create advertising set (err %d)\n", err);
1401
1402 return err;
1403 }
1404
1405 err = bt_le_ext_adv_set_data(ext_adv, ad, ARRAY_SIZE(ad), NULL, 0);
1406 if (err != 0) {
1407 printk("Failed to set advertising data (err %d)\n", err);
1408
1409 return err;
1410 }
1411
1412 err = bt_le_ext_adv_start(ext_adv, BT_LE_EXT_ADV_START_DEFAULT);
1413 if (err != 0) {
1414 printk("Failed to start advertising set (err %d)\n", err);
1415
1416 return err;
1417 }
1418
1419 return 0;
1420 }
1421
stop_adv(void)1422 static int stop_adv(void)
1423 {
1424 int err;
1425
1426 err = bt_le_ext_adv_stop(ext_adv);
1427 if (err != 0) {
1428 printk("Failed to stop advertising set (err %d)\n", err);
1429
1430 return err;
1431 }
1432
1433 err = bt_le_ext_adv_delete(ext_adv);
1434 if (err != 0) {
1435 printk("Failed to delete advertising set (err %d)\n", err);
1436
1437 return err;
1438 }
1439
1440 ext_adv = NULL;
1441
1442 return 0;
1443 }
1444
pa_sync_create(void)1445 static int pa_sync_create(void)
1446 {
1447 struct bt_le_per_adv_sync_param create_params = {0};
1448
1449 bt_addr_le_copy(&create_params.addr, &broadcaster_addr);
1450 create_params.options = BT_LE_PER_ADV_SYNC_OPT_FILTER_DUPLICATE;
1451 create_params.sid = broadcaster_info.sid;
1452 create_params.skip = PA_SYNC_SKIP;
1453 create_params.timeout = interval_to_sync_timeout(broadcaster_info.interval);
1454
1455 return bt_le_per_adv_sync_create(&create_params, &pa_sync);
1456 }
1457
main(void)1458 int main(void)
1459 {
1460 int err;
1461
1462 err = init();
1463 if (err) {
1464 printk("Init failed (err %d)\n", err);
1465 return 0;
1466 }
1467
1468 for (size_t i = 0U; i < ARRAY_SIZE(streams_p); i++) {
1469 streams_p[i] = &streams[i].stream;
1470 #if defined(CONFIG_LIBLC3)
1471 k_mutex_init(&streams[i].lc3_decoder_mutex);
1472 #endif /* defined(CONFIG_LIBLC3) */
1473 }
1474
1475 while (true) {
1476 uint32_t sync_bitfield;
1477
1478 err = reset();
1479 if (err != 0) {
1480 printk("Resetting failed: %d - Aborting\n", err);
1481
1482 return 0;
1483 }
1484
1485 if (IS_ENABLED(CONFIG_SCAN_OFFLOAD)) {
1486 if (broadcast_assistant_conn == NULL) {
1487 k_sem_reset(&sem_connected);
1488
1489 printk("Starting advertising\n");
1490 /* Stop advertising before starting if needed */
1491 if (ext_adv != NULL) {
1492 err = stop_adv();
1493 if (err != 0) {
1494 printk("Unable to stop advertising: %d\n", err);
1495
1496 return 0;
1497 }
1498 }
1499 err = start_adv();
1500 if (err != 0) {
1501 printk("Unable to start advertising connectable: %d\n",
1502 err);
1503
1504 return 0;
1505 }
1506
1507 printk("Waiting for Broadcast Assistant\n");
1508 err = k_sem_take(&sem_connected, ADV_TIMEOUT);
1509 if (err != 0) {
1510 printk("No Broadcast Assistant connected\n");
1511
1512 err = stop_adv();
1513 if (err != 0) {
1514 printk("Unable to stop advertising: %d\n", err);
1515
1516 return 0;
1517 }
1518 }
1519 }
1520
1521 if (broadcast_assistant_conn != NULL) {
1522 k_sem_reset(&sem_pa_request);
1523 k_sem_reset(&sem_past_request);
1524 k_sem_reset(&sem_disconnected);
1525
1526 /* Wait for the PA request to determine if we
1527 * should start scanning, or wait for PAST
1528 */
1529 printk("Waiting for PA sync request\n");
1530 err = k_sem_take(&sem_pa_request,
1531 BROADCAST_ASSISTANT_TIMEOUT);
1532 if (err != 0) {
1533 printk("sem_pa_request timed out, resetting\n");
1534 continue;
1535 }
1536
1537 if (k_sem_take(&sem_past_request, K_NO_WAIT) == 0) {
1538 goto wait_for_pa_sync;
1539 } /* else continue with scanning below */
1540 }
1541 }
1542
1543 if (strlen(CONFIG_TARGET_BROADCAST_NAME) > 0U) {
1544 printk("Scanning for broadcast sources containing "
1545 "`" CONFIG_TARGET_BROADCAST_NAME "`\n");
1546 } else {
1547 printk("Scanning for broadcast sources\n");
1548 }
1549
1550 err = bt_le_scan_start(BT_LE_SCAN_ACTIVE, NULL);
1551 if (err != 0 && err != -EALREADY) {
1552 printk("Unable to start scan for broadcast sources: %d\n",
1553 err);
1554 return 0;
1555 }
1556
1557 printk("Waiting for Broadcaster\n");
1558 err = k_sem_take(&sem_broadcaster_found, SEM_TIMEOUT);
1559 if (err != 0) {
1560 printk("sem_broadcaster_found timed out, resetting\n");
1561 continue;
1562 }
1563
1564 err = bt_le_scan_stop();
1565 if (err != 0) {
1566 printk("bt_le_scan_stop failed with %d, resetting\n", err);
1567 continue;
1568 }
1569
1570 printk("Attempting to PA sync to the broadcaster with id 0x%06X\n",
1571 broadcaster_broadcast_id);
1572 err = pa_sync_create();
1573 if (err != 0) {
1574 printk("Could not create Broadcast PA sync: %d, resetting\n", err);
1575 continue;
1576 }
1577
1578 wait_for_pa_sync:
1579 printk("Waiting for PA synced\n");
1580 err = k_sem_take(&sem_pa_synced, SEM_TIMEOUT);
1581 if (err != 0) {
1582 printk("sem_pa_synced timed out, resetting\n");
1583 continue;
1584 }
1585
1586 printk("Broadcast source PA synced, creating Broadcast Sink\n");
1587 err = bt_bap_broadcast_sink_create(pa_sync, broadcaster_broadcast_id,
1588 &broadcast_sink);
1589 if (err != 0) {
1590 printk("Failed to create broadcast sink: %d\n", err);
1591 continue;
1592 }
1593
1594 printk("Broadcast Sink created, waiting for BASE\n");
1595 err = k_sem_take(&sem_base_received, SEM_TIMEOUT);
1596 if (err != 0) {
1597 printk("sem_base_received timed out, resetting\n");
1598 continue;
1599 }
1600
1601 printk("BASE received, waiting for syncable\n");
1602 err = k_sem_take(&sem_syncable, SEM_TIMEOUT);
1603 if (err != 0) {
1604 printk("sem_syncable timed out, resetting\n");
1605 continue;
1606 }
1607
1608 /* sem_broadcast_code_received is also given if the
1609 * broadcast is not encrypted
1610 */
1611 printk("Waiting for broadcast code\n");
1612 err = k_sem_take(&sem_broadcast_code_received, SEM_TIMEOUT);
1613 if (err != 0) {
1614 printk("sem_broadcast_code_received timed out, resetting\n");
1615 continue;
1616 }
1617
1618 printk("Waiting for BIS sync request\n");
1619 err = k_sem_take(&sem_bis_sync_requested, SEM_TIMEOUT);
1620 if (err != 0) {
1621 printk("sem_bis_sync_requested timed out, resetting\n");
1622 continue;
1623 }
1624
1625 sync_bitfield = bis_index_bitfield & requested_bis_sync;
1626 stream_count = 0;
1627 for (int i = 0; i < BT_ISO_MAX_GROUP_ISO_COUNT; i++) {
1628 if ((sync_bitfield & BIT(i)) != 0) {
1629 stream_count++;
1630 }
1631 }
1632
1633 printk("Syncing to broadcast with bitfield: 0x%08x = 0x%08x (bis_index) & 0x%08x "
1634 "(req_bis_sync), stream_count = %u\n",
1635 sync_bitfield, bis_index_bitfield, requested_bis_sync, stream_count);
1636
1637 err = bt_bap_broadcast_sink_sync(broadcast_sink, sync_bitfield, streams_p,
1638 sink_broadcast_code);
1639 if (err != 0) {
1640 printk("Unable to sync to broadcast source: %d\n", err);
1641 return 0;
1642 }
1643
1644 printk("Waiting for stream(s) started\n");
1645 err = k_sem_take(&sem_big_synced, SEM_TIMEOUT);
1646 if (err != 0) {
1647 printk("sem_big_synced timed out, resetting\n");
1648 continue;
1649 }
1650
1651 printk("Waiting for PA disconnected\n");
1652 k_sem_take(&sem_pa_sync_lost, K_FOREVER);
1653
1654 printk("Waiting for sink to stop\n");
1655 err = k_sem_take(&sem_broadcast_sink_stopped, SEM_TIMEOUT);
1656 if (err != 0) {
1657 printk("sem_broadcast_sink_stopped timed out, resetting\n");
1658 continue;
1659 }
1660 }
1661
1662 return 0;
1663 }
1664
