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