1 /**
2 * @file
3 * @brief Bluetooth BAP Broadcast Sink LC3 extension
4 *
5 * This files handles all the LC3 related functionality for the sample
6 *
7 * Copyright (c) 2025 Nordic Semiconductor ASA
8 *
9 * SPDX-License-Identifier: Apache-2.0
10 */
11
12 #include <errno.h>
13 #include <stdbool.h>
14 #include <stddef.h>
15 #include <stdint.h>
16 #include <string.h>
17
18 #include <zephyr/autoconf.h>
19 #include <zephyr/bluetooth/audio/audio.h>
20 #include <zephyr/bluetooth/hci_types.h>
21 #include <zephyr/bluetooth/iso.h>
22 #include <zephyr/device.h>
23 #include <zephyr/devicetree.h>
24 #include <zephyr/kernel.h>
25 #include <zephyr/logging/log.h>
26 #include <zephyr/logging/log_core.h>
27 #include <zephyr/net_buf.h>
28 #include <zephyr/shell/shell.h>
29 #include <zephyr/sys/ring_buffer.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 #include <zephyr/usb/usb_device.h>
35 #include <zephyr/usb/class/usb_audio.h>
36
37 #include <lc3.h>
38
39 #include "lc3.h"
40 #include "stream_rx.h"
41 #include "usb.h"
42
43 LOG_MODULE_REGISTER(lc3, CONFIG_LOG_DEFAULT_LEVEL);
44
45 #define LC3_ENCODER_STACK_SIZE 4096
46 #define LC3_ENCODER_PRIORITY 5
47
48 struct lc3_data {
49 void *fifo_reserved; /* 1st word reserved for use by FIFO */
50 struct net_buf *buf;
51 struct stream_rx *stream;
52 uint32_t ts;
53 bool do_plc;
54 };
55
56 K_MEM_SLAB_DEFINE_STATIC(lc3_data_slab, sizeof(struct lc3_data), CONFIG_BT_ISO_RX_BUF_COUNT,
57 __alignof__(struct lc3_data));
58
59 static int16_t lc3_rx_buf[LC3_MAX_NUM_SAMPLES_MONO];
60 static K_FIFO_DEFINE(lc3_in_fifo);
61
62 /* We only want to send USB to left/right from a single stream. If we have 2 left streams, the
63 * outgoing audio is going to be terrible.
64 * Since a stream can contain stereo data, both of these may be the same stream.
65 */
66 static struct stream_rx *usb_left_stream;
67 static struct stream_rx *usb_right_stream;
68
init_lc3_decoder(struct stream_rx * stream,uint32_t lc3_frame_duration_us,uint32_t lc3_freq_hz)69 static int init_lc3_decoder(struct stream_rx *stream, uint32_t lc3_frame_duration_us,
70 uint32_t lc3_freq_hz)
71 {
72 if (stream == NULL) {
73 LOG_ERR("NULL stream to init LC3 decoder");
74 return -EINVAL;
75 }
76
77 if (stream->lc3_decoder != NULL) {
78 LOG_ERR("Already initialized");
79 return -EALREADY;
80 }
81
82 if (lc3_freq_hz == 0U || lc3_frame_duration_us == 0U) {
83 LOG_ERR("Invalid freq (%u) or frame duration (%u)", lc3_freq_hz,
84 lc3_frame_duration_us);
85
86 return -EINVAL;
87 }
88
89 LOG_INF("Initializing the LC3 decoder with %u us duration and %u Hz frequency",
90 lc3_frame_duration_us, lc3_freq_hz);
91 /* Create the decoder instance. This shall complete before stream_started() is called. */
92 stream->lc3_decoder =
93 lc3_setup_decoder(lc3_frame_duration_us, lc3_freq_hz,
94 IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT) ? USB_SAMPLE_RATE_HZ : 0,
95 &stream->lc3_decoder_mem);
96 if (stream->lc3_decoder == NULL) {
97 LOG_ERR("Failed to setup LC3 decoder - wrong parameters?\n");
98 return -EINVAL;
99 }
100
101 LOG_INF("Initialized LC3 decoder for %p", stream);
102
103 return 0;
104 }
105
decode_frame(struct lc3_data * data,size_t frame_cnt)106 static bool decode_frame(struct lc3_data *data, size_t frame_cnt)
107 {
108 const struct stream_rx *stream = data->stream;
109 const size_t total_frames = stream->lc3_chan_cnt * stream->lc3_frame_blocks_per_sdu;
110 const uint16_t octets_per_frame = stream->lc3_octets_per_frame;
111 struct net_buf *buf = data->buf;
112 void *iso_data;
113 int err;
114
115 if (data->do_plc) {
116 iso_data = NULL; /* perform PLC */
117
118 #if CONFIG_INFO_REPORTING_INTERVAL > 0
119 if ((stream->reporting_info.lc3_decoded_cnt % CONFIG_INFO_REPORTING_INTERVAL) ==
120 0) {
121 LOG_DBG("[%zu]: Performing PLC", stream->reporting_info.lc3_decoded_cnt);
122 }
123 #endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */
124
125 data->do_plc = false; /* clear flag */
126 } else {
127 iso_data = net_buf_pull_mem(data->buf, octets_per_frame);
128
129 #if CONFIG_INFO_REPORTING_INTERVAL > 0
130 if ((stream->reporting_info.lc3_decoded_cnt % CONFIG_INFO_REPORTING_INTERVAL) ==
131 0) {
132 LOG_DBG("[%zu]: Decoding frame of size %u (%u/%u)",
133 stream->reporting_info.lc3_decoded_cnt, octets_per_frame,
134 frame_cnt + 1, total_frames);
135 }
136 #endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */
137 }
138
139 err = lc3_decode(stream->lc3_decoder, iso_data, octets_per_frame, LC3_PCM_FORMAT_S16,
140 lc3_rx_buf, 1);
141 if (err < 0) {
142 LOG_ERR("Failed to decode LC3 data (%u/%u - %u/%u)", frame_cnt + 1, total_frames,
143 octets_per_frame * frame_cnt, buf->len);
144 return false;
145 }
146
147 return true;
148 }
149
get_lc3_chan_alloc_from_index(const struct stream_rx * stream,uint8_t index,enum bt_audio_location * chan_alloc)150 static int get_lc3_chan_alloc_from_index(const struct stream_rx *stream, uint8_t index,
151 enum bt_audio_location *chan_alloc)
152 {
153 #if defined(CONFIG_USE_USB_AUDIO_OUTPUT)
154 const bool has_left = (stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
155 const bool has_right = (stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
156 const bool is_mono = stream->lc3_chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
157 const bool is_left = index == 0 && has_left;
158 const bool is_right = has_right && (index == 0U || (index == 1U && has_left));
159
160 /* LC3 is always Left before Right, so we can use the index and the stream channel
161 * allocation to determine if index 0 is left or right.
162 */
163 if (is_left) {
164 *chan_alloc = BT_AUDIO_LOCATION_FRONT_LEFT;
165 } else if (is_right) {
166 *chan_alloc = BT_AUDIO_LOCATION_FRONT_RIGHT;
167 } else if (is_mono) {
168 *chan_alloc = BT_AUDIO_LOCATION_MONO_AUDIO;
169 } else {
170 /* Not suitable for USB */
171 return -EINVAL;
172 }
173
174 return 0;
175 #else /* !CONFIG_USE_USB_AUDIO_OUTPUT */
176 return -EINVAL;
177 #endif /* CONFIG_USE_USB_AUDIO_OUTPUT */
178 }
179
decode_frame_block(struct lc3_data * data,size_t frame_cnt)180 static size_t decode_frame_block(struct lc3_data *data, size_t frame_cnt)
181 {
182 const struct stream_rx *stream = data->stream;
183 const uint8_t chan_cnt = stream->lc3_chan_cnt;
184 size_t decoded_frames = 0U;
185
186 for (uint8_t i = 0U; i < chan_cnt; i++) {
187 /* We provide the total number of decoded frames to `decode_frame` for logging
188 * purposes
189 */
190 if (decode_frame(data, frame_cnt + decoded_frames)) {
191 decoded_frames++;
192
193 if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) {
194 enum bt_audio_location chan_alloc;
195 int err;
196
197 err = get_lc3_chan_alloc_from_index(stream, i, &chan_alloc);
198 if (err != 0) {
199 /* Not suitable for USB */
200 continue;
201 }
202
203 /* We only want to left or right from one stream to USB */
204 if ((chan_alloc == BT_AUDIO_LOCATION_FRONT_LEFT &&
205 stream != usb_left_stream) ||
206 (chan_alloc == BT_AUDIO_LOCATION_FRONT_RIGHT &&
207 stream != usb_right_stream)) {
208 continue;
209 }
210
211 /* TODO: Add support for properly support the presentation delay.
212 * For now we just send audio to USB as soon as we get it
213 */
214 err = usb_add_frame_to_usb(chan_alloc, lc3_rx_buf,
215 sizeof(lc3_rx_buf), data->ts);
216 if (err == -EINVAL) {
217 continue;
218 }
219 }
220 } else {
221 /* If decoding failed, we clear the data to USB as it would contain
222 * invalid data
223 */
224 if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) {
225 usb_clear_frames_to_usb();
226 }
227
228 break;
229 }
230 }
231
232 return decoded_frames;
233 }
234
do_lc3_decode(struct lc3_data * data)235 static void do_lc3_decode(struct lc3_data *data)
236 {
237 struct stream_rx *stream = data->stream;
238
239 if (stream->lc3_decoder != NULL) {
240 const uint8_t frame_blocks_per_sdu = stream->lc3_frame_blocks_per_sdu;
241 size_t frame_cnt;
242
243 frame_cnt = 0;
244 for (uint8_t i = 0U; i < frame_blocks_per_sdu; i++) {
245 const size_t decoded_frames = decode_frame_block(data, frame_cnt);
246
247 if (decoded_frames == 0) {
248 break;
249 }
250
251 frame_cnt += decoded_frames;
252 }
253
254 #if CONFIG_INFO_REPORTING_INTERVAL > 0
255 stream->reporting_info.lc3_decoded_cnt++;
256 #endif /* CONFIG_INFO_REPORTING_INTERVAL > 0 */
257 }
258
259 net_buf_unref(data->buf);
260 }
261
lc3_decoder_thread_func(void * arg1,void * arg2,void * arg3)262 static void lc3_decoder_thread_func(void *arg1, void *arg2, void *arg3)
263 {
264 while (true) {
265 struct lc3_data *data = k_fifo_get(&lc3_in_fifo, K_FOREVER);
266 struct stream_rx *stream = data->stream;
267
268 if (stream->lc3_decoder == NULL) {
269 LOG_WRN("Decoder is NULL, discarding data from FIFO");
270 k_mem_slab_free(&lc3_data_slab, (void *)data);
271 continue; /* Wait for new data */
272 }
273
274 do_lc3_decode(data);
275
276 k_mem_slab_free(&lc3_data_slab, (void *)data);
277 }
278 }
279
lc3_enable(struct stream_rx * stream)280 int lc3_enable(struct stream_rx *stream)
281 {
282 const struct bt_audio_codec_cfg *codec_cfg = stream->stream.codec_cfg;
283 uint32_t lc3_frame_duration_us;
284 uint32_t lc3_freq_hz;
285 int ret;
286
287 if (codec_cfg->id != BT_HCI_CODING_FORMAT_LC3) {
288 return -EINVAL;
289 }
290
291 ret = bt_audio_codec_cfg_get_freq(codec_cfg);
292 if (ret >= 0) {
293 ret = bt_audio_codec_cfg_freq_to_freq_hz(ret);
294
295 if (ret > 0) {
296 if (ret == 8000 || ret == 16000 || ret == 24000 || ret == 32000 ||
297 ret == 48000) {
298 lc3_freq_hz = (uint32_t)ret;
299 } else {
300 LOG_ERR("Unsupported frequency for LC3: %d", ret);
301 lc3_freq_hz = 0U;
302 }
303 } else {
304 LOG_ERR("Invalid frequency: %d", ret);
305 lc3_freq_hz = 0U;
306 }
307 } else {
308 LOG_ERR("Could not get frequency: %d", ret);
309 lc3_freq_hz = 0U;
310 }
311
312 if (lc3_freq_hz == 0U) {
313 return -EINVAL;
314 }
315
316 ret = bt_audio_codec_cfg_get_frame_dur(codec_cfg);
317 if (ret >= 0) {
318 ret = bt_audio_codec_cfg_frame_dur_to_frame_dur_us(ret);
319 if (ret > 0) {
320 lc3_frame_duration_us = (uint32_t)ret;
321 } else {
322 LOG_ERR("Invalid frame duration: %d", ret);
323 lc3_frame_duration_us = 0U;
324 }
325 } else {
326 LOG_ERR("Could not get frame duration: %d", ret);
327 lc3_frame_duration_us = 0U;
328 }
329
330 if (lc3_frame_duration_us == 0U) {
331 return -EINVAL;
332 }
333
334 ret = bt_audio_codec_cfg_get_chan_allocation(codec_cfg, &stream->lc3_chan_allocation, true);
335 if (ret == 0) {
336 stream->lc3_chan_cnt = bt_audio_get_chan_count(stream->lc3_chan_allocation);
337 } else {
338 LOG_DBG("Could not get channel allocation: %d", ret);
339 stream->lc3_chan_cnt = 0U;
340 }
341
342 if (stream->lc3_chan_cnt == 0U) {
343 return -EINVAL;
344 }
345
346 ret = bt_audio_codec_cfg_get_frame_blocks_per_sdu(codec_cfg, true);
347 if (ret >= 0) {
348 stream->lc3_frame_blocks_per_sdu = (uint8_t)ret;
349 } else {
350 LOG_ERR("Could not get frame blocks per SDU: %d", ret);
351 stream->lc3_frame_blocks_per_sdu = 0U;
352 }
353
354 if (stream->lc3_frame_blocks_per_sdu == 0U) {
355 return -EINVAL;
356 }
357
358 ret = bt_audio_codec_cfg_get_octets_per_frame(codec_cfg);
359 if (ret >= 0) {
360 stream->lc3_octets_per_frame = (uint16_t)ret;
361 } else {
362 LOG_ERR("Could not get octets per frame: %d", ret);
363 stream->lc3_octets_per_frame = 0U;
364 }
365
366 if (stream->lc3_octets_per_frame == 0U) {
367 return -EINVAL;
368 }
369
370 if (stream->lc3_decoder == NULL) {
371 const int err = init_lc3_decoder(stream, lc3_frame_duration_us, lc3_freq_hz);
372
373 if (err != 0) {
374 LOG_ERR("Failed to init LC3 decoder: %d", err);
375
376 return err;
377 }
378 }
379
380 if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) {
381 if ((stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0) {
382 if (usb_left_stream == NULL) {
383 LOG_INF("Setting USB left stream to %p", stream);
384 usb_left_stream = stream;
385 } else {
386 LOG_WRN("Multiple left streams started");
387 }
388 }
389
390 if ((stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0) {
391 if (usb_right_stream == NULL) {
392 LOG_INF("Setting USB right stream to %p", stream);
393 usb_right_stream = stream;
394 } else {
395 LOG_WRN("Multiple right streams started");
396 }
397 }
398 }
399
400 return 0;
401 }
402
lc3_disable(struct stream_rx * stream)403 int lc3_disable(struct stream_rx *stream)
404 {
405 if (stream->lc3_decoder == NULL) {
406 return -EINVAL;
407 }
408
409 stream->lc3_decoder = NULL;
410
411 if (IS_ENABLED(CONFIG_USE_USB_AUDIO_OUTPUT)) {
412 if (usb_left_stream == stream) {
413 usb_left_stream = NULL;
414 }
415 if (usb_right_stream == stream) {
416 usb_right_stream = NULL;
417 }
418 }
419
420 return 0;
421 }
422
lc3_enqueue_for_decoding(struct stream_rx * stream,const struct bt_iso_recv_info * info,struct net_buf * buf)423 void lc3_enqueue_for_decoding(struct stream_rx *stream, const struct bt_iso_recv_info *info,
424 struct net_buf *buf)
425 {
426 const uint8_t frame_blocks_per_sdu = stream->lc3_frame_blocks_per_sdu;
427 const uint16_t octets_per_frame = stream->lc3_octets_per_frame;
428 const uint8_t chan_cnt = stream->lc3_chan_cnt;
429 struct lc3_data *data;
430
431 if (stream->lc3_decoder == NULL) {
432 return;
433 }
434
435 /* Allocate a context that holds both the buffer and the stream so that we can
436 * send both of these values to the LC3 decoder thread as a single struct
437 * in a FIFO
438 */
439 if (k_mem_slab_alloc(&lc3_data_slab, (void **)&data, K_NO_WAIT)) {
440 LOG_WRN("Could not allocate LC3 data item");
441 return;
442 }
443
444 if ((info->flags & BT_ISO_FLAGS_VALID) == 0) {
445 data->do_plc = true;
446 } else if (buf->len != (octets_per_frame * chan_cnt * frame_blocks_per_sdu)) {
447 if (buf->len != 0U) {
448 LOG_WRN("Expected %u frame blocks with %u channels of size %u, but "
449 "length is %u",
450 frame_blocks_per_sdu, chan_cnt, octets_per_frame, buf->len);
451 }
452
453 data->do_plc = true;
454 }
455
456 data->buf = net_buf_ref(buf);
457 data->stream = stream;
458 if (info->flags & BT_ISO_FLAGS_TS) {
459 data->ts = info->ts;
460 } else {
461 data->ts = 0U;
462 }
463
464 k_fifo_put(&lc3_in_fifo, data);
465 }
466
lc3_init(void)467 int lc3_init(void)
468 {
469 static K_KERNEL_STACK_DEFINE(lc3_decoder_thread_stack, 4096);
470 const int lc3_decoder_thread_prio = K_PRIO_PREEMPT(5);
471 static struct k_thread lc3_decoder_thread;
472 static bool initialized;
473
474 if (initialized) {
475 return -EALREADY;
476 }
477
478 k_thread_create(&lc3_decoder_thread, lc3_decoder_thread_stack,
479 K_KERNEL_STACK_SIZEOF(lc3_decoder_thread_stack), lc3_decoder_thread_func,
480 NULL, NULL, NULL, lc3_decoder_thread_prio, 0, K_NO_WAIT);
481 k_thread_name_set(&lc3_decoder_thread, "LC3 Decoder");
482
483 LOG_INF("LC3 initialized");
484 initialized = true;
485
486 return 0;
487 }
488