1 /*
2 * Copyright (c) 2023-2024 Nordic Semiconductor ASA
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <stdint.h>
8 #include <stdlib.h>
9
10 #include <sample_usbd.h>
11 #include "feedback.h"
12
13 #include <zephyr/cache.h>
14 #include <zephyr/device.h>
15 #include <zephyr/usb/usbd.h>
16 #include <zephyr/usb/class/usbd_uac2.h>
17 #include <zephyr/drivers/i2s.h>
18 #include <zephyr/logging/log.h>
19
20 LOG_MODULE_REGISTER(uac2_sample, LOG_LEVEL_INF);
21
22 #define HEADPHONES_OUT_TERMINAL_ID UAC2_ENTITY_ID(DT_NODELABEL(out_terminal))
23 #define MICROPHONE_IN_TERMINAL_ID UAC2_ENTITY_ID(DT_NODELABEL(in_terminal))
24
25 #define SAMPLES_PER_SOF 48
26 #define SAMPLE_FREQUENCY (SAMPLES_PER_SOF * 1000)
27 #define SAMPLE_BIT_WIDTH 16
28 #define NUMBER_OF_CHANNELS 2
29 #define BYTES_PER_SAMPLE DIV_ROUND_UP(SAMPLE_BIT_WIDTH, 8)
30 #define BYTES_PER_SLOT (BYTES_PER_SAMPLE * NUMBER_OF_CHANNELS)
31 #define MIN_BLOCK_SIZE ((SAMPLES_PER_SOF - 1) * BYTES_PER_SLOT)
32 #define BLOCK_SIZE (SAMPLES_PER_SOF * BYTES_PER_SLOT)
33 #define MAX_BLOCK_SIZE ((SAMPLES_PER_SOF + 1) * BYTES_PER_SLOT)
34
35 /* Absolute minimum is 5 TX buffers (1 actively consumed by I2S, 2nd queued as
36 * next buffer, 3rd acquired by USB stack to receive data to, and 2 to handle
37 * SOF/I2S offset errors), but add 2 additional buffers to prevent out of memory
38 * errors when USB host decides to perform rapid terminal enable/disable cycles.
39 */
40 #define I2S_BLOCKS 7
41 K_MEM_SLAB_DEFINE_STATIC(i2s_tx_slab, MAX_BLOCK_SIZE, I2S_BLOCKS, 4);
42 K_MEM_SLAB_DEFINE_STATIC(i2s_rx_slab, MAX_BLOCK_SIZE, I2S_BLOCKS, 4);
43
44 struct usb_i2s_ctx {
45 const struct device *i2s_dev;
46 bool headphones_enabled;
47 bool microphone_enabled;
48 bool i2s_started;
49 bool rx_started;
50 bool usb_data_received;
51 /* Counter used to determine when to start I2S and then when to start
52 * sending RX packets to host. Overflows are not a problem because this
53 * variable is not necessary after both I2S and RX is started.
54 */
55 uint8_t i2s_counter;
56 struct feedback_ctx *fb;
57
58 /* Leftover samples from I2S receive buffer, already compacted to mono,
59 * that were not sent to host. The buffer, if not NULL, is allocated
60 * from I2S RX slab.
61 */
62 uint8_t *pending_mic_buf;
63 uint8_t pending_mic_samples;
64
65 /* Rolling bit buffers for tracking nominal + 1 and nominal - 1 samples
66 * sent. Bits are mutually exclusive, i.e.:
67 * plus_ones | minus_ones = plus_ones ^ minus_ones
68 *
69 * Used to avoid overcompensation in feedback regulator. LSBs indicate
70 * latest write size.
71 */
72 uint8_t plus_ones;
73 uint8_t minus_ones;
74 };
75
uac2_terminal_update_cb(const struct device * dev,uint8_t terminal,bool enabled,bool microframes,void * user_data)76 static void uac2_terminal_update_cb(const struct device *dev, uint8_t terminal,
77 bool enabled, bool microframes,
78 void *user_data)
79 {
80 struct usb_i2s_ctx *ctx = user_data;
81
82 /* This sample is for Full-Speed only devices. */
83 __ASSERT_NO_MSG(microframes == false);
84
85 if (terminal == HEADPHONES_OUT_TERMINAL_ID) {
86 ctx->headphones_enabled = enabled;
87 } else if (terminal == MICROPHONE_IN_TERMINAL_ID) {
88 ctx->microphone_enabled = enabled;
89 }
90
91 if (ctx->i2s_started && !ctx->headphones_enabled &&
92 !ctx->microphone_enabled) {
93 i2s_trigger(ctx->i2s_dev, I2S_DIR_BOTH, I2S_TRIGGER_DROP);
94 ctx->i2s_started = false;
95 ctx->i2s_counter = 0;
96 ctx->plus_ones = ctx->minus_ones = 0;
97 if (ctx->pending_mic_samples) {
98 k_mem_slab_free(&i2s_rx_slab, ctx->pending_mic_buf);
99 ctx->pending_mic_buf = NULL;
100 ctx->pending_mic_samples = 0;
101 }
102 }
103 }
104
uac2_get_recv_buf(const struct device * dev,uint8_t terminal,uint16_t size,void * user_data)105 static void *uac2_get_recv_buf(const struct device *dev, uint8_t terminal,
106 uint16_t size, void *user_data)
107 {
108 ARG_UNUSED(dev);
109 struct usb_i2s_ctx *ctx = user_data;
110 void *buf = NULL;
111 int ret;
112
113 if (terminal == HEADPHONES_OUT_TERMINAL_ID) {
114 __ASSERT_NO_MSG(size <= MAX_BLOCK_SIZE);
115
116 if (!ctx->headphones_enabled) {
117 LOG_ERR("Buffer request on disabled terminal");
118 return NULL;
119 }
120
121 ret = k_mem_slab_alloc(&i2s_tx_slab, &buf, K_NO_WAIT);
122 if (ret != 0) {
123 buf = NULL;
124 }
125 }
126
127 return buf;
128 }
129
uac2_data_recv_cb(const struct device * dev,uint8_t terminal,void * buf,uint16_t size,void * user_data)130 static void uac2_data_recv_cb(const struct device *dev, uint8_t terminal,
131 void *buf, uint16_t size, void *user_data)
132 {
133 struct usb_i2s_ctx *ctx = user_data;
134 int ret;
135
136 ctx->usb_data_received = true;
137
138 if (!ctx->headphones_enabled && !ctx->microphone_enabled) {
139 k_mem_slab_free(&i2s_tx_slab, buf);
140 return;
141 }
142
143 if (!size) {
144 /* This code path is expected when host only records microphone
145 * data and is not streaming any audio to the headphones. Simply
146 * transmit as many zero-filled samples were last sent to allow
147 * the feedback regulator to work.
148 *
149 * When host is streaming audio, this can be a transient error.
150 * While the "feedback regulator delay" is likely to differ,
151 * it is still probably best to just zero-fill last sent number
152 * of samples. If we overcompensate as a result, the situation
153 * will stabilize after a while anyway.
154 *
155 * In either case, we have to keep I2S going and the only way
156 * we can control the SOF to I2S offset is by varying the number
157 * of samples sent.
158 */
159 if (ctx->plus_ones & 1) {
160 size = (SAMPLES_PER_SOF + 1) * BYTES_PER_SLOT;
161 } else if (ctx->minus_ones & 1) {
162 size = (SAMPLES_PER_SOF - 1) * BYTES_PER_SLOT;
163 } else {
164 size = SAMPLES_PER_SOF * BYTES_PER_SLOT;
165 }
166 memset(buf, 0, size);
167 sys_cache_data_flush_range(buf, size);
168 }
169
170 LOG_DBG("Received %d data to input terminal %d", size, terminal);
171
172 ret = i2s_write(ctx->i2s_dev, buf, size);
173 if (ret < 0) {
174 ctx->i2s_started = false;
175 ctx->i2s_counter = 0;
176 ctx->plus_ones = ctx->minus_ones = 0;
177 if (ctx->pending_mic_samples) {
178 k_mem_slab_free(&i2s_rx_slab, ctx->pending_mic_buf);
179 ctx->pending_mic_buf = NULL;
180 ctx->pending_mic_samples = 0;
181 }
182
183 /* Most likely underrun occurred, prepare I2S restart */
184 i2s_trigger(ctx->i2s_dev, I2S_DIR_BOTH, I2S_TRIGGER_PREPARE);
185
186 ret = i2s_write(ctx->i2s_dev, buf, size);
187 if (ret < 0) {
188 /* Drop data block, will try again on next frame */
189 k_mem_slab_free(&i2s_tx_slab, buf);
190 }
191 }
192
193 if (ret == 0) {
194 ctx->i2s_counter++;
195 }
196 }
197
uac2_buf_release_cb(const struct device * dev,uint8_t terminal,void * buf,void * user_data)198 static void uac2_buf_release_cb(const struct device *dev, uint8_t terminal,
199 void *buf, void *user_data)
200 {
201 if (terminal == MICROPHONE_IN_TERMINAL_ID) {
202 k_mem_slab_free(&i2s_rx_slab, buf);
203 }
204 }
205
206 /* Determine next number of samples to send, called at most once every SOF */
next_mic_num_samples(struct usb_i2s_ctx * ctx)207 static int next_mic_num_samples(struct usb_i2s_ctx *ctx)
208 {
209 int offset = feedback_samples_offset(ctx->fb);
210
211 /* The rolling buffers essentially handle controller dead time, i.e.
212 * the buffers are used to prevent overcompensating on feedback offset.
213 * Remove the oldest entry by shifting the values by one bit.
214 */
215 ctx->plus_ones <<= 1;
216 ctx->minus_ones <<= 1;
217
218 if ((offset < 0) && (POPCOUNT(ctx->plus_ones) < -offset)) {
219 /* I2S buffer starts at least 1 sample before SOF, send nominal
220 * + 1 samples to host in order to shift offset towards 0.
221 */
222 ctx->plus_ones |= 1;
223 return SAMPLES_PER_SOF + 1;
224 }
225
226 if ((offset > 0) && (POPCOUNT(ctx->minus_ones) < offset)) {
227 /* I2S buffer starts at least 1 sample after SOF, send nominal
228 * - 1 samples to host in order to shift offset towards 0
229 */
230 ctx->minus_ones |= 1;
231 return SAMPLES_PER_SOF - 1;
232 }
233
234 /* I2S is either spot on, or the offset is expected to correct soon */
235 return SAMPLES_PER_SOF;
236 }
237
process_mic_data(const struct device * dev,struct usb_i2s_ctx * ctx)238 static void process_mic_data(const struct device *dev, struct usb_i2s_ctx *ctx)
239 {
240 size_t num_bytes;
241 uint8_t *dst, *src;
242 uint8_t *mic_buf;
243 void *rx_block;
244 int ret;
245 int samples_to_send, mic_samples, rx_samples, leftover_samples;
246
247 samples_to_send = next_mic_num_samples(ctx);
248
249 if (ctx->pending_mic_samples >= samples_to_send) {
250 /* No need to fetch new I2S samples, this happens shortly after
251 * we have "borrowed" samples from next buffer. This is expected
252 * and means that the streams have synchronized.
253 */
254 rx_block = NULL;
255 rx_samples = 0;
256 } else {
257 ret = i2s_read(ctx->i2s_dev, &rx_block, &num_bytes);
258 if (ret) {
259 /* No data available, I2S will restart soon */
260 return;
261 }
262 sys_cache_data_invd_range(rx_block, num_bytes);
263
264 /* I2S operates on 2 channels (stereo) */
265 rx_samples = num_bytes / (BYTES_PER_SAMPLE * 2);
266 }
267
268 /* Prepare microphone data to send, use pending samples if any */
269 src = rx_block;
270 if (ctx->pending_mic_buf) {
271 mic_buf = ctx->pending_mic_buf;
272 mic_samples = ctx->pending_mic_samples;
273 dst = &ctx->pending_mic_buf[mic_samples * BYTES_PER_SAMPLE];
274 } else if (rx_samples >= 1) {
275 /* First sample is already in place */
276 mic_buf = rx_block;
277 dst = &mic_buf[BYTES_PER_SAMPLE];
278 src += 2 * BYTES_PER_SAMPLE;
279 mic_samples = 1;
280 rx_samples--;
281 } else {
282 /* Something went horribly wrong, free the buffer and leave */
283 k_mem_slab_free(&i2s_rx_slab, rx_block);
284 return;
285 }
286
287 /* Copy as many samples as possible, stop if mic buffer is ready */
288 while ((mic_samples < samples_to_send) && (rx_samples > 0)) {
289 memcpy(dst, src, BYTES_PER_SAMPLE);
290
291 dst += BYTES_PER_SAMPLE;
292 src += 2 * BYTES_PER_SAMPLE;
293
294 mic_samples++;
295 rx_samples--;
296 }
297
298 /* Is mic buffer ready to go? */
299 if (mic_samples < samples_to_send) {
300 /* No, we have to borrow sample from next buffer. This can only
301 * happen if we fully drained current receive buffer.
302 */
303 __ASSERT_NO_MSG(rx_samples == 0);
304
305 if (rx_block != mic_buf) {
306 /* RX buffer no longer needed, samples are in mic_buf */
307 k_mem_slab_free(&i2s_rx_slab, rx_block);
308 }
309
310 ret = i2s_read(ctx->i2s_dev, &rx_block, &num_bytes);
311 if (ret) {
312 /* No data, I2S will likely restart due to error soon */
313 ctx->pending_mic_buf = mic_buf;
314 ctx->pending_mic_samples = mic_samples;
315 return;
316 }
317 sys_cache_data_invd_range(rx_block, num_bytes);
318
319 src = rx_block;
320 rx_samples = num_bytes / (BYTES_PER_SAMPLE * 2);
321 }
322
323 /* Copy remaining sample, under normal conditions (i.e. connected to
324 * non-malicious host) this is guaranteed to fully fill mic_buf.
325 */
326 while ((mic_samples < samples_to_send) && (rx_samples > 0)) {
327 memcpy(dst, src, BYTES_PER_SAMPLE);
328
329 dst += BYTES_PER_SAMPLE;
330 src += 2 * BYTES_PER_SAMPLE;
331
332 mic_samples++;
333 rx_samples--;
334 }
335
336 /* Are we still short on samples? */
337 if (mic_samples < samples_to_send) {
338 /* The only possibility for this code to execute is that we were
339 * short on samples and the next block (pointed to by rx_block)
340 * did not contain enough samples to fill the gap.
341 */
342 __ASSERT_NO_MSG(rx_block != mic_buf);
343
344 /* Bailing out at this point likely leads to faster recovery.
345 * Note that this should never happen during normal operation.
346 */
347 ctx->pending_mic_buf = mic_buf;
348 ctx->pending_mic_samples = mic_samples;
349
350 /* RX buffer is no longer needed */
351 k_mem_slab_free(&i2s_rx_slab, rx_block);
352 return;
353 }
354
355 /* Handle any potential leftover, start by sanitizing length */
356 leftover_samples = mic_samples - samples_to_send + rx_samples;
357 if (leftover_samples > (MAX_BLOCK_SIZE / BYTES_PER_SAMPLE)) {
358 size_t dropped_samples =
359 leftover_samples - (MAX_BLOCK_SIZE / BYTES_PER_SAMPLE);
360
361 LOG_WRN("Too many leftover samples, dropping %d samples",
362 dropped_samples);
363 if (rx_samples >= dropped_samples) {
364 rx_samples -= dropped_samples;
365 } else {
366 mic_samples -= (dropped_samples - rx_samples);
367 rx_samples = 0;
368 }
369
370 leftover_samples = (MAX_BLOCK_SIZE / BYTES_PER_SAMPLE);
371 }
372
373 if (leftover_samples == 0) {
374 /* No leftover samples */
375 if ((rx_block != NULL) && (rx_block != mic_buf)) {
376 /* All samples were copied, free source buffer */
377 k_mem_slab_free(&i2s_rx_slab, rx_block);
378 }
379 rx_block = NULL;
380 } else if ((mic_samples > samples_to_send) ||
381 ((rx_samples > 0) && (rx_block == mic_buf))) {
382 /* Leftover samples have to be copied to new buffer */
383 ret = k_mem_slab_alloc(&i2s_rx_slab, &rx_block, K_NO_WAIT);
384 if (ret != 0) {
385 LOG_WRN("Out of memory dropping %d samples",
386 leftover_samples);
387 mic_samples = samples_to_send;
388 rx_samples = 0;
389 rx_block = NULL;
390 }
391 }
392
393 /* At this point rx_block is either
394 * * NULL if there are no leftover samples, OR
395 * * src buffer if leftover data can be copied from back to front, OR
396 * * brand new buffer if there is leftover data in mic buffer.
397 */
398 ctx->pending_mic_buf = rx_block;
399 ctx->pending_mic_samples = 0;
400
401 /* Copy excess samples from pending mic buf, if any */
402 if (mic_samples > samples_to_send) {
403 size_t bytes;
404
405 /* Samples in mic buffer are already compacted */
406 bytes = (mic_samples - samples_to_send) * BYTES_PER_SAMPLE;
407 memcpy(ctx->pending_mic_buf, &mic_buf[mic_samples], bytes);
408
409 ctx->pending_mic_samples = mic_samples - samples_to_send;
410 dst = &ctx->pending_mic_buf[bytes];
411 } else {
412 dst = ctx->pending_mic_buf;
413 }
414
415 /* Copy excess samples from src buffer, so we don't lose any */
416 while (rx_samples > 0) {
417 memcpy(dst, src, BYTES_PER_SAMPLE);
418
419 dst += BYTES_PER_SAMPLE;
420 src += 2 * BYTES_PER_SAMPLE;
421
422 ctx->pending_mic_samples++;
423 rx_samples--;
424 }
425
426 /* Finally send the microphone samples to host */
427 sys_cache_data_flush_range(mic_buf, mic_samples * BYTES_PER_SAMPLE);
428 if (usbd_uac2_send(dev, MICROPHONE_IN_TERMINAL_ID,
429 mic_buf, mic_samples * BYTES_PER_SAMPLE) < 0) {
430 k_mem_slab_free(&i2s_rx_slab, mic_buf);
431 }
432 }
433
uac2_sof(const struct device * dev,void * user_data)434 static void uac2_sof(const struct device *dev, void *user_data)
435 {
436 ARG_UNUSED(dev);
437 struct usb_i2s_ctx *ctx = user_data;
438
439 if (ctx->i2s_started) {
440 feedback_process(ctx->fb);
441 }
442
443 /* If we didn't receive data since last SOF but either terminal is
444 * enabled, then we have to come up with the buffer ourself to keep
445 * I2S going.
446 */
447 if (!ctx->usb_data_received &&
448 (ctx->microphone_enabled || ctx->headphones_enabled)) {
449 /* No data received since last SOF but we have to keep going */
450 void *buf;
451 int ret;
452
453 ret = k_mem_slab_alloc(&i2s_tx_slab, &buf, K_NO_WAIT);
454 if (ret != 0) {
455 buf = NULL;
456 }
457
458 if (buf) {
459 /* Use size 0 to utilize zero-fill functionality */
460 uac2_data_recv_cb(dev, HEADPHONES_OUT_TERMINAL_ID,
461 buf, 0, user_data);
462 }
463 }
464 ctx->usb_data_received = false;
465
466 /* We want to maintain 3 SOFs delay, i.e. samples received from host
467 * during SOF n should be transmitted on I2S during SOF n+3. This
468 * provides enough wiggle room for software scheduling that effectively
469 * eliminates "buffers not provided in time" problem.
470 *
471 * ">= 2" translates into 3 SOFs delay because the timeline is:
472 * USB SOF n
473 * OUT DATA0 n received from host
474 * USB SOF n+1
475 * DATA0 n is available to UDC driver (See Universal Serial Bus
476 * Specification Revision 2.0 5.12.5 Data Prebuffering) and copied
477 * to I2S buffer before SOF n+2; i2s_counter = 1
478 * OUT DATA0 n+1 received from host
479 * USB SOF n+2
480 * DATA0 n+1 is copied; i2s_counter = 2
481 * OUT DATA0 n+2 received from host
482 * USB SOF n+3
483 * This function triggers I2S start
484 * DATA0 n+2 is copied; i2s_counter is no longer relevant
485 * OUT DATA0 n+3 received from host
486 */
487 if (!ctx->i2s_started &&
488 (ctx->headphones_enabled || ctx->microphone_enabled) &&
489 ctx->i2s_counter >= 2) {
490 i2s_trigger(ctx->i2s_dev, I2S_DIR_BOTH, I2S_TRIGGER_START);
491 ctx->i2s_started = true;
492 feedback_start(ctx->fb, ctx->i2s_counter);
493 ctx->i2s_counter = 0;
494 }
495
496 /* Start sending I2S RX data only when there are at least 3 buffers
497 * ready with data. This guarantees that there'll always be a buffer
498 * available from which sample can be borrowed.
499 */
500 if (!ctx->rx_started && ctx->i2s_started && ctx->i2s_counter >= 3) {
501 ctx->rx_started = true;
502 }
503
504 if (ctx->rx_started) {
505 process_mic_data(dev, ctx);
506 }
507 }
508
509 static struct uac2_ops usb_audio_ops = {
510 .sof_cb = uac2_sof,
511 .terminal_update_cb = uac2_terminal_update_cb,
512 .get_recv_buf = uac2_get_recv_buf,
513 .data_recv_cb = uac2_data_recv_cb,
514 .buf_release_cb = uac2_buf_release_cb,
515 };
516
517 static struct usb_i2s_ctx main_ctx;
518
main(void)519 int main(void)
520 {
521 const struct device *dev = DEVICE_DT_GET(DT_NODELABEL(uac2_headset));
522 struct usbd_context *sample_usbd;
523 struct i2s_config config;
524 int ret;
525
526 main_ctx.i2s_dev = DEVICE_DT_GET(DT_NODELABEL(i2s_rxtx));
527
528 if (!device_is_ready(main_ctx.i2s_dev)) {
529 printk("%s is not ready\n", main_ctx.i2s_dev->name);
530 return 0;
531 }
532
533 config.word_size = SAMPLE_BIT_WIDTH;
534 config.channels = NUMBER_OF_CHANNELS;
535 config.format = I2S_FMT_DATA_FORMAT_I2S;
536 config.options = I2S_OPT_BIT_CLK_MASTER | I2S_OPT_FRAME_CLK_MASTER;
537 config.frame_clk_freq = SAMPLE_FREQUENCY;
538 config.mem_slab = &i2s_tx_slab;
539 config.block_size = MAX_BLOCK_SIZE;
540 config.timeout = 0;
541
542 ret = i2s_configure(main_ctx.i2s_dev, I2S_DIR_TX, &config);
543 if (ret < 0) {
544 printk("Failed to configure TX stream: %d\n", ret);
545 return 0;
546 }
547
548 config.mem_slab = &i2s_rx_slab;
549 ret = i2s_configure(main_ctx.i2s_dev, I2S_DIR_RX, &config);
550 if (ret < 0) {
551 printk("Failed to configure RX stream: %d\n", ret);
552 return 0;
553 }
554
555 main_ctx.fb = feedback_init();
556
557 usbd_uac2_set_ops(dev, &usb_audio_ops, &main_ctx);
558
559 sample_usbd = sample_usbd_init_device(NULL);
560 if (sample_usbd == NULL) {
561 return -ENODEV;
562 }
563
564 ret = usbd_enable(sample_usbd);
565 if (ret) {
566 return ret;
567 }
568
569 return 0;
570 }
571
