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