1 /**
2 * @file
3 * @brief Bluetooth Basic Audio Profile shell USB extension
4 *
5 * This files handles all the USB related functionality to audio in/out for the BAP shell
6 *
7 * Copyright (c) 2024 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 #if defined(CONFIG_SOC_NRF5340_CPUAPP)
35 #include <nrfx_clock.h>
36 #include <drivers/nrfx_errors.h>
37 #include <hal/nrf_clock.h>
38 #endif /* CONFIG_SOC_NRF5340_CPUAPP */
39
40 #include "audio.h"
41
42 LOG_MODULE_REGISTER(bap_usb, CONFIG_BT_BAP_STREAM_LOG_LEVEL);
43
44 #define USB_ENQUEUE_COUNT 30U /* 30ms */
45 #define USB_FRAME_DURATION_US 1000U
46 #define USB_SAMPLE_CNT ((USB_FRAME_DURATION_US * USB_SAMPLE_RATE) / USEC_PER_SEC)
47 #define USB_BYTES_PER_SAMPLE sizeof(int16_t)
48 #define USB_MONO_FRAME_SIZE (USB_SAMPLE_CNT * USB_BYTES_PER_SAMPLE)
49 #define USB_CHANNELS 2U
50 #define USB_STEREO_FRAME_SIZE (USB_MONO_FRAME_SIZE * USB_CHANNELS)
51 #define USB_OUT_RING_BUF_SIZE (CONFIG_BT_ISO_RX_BUF_COUNT * LC3_MAX_NUM_SAMPLES_STEREO)
52 #define USB_IN_RING_BUF_SIZE (USB_MONO_FRAME_SIZE * USB_ENQUEUE_COUNT)
53
54 #if defined CONFIG_BT_AUDIO_RX
55 struct decoded_sdu {
56 int16_t right_frames[MAX_CODEC_FRAMES_PER_SDU][LC3_MAX_NUM_SAMPLES_MONO];
57 int16_t left_frames[MAX_CODEC_FRAMES_PER_SDU][LC3_MAX_NUM_SAMPLES_MONO];
58 size_t right_frames_cnt;
59 size_t left_frames_cnt;
60 size_t mono_frames_cnt;
61 uint32_t ts;
62 } decoded_sdu;
63
64 RING_BUF_DECLARE(usb_out_ring_buf, USB_OUT_RING_BUF_SIZE);
65 NET_BUF_POOL_DEFINE(usb_out_buf_pool, USB_ENQUEUE_COUNT, USB_STEREO_FRAME_SIZE, 0, net_buf_destroy);
66
67 /* USB consumer callback, called every 1ms, consumes data from ring-buffer */
usb_data_request_cb(const struct device * dev)68 static void usb_data_request_cb(const struct device *dev)
69 {
70 uint8_t usb_audio_data[USB_STEREO_FRAME_SIZE] = {0};
71 struct net_buf *pcm_buf;
72 uint32_t size;
73 int err;
74
75 if (bap_get_rx_streaming_cnt() == 0) {
76 /* no-op as we have no streams that receive data */
77 return;
78 }
79
80 pcm_buf = net_buf_alloc(&usb_out_buf_pool, K_NO_WAIT);
81 if (pcm_buf == NULL) {
82 LOG_WRN("Could not allocate pcm_buf");
83 return;
84 }
85
86 /* This may fail without causing issues since usb_audio_data is 0-initialized */
87 size = ring_buf_get(&usb_out_ring_buf, usb_audio_data, sizeof(usb_audio_data));
88
89 net_buf_add_mem(pcm_buf, usb_audio_data, sizeof(usb_audio_data));
90
91 if (size != 0) {
92 static size_t cnt;
93
94 if ((++cnt % bap_get_stats_interval()) == 0U) {
95 LOG_INF("[%zu]: Sending USB audio", cnt);
96 }
97 } else {
98 static size_t cnt;
99
100 if ((++cnt % bap_get_stats_interval()) == 0U) {
101 LOG_INF("[%zu]: Sending empty USB audio", cnt);
102 }
103 }
104
105 err = usb_audio_send(dev, pcm_buf, sizeof(usb_audio_data));
106 if (err != 0) {
107 static size_t cnt;
108
109 cnt++;
110 if ((cnt % 1000) == 0) {
111 LOG_ERR("Failed to send USB audio: %d (%zu)", err, cnt);
112 }
113
114 net_buf_unref(pcm_buf);
115 }
116 }
117
usb_data_written_cb(const struct device * dev,struct net_buf * buf,size_t size)118 static void usb_data_written_cb(const struct device *dev, struct net_buf *buf, size_t size)
119 {
120 /* Unreference the buffer now that the USB is done with it */
121 net_buf_unref(buf);
122 }
123
bap_usb_send_frames_to_usb(void)124 static void bap_usb_send_frames_to_usb(void)
125 {
126 const bool is_left_only =
127 decoded_sdu.right_frames_cnt == 0U && decoded_sdu.mono_frames_cnt == 0U;
128 const bool is_right_only =
129 decoded_sdu.left_frames_cnt == 0U && decoded_sdu.mono_frames_cnt == 0U;
130 const bool is_mono_only =
131 decoded_sdu.left_frames_cnt == 0U && decoded_sdu.right_frames_cnt == 0U;
132 const bool is_single_channel = is_left_only || is_right_only || is_mono_only;
133 const size_t frame_cnt =
134 MAX(decoded_sdu.mono_frames_cnt,
135 MAX(decoded_sdu.left_frames_cnt, decoded_sdu.right_frames_cnt));
136 static size_t cnt;
137
138 /* Send frames to USB - If we only have a single channel we mix it to stereo */
139 for (size_t i = 0U; i < frame_cnt; i++) {
140 static int16_t stereo_frame[LC3_MAX_NUM_SAMPLES_STEREO];
141 const int16_t *right_frame = decoded_sdu.right_frames[i];
142 const int16_t *left_frame = decoded_sdu.left_frames[i];
143 const int16_t *mono_frame = decoded_sdu.left_frames[i]; /* use left as mono */
144 static size_t fail_cnt;
145 uint32_t rb_size;
146
147 /* Not enough space to store data */
148 if (ring_buf_space_get(&usb_out_ring_buf) < sizeof(stereo_frame)) {
149 if ((fail_cnt % bap_get_stats_interval()) == 0U) {
150 LOG_WRN("[%zu] Could not send more than %zu frames to USB",
151 fail_cnt, i);
152 }
153
154 fail_cnt++;
155
156 break;
157 }
158
159 fail_cnt = 0U;
160
161 /* Generate the stereo frame
162 *
163 * If we only have single channel then we mix that to stereo
164 */
165 for (int j = 0; j < LC3_MAX_NUM_SAMPLES_MONO; j++) {
166 if (is_single_channel) {
167 int16_t sample = 0;
168
169 /* Mix to stereo as LRLRLRLR */
170 if (is_left_only) {
171 sample = left_frame[j];
172 } else if (is_right_only) {
173 sample = right_frame[j];
174 } else if (is_mono_only) {
175 sample = mono_frame[j];
176 }
177
178 stereo_frame[j * 2] = sample;
179 stereo_frame[j * 2 + 1] = sample;
180 } else {
181 stereo_frame[j * 2] = left_frame[j];
182 stereo_frame[j * 2 + 1] = right_frame[j];
183 }
184 }
185
186 rb_size = ring_buf_put(&usb_out_ring_buf, (uint8_t *)stereo_frame,
187 sizeof(stereo_frame));
188 if (rb_size != sizeof(stereo_frame)) {
189 LOG_WRN("Failed to put frame on USB ring buf");
190
191 break;
192 }
193 }
194
195 if ((++cnt % bap_get_stats_interval()) == 0U) {
196 LOG_INF("[%zu]: Sending %u USB audio frame", cnt, frame_cnt);
197 }
198
199 bap_usb_clear_frames_to_usb();
200 }
201
ts_overflowed(uint32_t ts)202 static bool ts_overflowed(uint32_t ts)
203 {
204 /* If the timestamp is a factor of 10 in difference, then we assume that TS overflowed
205 * We cannot simply check if `ts < decoded_sdu.ts` as that could also indicate old data
206 */
207 return ((uint64_t)ts * 10 < decoded_sdu.ts);
208 }
209
bap_usb_add_frame_to_usb(enum bt_audio_location chan_allocation,const int16_t * frame,size_t frame_size,uint32_t ts)210 int bap_usb_add_frame_to_usb(enum bt_audio_location chan_allocation, const int16_t *frame,
211 size_t frame_size, uint32_t ts)
212 {
213 const bool is_left = (chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
214 const bool is_right = (chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
215 const bool is_mono = chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
216 const uint8_t ts_jitter_us = 100; /* timestamps may have jitter */
217
218 static size_t cnt;
219
220 if ((++cnt % bap_get_stats_interval()) == 0U) {
221 LOG_INF("[%zu]: Adding USB audio frame", cnt);
222 }
223
224 if (frame_size > LC3_MAX_NUM_SAMPLES_MONO * sizeof(int16_t) || frame_size == 0U) {
225 LOG_DBG("Invalid frame of size %zu", frame_size);
226
227 return -EINVAL;
228 }
229
230 if (bt_audio_get_chan_count(chan_allocation) != 1) {
231 LOG_DBG("Invalid channel allocation %d", chan_allocation);
232
233 return -EINVAL;
234 }
235
236 if (((is_left || is_right) && decoded_sdu.mono_frames_cnt != 0) ||
237 (is_mono &&
238 (decoded_sdu.left_frames_cnt != 0U || decoded_sdu.right_frames_cnt != 0U))) {
239 LOG_DBG("Cannot mix and match mono with left or right");
240
241 return -EINVAL;
242 }
243
244 /* Check if the frame can be combined with a previous frame from another channel, of if
245 * we have to send previous data to USB and then store the current frame
246 *
247 * This is done by comparing the timestamps of the frames, and in the case that they are the
248 * same, there are additional checks to see if we have received more left than right frames,
249 * in which case we also send existing data
250 */
251
252 if (ts + ts_jitter_us < decoded_sdu.ts && !ts_overflowed(ts)) {
253 /* Old data, discard */
254 return -ENOEXEC;
255 } else if (ts > decoded_sdu.ts + ts_jitter_us || ts_overflowed(ts)) {
256 /* We are getting new data - Send existing data to ring buffer */
257 bap_usb_send_frames_to_usb();
258 } else { /* same timestamp */
259 bool send = false;
260
261 if (is_left && decoded_sdu.left_frames_cnt > decoded_sdu.right_frames_cnt) {
262 /* We are receiving left again before a right, send to USB */
263 send = true;
264 } else if (is_right && decoded_sdu.right_frames_cnt > decoded_sdu.left_frames_cnt) {
265 /* We are receiving right again before a left, send to USB */
266 send = true;
267 } else if (is_mono) {
268 /* always send mono as it comes */
269 send = true;
270 }
271
272 if (send) {
273 bap_usb_send_frames_to_usb();
274 }
275 }
276
277 if (is_left) {
278 if (decoded_sdu.left_frames_cnt >= ARRAY_SIZE(decoded_sdu.left_frames)) {
279 LOG_WRN("Could not add more left frames");
280
281 return -ENOMEM;
282 }
283
284 memcpy(decoded_sdu.left_frames[decoded_sdu.left_frames_cnt++], frame, frame_size);
285 } else if (is_right) {
286 if (decoded_sdu.right_frames_cnt >= ARRAY_SIZE(decoded_sdu.right_frames)) {
287 LOG_WRN("Could not add more right frames");
288
289 return -ENOMEM;
290 }
291
292 memcpy(decoded_sdu.right_frames[decoded_sdu.right_frames_cnt++], frame, frame_size);
293 } else if (is_mono) {
294 /* Use left as mono*/
295 if (decoded_sdu.mono_frames_cnt >= ARRAY_SIZE(decoded_sdu.left_frames)) {
296 LOG_WRN("Could not add more mono frames");
297
298 return -ENOMEM;
299 }
300
301 memcpy(decoded_sdu.left_frames[decoded_sdu.mono_frames_cnt++], frame, frame_size);
302 } else {
303 /* Unsupported channel */
304 LOG_DBG("Unsupported channel %d", chan_allocation);
305
306 return -EINVAL;
307 }
308
309 decoded_sdu.ts = ts;
310
311 return 0;
312 }
313
bap_usb_clear_frames_to_usb(void)314 void bap_usb_clear_frames_to_usb(void)
315 {
316 decoded_sdu.mono_frames_cnt = 0U;
317 decoded_sdu.right_frames_cnt = 0U;
318 decoded_sdu.left_frames_cnt = 0U;
319 decoded_sdu.ts = 0U;
320 }
321 #endif /* CONFIG_BT_AUDIO_RX */
322
323 #if defined(CONFIG_BT_AUDIO_TX)
324 BUILD_ASSERT((USB_IN_RING_BUF_SIZE % USB_MONO_FRAME_SIZE) == 0);
325 static int16_t usb_in_left_ring_buffer[USB_IN_RING_BUF_SIZE];
326 static int16_t usb_in_right_ring_buffer[USB_IN_RING_BUF_SIZE];
327 static size_t write_index; /* Points to the oldest/uninitialized data */
328
bap_usb_get_read_cnt(const struct shell_stream * sh_stream)329 size_t bap_usb_get_read_cnt(const struct shell_stream *sh_stream)
330 {
331 return (USB_SAMPLE_CNT * sh_stream->lc3_frame_duration_us) / USEC_PER_MSEC;
332 }
333
bap_usb_get_frame_size(const struct shell_stream * sh_stream)334 size_t bap_usb_get_frame_size(const struct shell_stream *sh_stream)
335 {
336 return USB_BYTES_PER_SAMPLE * bap_usb_get_read_cnt(sh_stream);
337 }
338
stream_cb(struct shell_stream * sh_stream,void * user_data)339 static void stream_cb(struct shell_stream *sh_stream, void *user_data)
340 {
341 if (sh_stream->is_tx) {
342 const bool has_left =
343 (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
344 const bool has_right =
345 (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
346 const bool has_stereo = has_right && has_left;
347 const bool is_mono = sh_stream->lc3_chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
348 const size_t old_write_index = POINTER_TO_UINT(user_data);
349 const bool overflowed = write_index < old_write_index;
350 size_t read_idx;
351
352 if (has_stereo) {
353 /* These should always be the same */
354 read_idx = MIN(sh_stream->tx.left_read_idx, sh_stream->tx.right_read_idx);
355 } else if (has_left || is_mono) {
356 read_idx = sh_stream->tx.left_read_idx;
357 } else if (has_right) {
358 read_idx = sh_stream->tx.right_read_idx;
359 } else {
360 /* Not a valid USB stream */
361 return;
362 }
363
364 /* If we are overwriting data that the stream is currently pointing to, then we
365 * need to update the index so that the stream will point to the oldest valid data
366 */
367 if (read_idx > old_write_index) {
368 if (read_idx < write_index || (overflowed && read_idx < write_index)) {
369 sh_stream->tx.left_read_idx = write_index;
370 sh_stream->tx.right_read_idx = write_index;
371 }
372 }
373 }
374 }
375
usb_data_received_cb(const struct device * dev,struct net_buf * buf,size_t size)376 static void usb_data_received_cb(const struct device *dev, struct net_buf *buf, size_t size)
377 {
378 const size_t old_write_index = write_index;
379 static size_t cnt;
380 int16_t *pcm;
381
382 if (buf == NULL) {
383 return;
384 }
385
386 if (size != USB_STEREO_FRAME_SIZE) {
387 net_buf_unref(buf);
388
389 return;
390 }
391
392 pcm = (int16_t *)buf->data;
393
394 /* Split the data into left and right as LC3 uses LLLLRRRR instead of LRLRLRLR as USB
395 *
396 * Since the left and right buffer sizes are a factor of USB_SAMPLE_CNT, then we can always
397 * add USB_SAMPLE_CNT in a single go without needing to check the remaining size as that
398 * can be done once afterwards
399 */
400 for (size_t i = 0U, j = 0U; i < USB_SAMPLE_CNT; i++, j += USB_CHANNELS) {
401 usb_in_left_ring_buffer[write_index + i] = pcm[j];
402 usb_in_right_ring_buffer[write_index + i] = pcm[j + 1];
403 }
404
405 write_index += USB_SAMPLE_CNT;
406
407 if (write_index == USB_IN_RING_BUF_SIZE) {
408 /* Overflow so that we start overwriting oldest */
409 write_index = 0U;
410 }
411
412 /* Update the read pointers of each stream to ensure that the new write index is not larger
413 * than their read indexes
414 */
415 bap_foreach_stream(stream_cb, UINT_TO_POINTER(old_write_index));
416
417 if ((++cnt % bap_get_stats_interval()) == 0U) {
418 LOG_DBG("USB Data received (count = %d)", cnt);
419 }
420
421 net_buf_unref(buf);
422 }
423
bap_usb_can_get_full_sdu(struct shell_stream * sh_stream)424 bool bap_usb_can_get_full_sdu(struct shell_stream *sh_stream)
425 {
426 const bool has_left = (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
427 const bool has_right =
428 (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
429 const bool has_stereo = has_right && has_left;
430 const bool is_mono = sh_stream->lc3_chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
431 const uint32_t read_cnt = bap_usb_get_read_cnt(sh_stream);
432 const uint32_t retrieve_cnt = read_cnt * sh_stream->lc3_frame_blocks_per_sdu;
433 static bool failed_last_time;
434 size_t read_idx;
435 size_t buffer_cnt;
436
437 if (has_stereo) {
438 /* These should always be the same */
439 read_idx = MIN(sh_stream->tx.left_read_idx, sh_stream->tx.right_read_idx);
440 } else if (has_left || is_mono) {
441 read_idx = sh_stream->tx.left_read_idx;
442 } else if (has_right) {
443 read_idx = sh_stream->tx.right_read_idx;
444 } else {
445 return false;
446 }
447
448 if (read_idx <= write_index) {
449 buffer_cnt = write_index - read_idx;
450 } else {
451 /* Handle the case where the read spans across the end of the buffer */
452 buffer_cnt = write_index + (USB_IN_RING_BUF_SIZE - read_idx);
453 }
454
455 if (buffer_cnt < retrieve_cnt) {
456 /* Not enough for a frame yet */
457 if (!failed_last_time) {
458 LOG_WRN("Ring buffer (%u/%u) does not contain enough for an entire SDU %u",
459 buffer_cnt, USB_IN_RING_BUF_SIZE, retrieve_cnt);
460 }
461
462 failed_last_time = true;
463
464 return false;
465 }
466
467 failed_last_time = false;
468
469 return true;
470 }
471
472 /**
473 * Reads @p size octets from src, handling wrapping and returns the new idx
474 * (which is lower than @p idx in the case of wrapping)
475 *
476 * bap_usb_can_get_full_sdu should always be called before this to ensure that we are getting
477 * valid data
478 */
usb_ring_buf_get(int16_t dest[],int16_t src[],size_t idx,size_t cnt)479 static size_t usb_ring_buf_get(int16_t dest[], int16_t src[], size_t idx, size_t cnt)
480 {
481 size_t new_idx;
482
483 if (idx >= USB_IN_RING_BUF_SIZE) {
484 LOG_ERR("Invalid idx %zu", idx);
485
486 return 0;
487 }
488
489 if ((idx + cnt) < USB_IN_RING_BUF_SIZE) {
490 /* Simply copy of the data and increment the index*/
491 memcpy(dest, &src[idx], cnt * USB_BYTES_PER_SAMPLE);
492 new_idx = idx + cnt;
493 } else {
494 /* Handle wrapping */
495 const size_t first_read_cnt = USB_IN_RING_BUF_SIZE - idx;
496 const size_t second_read_cnt = cnt - first_read_cnt;
497
498 memcpy(dest, &src[idx], first_read_cnt * USB_BYTES_PER_SAMPLE);
499 memcpy(&dest[first_read_cnt], &src[0], second_read_cnt * USB_BYTES_PER_SAMPLE);
500
501 new_idx = second_read_cnt;
502 }
503
504 return new_idx;
505 }
506
bap_usb_get_frame(struct shell_stream * sh_stream,enum bt_audio_location chan_alloc,int16_t buffer[])507 void bap_usb_get_frame(struct shell_stream *sh_stream, enum bt_audio_location chan_alloc,
508 int16_t buffer[])
509 {
510 const bool is_left = (chan_alloc & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
511 const bool is_right = (chan_alloc & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
512 const bool is_mono = chan_alloc == BT_AUDIO_LOCATION_MONO_AUDIO;
513 const uint32_t read_cnt = bap_usb_get_read_cnt(sh_stream);
514
515 if (is_left || is_mono) {
516 sh_stream->tx.left_read_idx = usb_ring_buf_get(
517 buffer, usb_in_left_ring_buffer, sh_stream->tx.left_read_idx, read_cnt);
518 } else if (is_right) {
519 sh_stream->tx.right_read_idx = usb_ring_buf_get(
520 buffer, usb_in_right_ring_buffer, sh_stream->tx.right_read_idx, read_cnt);
521 }
522 }
523 #endif /* CONFIG_BT_AUDIO_TX */
524
bap_usb_init(void)525 int bap_usb_init(void)
526 {
527 const struct device *hs_dev = DEVICE_DT_GET(DT_NODELABEL(hs_0));
528 static const struct usb_audio_ops usb_ops = {
529 #if defined(CONFIG_BT_AUDIO_RX)
530 .data_request_cb = usb_data_request_cb,
531 .data_written_cb = usb_data_written_cb,
532 #endif /* CONFIG_BT_AUDIO_RX */
533 #if defined(CONFIG_BT_AUDIO_TX)
534 .data_received_cb = usb_data_received_cb,
535 #endif /* CONFIG_BT_AUDIO_TX */
536 };
537 int err;
538
539 if (!device_is_ready(hs_dev)) {
540 LOG_ERR("Cannot get USB Headset Device");
541 return -EIO;
542 }
543
544 usb_audio_register(hs_dev, &usb_ops);
545 err = usb_enable(NULL);
546 if (err != 0) {
547 LOG_ERR("Failed to enable USB");
548 return err;
549 }
550
551 if (IS_ENABLED(CONFIG_SOC_NRF5340_CPUAPP)) {
552 /* Use this to turn on 128 MHz clock for the nRF5340 cpu_app
553 * This may not be required, but reduces the risk of not decoding fast enough
554 * to keep up with USB
555 */
556 err = nrfx_clock_divider_set(NRF_CLOCK_DOMAIN_HFCLK, NRF_CLOCK_HFCLK_DIV_1);
557
558 err -= NRFX_ERROR_BASE_NUM;
559 if (err != 0) {
560 LOG_WRN("Failed to set 128 MHz: %d", err);
561 }
562 }
563
564 return 0;
565 }
566
