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 #endif /* CONFIG_SOC_NRF5340_CPUAPP */
37
38 #include "audio.h"
39
40 LOG_MODULE_REGISTER(bap_usb, CONFIG_BT_BAP_STREAM_LOG_LEVEL);
41
42 #define USB_ENQUEUE_COUNT 30U /* 30ms */
43 #define USB_FRAME_DURATION_US 1000U
44 #define USB_SAMPLE_CNT ((USB_FRAME_DURATION_US * USB_SAMPLE_RATE) / USEC_PER_SEC)
45 #define USB_BYTES_PER_SAMPLE sizeof(int16_t)
46 #define USB_MONO_FRAME_SIZE (USB_SAMPLE_CNT * USB_BYTES_PER_SAMPLE)
47 #define USB_CHANNELS 2U
48 #define USB_STEREO_FRAME_SIZE (USB_MONO_FRAME_SIZE * USB_CHANNELS)
49 #define USB_OUT_RING_BUF_SIZE (CONFIG_BT_ISO_RX_BUF_COUNT * LC3_MAX_NUM_SAMPLES_STEREO)
50 #define USB_IN_RING_BUF_SIZE (USB_MONO_FRAME_SIZE * USB_ENQUEUE_COUNT)
51
52 #if defined CONFIG_BT_AUDIO_RX
53 struct decoded_sdu {
54 int16_t right_frames[MAX_CODEC_FRAMES_PER_SDU][LC3_MAX_NUM_SAMPLES_MONO];
55 int16_t left_frames[MAX_CODEC_FRAMES_PER_SDU][LC3_MAX_NUM_SAMPLES_MONO];
56 size_t right_frames_cnt;
57 size_t left_frames_cnt;
58 size_t mono_frames_cnt;
59 uint32_t ts;
60 } decoded_sdu;
61
62 RING_BUF_DECLARE(usb_out_ring_buf, USB_OUT_RING_BUF_SIZE);
63 NET_BUF_POOL_DEFINE(usb_out_buf_pool, USB_ENQUEUE_COUNT, USB_STEREO_FRAME_SIZE, 0, net_buf_destroy);
64
65 /* USB consumer callback, called every 1ms, consumes data from ring-buffer */
usb_data_request_cb(const struct device * dev)66 static void usb_data_request_cb(const struct device *dev)
67 {
68 uint8_t usb_audio_data[USB_STEREO_FRAME_SIZE] = {0};
69 struct net_buf *pcm_buf;
70 uint32_t size;
71 int err;
72
73 if (bap_get_rx_streaming_cnt() == 0) {
74 /* no-op as we have no streams that receive data */
75 return;
76 }
77
78 pcm_buf = net_buf_alloc(&usb_out_buf_pool, K_NO_WAIT);
79 if (pcm_buf == NULL) {
80 LOG_WRN("Could not allocate pcm_buf");
81 return;
82 }
83
84 /* This may fail without causing issues since usb_audio_data is 0-initialized */
85 size = ring_buf_get(&usb_out_ring_buf, usb_audio_data, sizeof(usb_audio_data));
86
87 net_buf_add_mem(pcm_buf, usb_audio_data, sizeof(usb_audio_data));
88
89 if (size != 0) {
90 static size_t cnt;
91
92 if ((++cnt % bap_get_stats_interval()) == 0U) {
93 LOG_INF("[%zu]: Sending USB audio", cnt);
94 }
95 } else {
96 static size_t cnt;
97
98 if ((++cnt % bap_get_stats_interval()) == 0U) {
99 LOG_INF("[%zu]: Sending empty USB audio", cnt);
100 }
101 }
102
103 err = usb_audio_send(dev, pcm_buf, sizeof(usb_audio_data));
104 if (err != 0) {
105 static size_t cnt;
106
107 cnt++;
108 if ((cnt % 1000) == 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
bap_usb_send_frames_to_usb(void)122 static void bap_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 ((fail_cnt % bap_get_stats_interval()) == 0U) {
148 LOG_WRN("[%zu] Could not send more than %zu frames to USB",
149 fail_cnt, i);
150 }
151
152 fail_cnt++;
153
154 break;
155 }
156
157 fail_cnt = 0U;
158
159 /* Generate the stereo frame
160 *
161 * If we only have single channel then we mix that to stereo
162 */
163 for (int j = 0; j < LC3_MAX_NUM_SAMPLES_MONO; j++) {
164 if (is_single_channel) {
165 int16_t sample = 0;
166
167 /* Mix to stereo as LRLRLRLR */
168 if (is_left_only) {
169 sample = left_frame[j];
170 } else if (is_right_only) {
171 sample = right_frame[j];
172 } else if (is_mono_only) {
173 sample = mono_frame[j];
174 }
175
176 stereo_frame[j * 2] = sample;
177 stereo_frame[j * 2 + 1] = sample;
178 } else {
179 stereo_frame[j * 2] = left_frame[j];
180 stereo_frame[j * 2 + 1] = right_frame[j];
181 }
182 }
183
184 rb_size = ring_buf_put(&usb_out_ring_buf, (uint8_t *)stereo_frame,
185 sizeof(stereo_frame));
186 if (rb_size != sizeof(stereo_frame)) {
187 LOG_WRN("Failed to put frame on USB ring buf");
188
189 break;
190 }
191 }
192
193 if ((++cnt % bap_get_stats_interval()) == 0U) {
194 LOG_INF("[%zu]: Sending %u USB audio frame", cnt, frame_cnt);
195 }
196
197 bap_usb_clear_frames_to_usb();
198 }
199
ts_overflowed(uint32_t ts)200 static bool ts_overflowed(uint32_t ts)
201 {
202 /* If the timestamp is a factor of 10 in difference, then we assume that TS overflowed
203 * We cannot simply check if `ts < decoded_sdu.ts` as that could also indicate old data
204 */
205 return ((uint64_t)ts * 10 < decoded_sdu.ts);
206 }
207
bap_usb_add_frame_to_usb(enum bt_audio_location chan_allocation,const int16_t * frame,size_t frame_size,uint32_t ts)208 int bap_usb_add_frame_to_usb(enum bt_audio_location chan_allocation, const int16_t *frame,
209 size_t frame_size, uint32_t ts)
210 {
211 const bool is_left = (chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
212 const bool is_right = (chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
213 const bool is_mono = chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
214 const uint8_t ts_jitter_us = 100; /* timestamps may have jitter */
215
216 static size_t cnt;
217
218 if ((++cnt % bap_get_stats_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 bap_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 bap_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
bap_usb_clear_frames_to_usb(void)312 void bap_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 #endif /* CONFIG_BT_AUDIO_RX */
320
321 #if defined(CONFIG_BT_AUDIO_TX)
322 BUILD_ASSERT((USB_IN_RING_BUF_SIZE % USB_MONO_FRAME_SIZE) == 0);
323 static int16_t usb_in_left_ring_buffer[USB_IN_RING_BUF_SIZE];
324 static int16_t usb_in_right_ring_buffer[USB_IN_RING_BUF_SIZE];
325 static size_t write_index; /* Points to the oldest/uninitialized data */
326
bap_usb_get_read_cnt(const struct shell_stream * sh_stream)327 size_t bap_usb_get_read_cnt(const struct shell_stream *sh_stream)
328 {
329 return (USB_SAMPLE_CNT * sh_stream->lc3_frame_duration_us) / USEC_PER_MSEC;
330 }
331
bap_usb_get_frame_size(const struct shell_stream * sh_stream)332 size_t bap_usb_get_frame_size(const struct shell_stream *sh_stream)
333 {
334 return USB_BYTES_PER_SAMPLE * bap_usb_get_read_cnt(sh_stream);
335 }
336
stream_cb(struct shell_stream * sh_stream,void * user_data)337 static void stream_cb(struct shell_stream *sh_stream, void *user_data)
338 {
339 if (sh_stream->is_tx) {
340 const bool has_left =
341 (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
342 const bool has_right =
343 (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
344 const bool has_stereo = has_right && has_left;
345 const bool is_mono = sh_stream->lc3_chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
346 const size_t old_write_index = POINTER_TO_UINT(user_data);
347 const bool overflowed = write_index < old_write_index;
348 size_t read_idx;
349
350 if (has_stereo) {
351 /* These should always be the same */
352 read_idx = MIN(sh_stream->tx.left_read_idx, sh_stream->tx.right_read_idx);
353 } else if (has_left || is_mono) {
354 read_idx = sh_stream->tx.left_read_idx;
355 } else if (has_right) {
356 read_idx = sh_stream->tx.right_read_idx;
357 } else {
358 /* Not a valid USB stream */
359 return;
360 }
361
362 /* If we are overwriting data that the stream is currently pointing to, then we
363 * need to update the index so that the stream will point to the oldest valid data
364 */
365 if (read_idx > old_write_index) {
366 if (read_idx < write_index || (overflowed && read_idx < write_index)) {
367 sh_stream->tx.left_read_idx = write_index;
368 sh_stream->tx.right_read_idx = write_index;
369 }
370 }
371 }
372 }
373
usb_data_received_cb(const struct device * dev,struct net_buf * buf,size_t size)374 static void usb_data_received_cb(const struct device *dev, struct net_buf *buf, size_t size)
375 {
376 const size_t old_write_index = write_index;
377 static size_t cnt;
378 int16_t *pcm;
379
380 if (buf == NULL) {
381 return;
382 }
383
384 if (size != USB_STEREO_FRAME_SIZE) {
385 net_buf_unref(buf);
386
387 return;
388 }
389
390 pcm = (int16_t *)buf->data;
391
392 /* Split the data into left and right as LC3 uses LLLLRRRR instead of LRLRLRLR as USB
393 *
394 * Since the left and right buffer sizes are a factor of USB_SAMPLE_CNT, then we can always
395 * add USB_SAMPLE_CNT in a single go without needing to check the remaining size as that
396 * can be done once afterwards
397 */
398 for (size_t i = 0U, j = 0U; i < USB_SAMPLE_CNT; i++, j += USB_CHANNELS) {
399 usb_in_left_ring_buffer[write_index + i] = pcm[j];
400 usb_in_right_ring_buffer[write_index + i] = pcm[j + 1];
401 }
402
403 write_index += USB_SAMPLE_CNT;
404
405 if (write_index == USB_IN_RING_BUF_SIZE) {
406 /* Overflow so that we start overwriting oldest */
407 write_index = 0U;
408 }
409
410 /* Update the read pointers of each stream to ensure that the new write index is not larger
411 * than their read indexes
412 */
413 bap_foreach_stream(stream_cb, UINT_TO_POINTER(old_write_index));
414
415 if ((++cnt % bap_get_stats_interval()) == 0U) {
416 LOG_DBG("USB Data received (count = %d)", cnt);
417 }
418
419 net_buf_unref(buf);
420 }
421
bap_usb_can_get_full_sdu(struct shell_stream * sh_stream)422 bool bap_usb_can_get_full_sdu(struct shell_stream *sh_stream)
423 {
424 const bool has_left = (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
425 const bool has_right =
426 (sh_stream->lc3_chan_allocation & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
427 const bool has_stereo = has_right && has_left;
428 const bool is_mono = sh_stream->lc3_chan_allocation == BT_AUDIO_LOCATION_MONO_AUDIO;
429 const uint32_t read_cnt = bap_usb_get_read_cnt(sh_stream);
430 const uint32_t retrieve_cnt = read_cnt * sh_stream->lc3_frame_blocks_per_sdu;
431 static bool failed_last_time;
432 size_t read_idx;
433 size_t buffer_cnt;
434
435 if (has_stereo) {
436 /* These should always be the same */
437 read_idx = MIN(sh_stream->tx.left_read_idx, sh_stream->tx.right_read_idx);
438 } else if (has_left || is_mono) {
439 read_idx = sh_stream->tx.left_read_idx;
440 } else if (has_right) {
441 read_idx = sh_stream->tx.right_read_idx;
442 } else {
443 return false;
444 }
445
446 if (read_idx <= write_index) {
447 buffer_cnt = write_index - read_idx;
448 } else {
449 /* Handle the case where the read spans across the end of the buffer */
450 buffer_cnt = write_index + (USB_IN_RING_BUF_SIZE - read_idx);
451 }
452
453 if (buffer_cnt < retrieve_cnt) {
454 /* Not enough for a frame yet */
455 if (!failed_last_time) {
456 LOG_WRN("Ring buffer (%u/%u) does not contain enough for an entire SDU %u",
457 buffer_cnt, USB_IN_RING_BUF_SIZE, retrieve_cnt);
458 }
459
460 failed_last_time = true;
461
462 return false;
463 }
464
465 failed_last_time = false;
466
467 return true;
468 }
469
470 /**
471 * Reads @p size octets from src, handling wrapping and returns the new idx
472 * (which is lower than @p idx in the case of wrapping)
473 *
474 * bap_usb_can_get_full_sdu should always be called before this to ensure that we are getting
475 * valid data
476 */
usb_ring_buf_get(int16_t dest[],int16_t src[],size_t idx,size_t cnt)477 static size_t usb_ring_buf_get(int16_t dest[], int16_t src[], size_t idx, size_t cnt)
478 {
479 size_t new_idx;
480
481 if (idx >= USB_IN_RING_BUF_SIZE) {
482 LOG_ERR("Invalid idx %zu", idx);
483
484 return 0;
485 }
486
487 if ((idx + cnt) < USB_IN_RING_BUF_SIZE) {
488 /* Simply copy of the data and increment the index*/
489 memcpy(dest, &src[idx], cnt * USB_BYTES_PER_SAMPLE);
490 new_idx = idx + cnt;
491 } else {
492 /* Handle wrapping */
493 const size_t first_read_cnt = USB_IN_RING_BUF_SIZE - idx;
494 const size_t second_read_cnt = cnt - first_read_cnt;
495
496 memcpy(dest, &src[idx], first_read_cnt * USB_BYTES_PER_SAMPLE);
497 memcpy(&dest[first_read_cnt], &src[0], second_read_cnt * USB_BYTES_PER_SAMPLE);
498
499 new_idx = second_read_cnt;
500 }
501
502 return new_idx;
503 }
504
bap_usb_get_frame(struct shell_stream * sh_stream,enum bt_audio_location chan_alloc,int16_t buffer[])505 void bap_usb_get_frame(struct shell_stream *sh_stream, enum bt_audio_location chan_alloc,
506 int16_t buffer[])
507 {
508 const bool is_left = (chan_alloc & BT_AUDIO_LOCATION_FRONT_LEFT) != 0;
509 const bool is_right = (chan_alloc & BT_AUDIO_LOCATION_FRONT_RIGHT) != 0;
510 const bool is_mono = chan_alloc == BT_AUDIO_LOCATION_MONO_AUDIO;
511 const uint32_t read_cnt = bap_usb_get_read_cnt(sh_stream);
512
513 if (is_left || is_mono) {
514 sh_stream->tx.left_read_idx = usb_ring_buf_get(
515 buffer, usb_in_left_ring_buffer, sh_stream->tx.left_read_idx, read_cnt);
516 } else if (is_right) {
517 sh_stream->tx.right_read_idx = usb_ring_buf_get(
518 buffer, usb_in_right_ring_buffer, sh_stream->tx.right_read_idx, read_cnt);
519 }
520 }
521 #endif /* CONFIG_BT_AUDIO_TX */
522
bap_usb_init(void)523 int bap_usb_init(void)
524 {
525 const struct device *hs_dev = DEVICE_DT_GET(DT_NODELABEL(hs_0));
526 static const struct usb_audio_ops usb_ops = {
527 #if defined(CONFIG_BT_AUDIO_RX)
528 .data_request_cb = usb_data_request_cb,
529 .data_written_cb = usb_data_written_cb,
530 #endif /* CONFIG_BT_AUDIO_RX */
531 #if defined(CONFIG_BT_AUDIO_TX)
532 .data_received_cb = usb_data_received_cb,
533 #endif /* CONFIG_BT_AUDIO_TX */
534 };
535 int err;
536
537 if (!device_is_ready(hs_dev)) {
538 LOG_ERR("Cannot get USB Headset Device");
539 return -EIO;
540 }
541
542 usb_audio_register(hs_dev, &usb_ops);
543 err = usb_enable(NULL);
544 if (err != 0) {
545 LOG_ERR("Failed to enable USB");
546 return err;
547 }
548
549 if (IS_ENABLED(CONFIG_SOC_NRF5340_CPUAPP)) {
550 /* Use this to turn on 128 MHz clock for the nRF5340 cpu_app
551 * This may not be required, but reduces the risk of not decoding fast enough
552 * to keep up with USB
553 */
554 err = nrfx_clock_divider_set(NRF_CLOCK_DOMAIN_HFCLK, NRF_CLOCK_HFCLK_DIV_1);
555
556 err -= NRFX_ERROR_BASE_NUM;
557 if (err != 0) {
558 LOG_WRN("Failed to set 128 MHz: %d", err);
559 }
560 }
561
562 return 0;
563 }
564
