1 /**
2 * @file
3 * @brief Bluetooth BAP Broadcast Sink Sample LC3
4 *
5 * This files handles all the USB related functionality to audio out 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/device.h>
21 #include <zephyr/devicetree.h>
22 #include <zephyr/kernel.h>
23 #include <zephyr/logging/log.h>
24 #include <zephyr/net_buf.h>
25 #include <zephyr/shell/shell.h>
26 #include <zephyr/sys/ring_buffer.h>
27 #include <zephyr/sys/util.h>
28 #include <zephyr/sys/util_macro.h>
29 #include <zephyr/sys_clock.h>
30 #include <zephyr/toolchain.h>
31 #include <zephyr/usb/usb_device.h>
32 #include <zephyr/usb/class/usb_audio.h>
33
34 #include "lc3.h"
35 #include "stream_rx.h"
36 #include "usb.h"
37
38 LOG_MODULE_REGISTER(usb, CONFIG_LOG_DEFAULT_LEVEL);
39
40 #define USB_ENQUEUE_COUNT 30U /* 30 times 1ms frames => 30ms */
41 #define USB_FRAME_DURATION_US 1000U
42 #define USB_SAMPLE_CNT ((USB_FRAME_DURATION_US * USB_SAMPLE_RATE_HZ) / USEC_PER_SEC)
43 #define USB_BYTES_PER_SAMPLE sizeof(int16_t)
44 #define USB_MONO_FRAME_SIZE (USB_SAMPLE_CNT * USB_BYTES_PER_SAMPLE)
45 #define USB_CHANNELS 2U
46 #define USB_STEREO_FRAME_SIZE (USB_MONO_FRAME_SIZE * USB_CHANNELS)
47 #define USB_OUT_RING_BUF_SIZE (CONFIG_BT_ISO_RX_BUF_COUNT * LC3_MAX_NUM_SAMPLES_STEREO)
48 #define USB_IN_RING_BUF_SIZE (USB_MONO_FRAME_SIZE * USB_ENQUEUE_COUNT)
49
50 struct decoded_sdu {
51 int16_t right_frames[CONFIG_MAX_CODEC_FRAMES_PER_SDU][LC3_MAX_NUM_SAMPLES_MONO];
52 int16_t left_frames[CONFIG_MAX_CODEC_FRAMES_PER_SDU][LC3_MAX_NUM_SAMPLES_MONO];
53 size_t right_frames_cnt;
54 size_t left_frames_cnt;
55 size_t mono_frames_cnt;
56 uint32_t ts;
57 } decoded_sdu;
58
59 RING_BUF_DECLARE(usb_out_ring_buf, USB_OUT_RING_BUF_SIZE);
60 NET_BUF_POOL_DEFINE(usb_out_buf_pool, USB_ENQUEUE_COUNT, USB_STEREO_FRAME_SIZE, 0, net_buf_destroy);
61
62 /* USB consumer callback, called every 1ms, consumes data from ring-buffer */
usb_data_request_cb(const struct device * dev)63 static void usb_data_request_cb(const struct device *dev)
64 {
65 uint8_t usb_audio_data[USB_STEREO_FRAME_SIZE] = {0};
66 struct net_buf *pcm_buf;
67 uint32_t size;
68 int err;
69
70 if (lc3_get_rx_streaming_cnt() == 0) {
71 /* no-op as we have no streams that receive data */
72 return;
73 }
74
75 pcm_buf = net_buf_alloc(&usb_out_buf_pool, K_NO_WAIT);
76 if (pcm_buf == NULL) {
77 LOG_WRN("Could not allocate pcm_buf");
78 return;
79 }
80
81 /* This may fail without causing issues since usb_audio_data is 0-initialized */
82 size = ring_buf_get(&usb_out_ring_buf, usb_audio_data, sizeof(usb_audio_data));
83
84 net_buf_add_mem(pcm_buf, usb_audio_data, sizeof(usb_audio_data));
85
86 if (size != 0) {
87 static size_t cnt;
88
89 if (CONFIG_INFO_REPORTING_INTERVAL > 0 &&
90 (++cnt % (CONFIG_INFO_REPORTING_INTERVAL * 10)) == 0U) {
91 LOG_INF("[%zu]: Sending USB audio", cnt);
92 }
93 } else {
94 static size_t cnt;
95
96 if (CONFIG_INFO_REPORTING_INTERVAL > 0 &&
97 (++cnt % (CONFIG_INFO_REPORTING_INTERVAL * 10)) == 0U) {
98 LOG_INF("[%zu]: Sending empty USB audio", cnt);
99 }
100 }
101
102 err = usb_audio_send(dev, pcm_buf, sizeof(usb_audio_data));
103 if (err != 0) {
104 static size_t cnt;
105
106 cnt++;
107 if (CONFIG_INFO_REPORTING_INTERVAL > 0 &&
108 (cnt % (CONFIG_INFO_REPORTING_INTERVAL * 10)) == 0) {
109 LOG_ERR("Failed to send USB audio: %d (%zu)", err, cnt);
110 }
111
112 net_buf_unref(pcm_buf);
113 }
114 }
115
usb_data_written_cb(const struct device * dev,struct net_buf * buf,size_t size)116 static void usb_data_written_cb(const struct device *dev, struct net_buf *buf, size_t size)
117 {
118 /* Unreference the buffer now that the USB is done with it */
119 net_buf_unref(buf);
120 }
121
usb_send_frames_to_usb(void)122 static void usb_send_frames_to_usb(void)
123 {
124 const bool is_left_only =
125 decoded_sdu.right_frames_cnt == 0U && decoded_sdu.mono_frames_cnt == 0U;
126 const bool is_right_only =
127 decoded_sdu.left_frames_cnt == 0U && decoded_sdu.mono_frames_cnt == 0U;
128 const bool is_mono_only =
129 decoded_sdu.left_frames_cnt == 0U && decoded_sdu.right_frames_cnt == 0U;
130 const bool is_single_channel = is_left_only || is_right_only || is_mono_only;
131 const size_t frame_cnt =
132 MAX(decoded_sdu.mono_frames_cnt,
133 MAX(decoded_sdu.left_frames_cnt, decoded_sdu.right_frames_cnt));
134 static size_t cnt;
135
136 /* Send frames to USB - If we only have a single channel we mix it to stereo */
137 for (size_t i = 0U; i < frame_cnt; i++) {
138 static int16_t stereo_frame[LC3_MAX_NUM_SAMPLES_STEREO];
139 const int16_t *right_frame = decoded_sdu.right_frames[i];
140 const int16_t *left_frame = decoded_sdu.left_frames[i];
141 const int16_t *mono_frame = decoded_sdu.left_frames[i]; /* use left as mono */
142 static size_t fail_cnt;
143 uint32_t rb_size;
144
145 /* Not enough space to store data */
146 if (ring_buf_space_get(&usb_out_ring_buf) < sizeof(stereo_frame)) {
147 if (CONFIG_INFO_REPORTING_INTERVAL > 0 &&
148 (fail_cnt % CONFIG_INFO_REPORTING_INTERVAL) == 0U) {
149 LOG_WRN("[%zu] Could not send more than %zu frames to USB",
150 fail_cnt, i);
151 }
152
153 fail_cnt++;
154
155 break;
156 }
157
158 fail_cnt = 0U;
159
160 /* Generate the stereo frame
161 *
162 * If we only have single channel then we mix that to stereo
163 */
164 for (int j = 0; j < LC3_MAX_NUM_SAMPLES_MONO; j++) {
165 if (is_single_channel) {
166 int16_t sample = 0;
167
168 /* Mix to stereo as LRLRLRLR */
169 if (is_left_only) {
170 sample = left_frame[j];
171 } else if (is_right_only) {
172 sample = right_frame[j];
173 } else if (is_mono_only) {
174 sample = mono_frame[j];
175 }
176
177 stereo_frame[j * 2] = sample;
178 stereo_frame[j * 2 + 1] = sample;
179 } else {
180 stereo_frame[j * 2] = left_frame[j];
181 stereo_frame[j * 2 + 1] = right_frame[j];
182 }
183 }
184
185 rb_size = ring_buf_put(&usb_out_ring_buf, (uint8_t *)stereo_frame,
186 sizeof(stereo_frame));
187 if (rb_size != sizeof(stereo_frame)) {
188 LOG_WRN("Failed to put frame on USB ring buf");
189
190 break;
191 }
192 }
193
194 if (CONFIG_INFO_REPORTING_INTERVAL > 0 && (++cnt % CONFIG_INFO_REPORTING_INTERVAL) == 0U) {
195 LOG_INF("[%zu]: Sending %u USB audio frame", cnt, frame_cnt);
196 }
197
198 usb_clear_frames_to_usb();
199 }
200
ts_overflowed(uint32_t ts)201 static bool ts_overflowed(uint32_t ts)
202 {
203 /* If the timestamp is a factor of 10 in difference, then we assume that TS overflowed
204 * We cannot simply check if `ts < decoded_sdu.ts` as that could also indicate old data
205 */
206 return ((uint64_t)ts * 10 < decoded_sdu.ts);
207 }
208
usb_add_frame_to_usb(enum bt_audio_location chan_allocation,const int16_t * frame,size_t frame_size,uint32_t ts)209 int usb_add_frame_to_usb(enum bt_audio_location chan_allocation, const int16_t *frame,
210 size_t frame_size, uint32_t ts)
211 {
212 const bool is_left = (chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
213 const bool is_right = (chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
214 const bool is_mono = chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
215 const uint8_t ts_jitter_us = 100; /* timestamps may have jitter */
216 static size_t cnt;
217
218 if (CONFIG_INFO_REPORTING_INTERVAL > 0 && (++cnt % CONFIG_INFO_REPORTING_INTERVAL) == 0U) {
219 LOG_INF("[%zu]: Adding USB audio frame", cnt);
220 }
221
222 if (frame_size > LC3_MAX_NUM_SAMPLES_MONO * sizeof(int16_t) || frame_size == 0U) {
223 LOG_DBG("Invalid frame of size %zu", frame_size);
224
225 return -EINVAL;
226 }
227
228 if (bt_audio_get_chan_count(chan_allocation) != 1) {
229 LOG_DBG("Invalid channel allocation %d", chan_allocation);
230
231 return -EINVAL;
232 }
233
234 if (((is_left || is_right) && decoded_sdu.mono_frames_cnt != 0) ||
235 (is_mono &&
236 (decoded_sdu.left_frames_cnt != 0U || decoded_sdu.right_frames_cnt != 0U))) {
237 LOG_DBG("Cannot mix and match mono with left or right");
238
239 return -EINVAL;
240 }
241
242 /* Check if the frame can be combined with a previous frame from another channel, of if
243 * we have to send previous data to USB and then store the current frame
244 *
245 * This is done by comparing the timestamps of the frames, and in the case that they are the
246 * same, there are additional checks to see if we have received more left than right frames,
247 * in which case we also send existing data
248 */
249
250 if (ts + ts_jitter_us < decoded_sdu.ts && !ts_overflowed(ts)) {
251 /* Old data, discard */
252 return -ENOEXEC;
253 } else if (ts > decoded_sdu.ts + ts_jitter_us || ts_overflowed(ts)) {
254 /* We are getting new data - Send existing data to ring buffer */
255 usb_send_frames_to_usb();
256 } else { /* same timestamp */
257 bool send = false;
258
259 if (is_left && decoded_sdu.left_frames_cnt > decoded_sdu.right_frames_cnt) {
260 /* We are receiving left again before a right, send to USB */
261 send = true;
262 } else if (is_right && decoded_sdu.right_frames_cnt > decoded_sdu.left_frames_cnt) {
263 /* We are receiving right again before a left, send to USB */
264 send = true;
265 } else if (is_mono) {
266 /* always send mono as it comes */
267 send = true;
268 }
269
270 if (send) {
271 usb_send_frames_to_usb();
272 }
273 }
274
275 if (is_left) {
276 if (decoded_sdu.left_frames_cnt >= ARRAY_SIZE(decoded_sdu.left_frames)) {
277 LOG_WRN("Could not add more left frames");
278
279 return -ENOMEM;
280 }
281
282 memcpy(decoded_sdu.left_frames[decoded_sdu.left_frames_cnt++], frame, frame_size);
283 } else if (is_right) {
284 if (decoded_sdu.right_frames_cnt >= ARRAY_SIZE(decoded_sdu.right_frames)) {
285 LOG_WRN("Could not add more right frames");
286
287 return -ENOMEM;
288 }
289
290 memcpy(decoded_sdu.right_frames[decoded_sdu.right_frames_cnt++], frame, frame_size);
291 } else if (is_mono) {
292 /* Use left as mono*/
293 if (decoded_sdu.mono_frames_cnt >= ARRAY_SIZE(decoded_sdu.left_frames)) {
294 LOG_WRN("Could not add more mono frames");
295
296 return -ENOMEM;
297 }
298
299 memcpy(decoded_sdu.left_frames[decoded_sdu.mono_frames_cnt++], frame, frame_size);
300 } else {
301 /* Unsupported channel */
302 LOG_DBG("Unsupported channel %d", chan_allocation);
303
304 return -EINVAL;
305 }
306
307 decoded_sdu.ts = ts;
308
309 return 0;
310 }
311
usb_clear_frames_to_usb(void)312 void usb_clear_frames_to_usb(void)
313 {
314 decoded_sdu.mono_frames_cnt = 0U;
315 decoded_sdu.right_frames_cnt = 0U;
316 decoded_sdu.left_frames_cnt = 0U;
317 decoded_sdu.ts = 0U;
318 }
319
usb_init(void)320 int usb_init(void)
321 {
322 const struct device *hs_dev = DEVICE_DT_GET(DT_NODELABEL(hs_0));
323 static const struct usb_audio_ops usb_ops = {
324 .data_request_cb = usb_data_request_cb,
325 .data_written_cb = usb_data_written_cb,
326 };
327 static bool initialized;
328 int err;
329
330 if (initialized) {
331 return -EALREADY;
332 }
333
334 if (!device_is_ready(hs_dev)) {
335 LOG_ERR("Cannot get USB Headset Device");
336 return -EIO;
337 }
338
339 usb_audio_register(hs_dev, &usb_ops);
340 err = usb_enable(NULL);
341 if (err != 0) {
342 LOG_ERR("Failed to enable USB");
343 return err;
344 }
345
346 LOG_INF("USB initialized");
347 initialized = true;
348
349 return 0;
350 }
351
