Add a custom audio flow control implementation

src/I2s/AudioBuffer.hpp

@@ -0,0 +1,145 @@
+/*  SPDX-License-Identifier: GPL-3.0-or-later
+ *
+ *  FM-DX Tuner
+ *  Copyright (C) 2024  Konrad Kosmatka
+ *
+ *  This program is free software; you can redistribute it and/or
+ *  modify it under the terms of the GNU General Public License
+ *  as published by the Free Software Foundation; either version 3
+ *  of the License, or (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ */
+
+#ifndef FMDX_TUNER_I2S_AUDIO_BUFFER_H
+#define FMDX_TUNER_I2S_AUDIO_BUFFER_H
+
+#include <cstdint>
+
+template <uint8_t ChunkSize,
+          uint8_t Capacity>
+class AudioBuffer
+{
+public:
+    AudioBuffer()
+    {
+        static_assert((Capacity % 2) == 0, "Capacity must be even");
+        this->clear();
+    }
+
+    void
+    clear()
+    {
+        this->readIndex = 0;
+        this->writeIndex = 0;
+        this->ready = false;
+        this->full = false;
+    }
+
+    uint8_t
+    fill() const
+    {
+        return this->full ? Capacity : (Capacity + this->writeIndex - this->readIndex) % Capacity;
+    }
+
+    const uint32_t*
+    pop()
+    {
+        if (!this->ready || 
+            (!this->full && this->readIndex == this->writeIndex))
+        {
+            return (uint32_t*)silence;
+        }
+
+        uint32_t *ret = this->buffer + (this->readIndex * ChunkSize);
+        this->readIndex = (this->readIndex + 1) % Capacity;
+        this->full = false;
+        return ret;
+    }
+
+    void
+    push(const uint32_t *data)
+    {
+        constexpr uint8_t halfFill = Capacity / 2;
+        const uint8_t currentFill = this->fill();
+
+        uint16_t peak = 0;
+        for (uint8_t i = 0; i < ChunkSize; i++)
+        {
+            const int16_t channelA = data[i] >> 16;
+            const int16_t channelB = (int16_t) data[i];
+
+            const uint16_t modA = (channelA > 0) ? channelA : -channelA;
+            const uint16_t modB = (channelB > 0) ? channelB : -channelB;
+
+            const uint16_t peakAB = std::max(modA, modB);
+            peak = std::max(peak, peakAB);
+        }
+
+        if (currentFill == Capacity)
+        {
+            /* Drop one chunk from the beginning of the buffer */
+            this->readIndex = (this->readIndex + 1) % Capacity;
+        }
+        else if (currentFill > halfFill)
+        {
+            const uint16_t threshold = 1 << (15 * (currentFill - halfFill) / halfFill);
+
+            if (peak < threshold)
+            {
+                /* Drop new chunk */
+                return;
+            }
+        }
+
+        this->insert(data);
+
+        if ((currentFill + 1) < halfFill)
+        {
+            const uint16_t threshold = 1 << (15 - (15 * currentFill / halfFill));
+
+            if (peak < threshold)
+            {
+                /* Insert one chunk of silence */
+                this->insert((uint32_t*)this->silence);
+            }
+        }
+
+        if (this->writeIndex == this->readIndex)
+        {
+            this->full = true;
+        }
+
+        if (!this->ready && 
+            this->writeIndex >= Capacity / 2)
+        {
+            this->ready = true;
+        }
+    }
+private:
+    void
+    insert(const uint32_t *data)
+    {
+        const uint16_t pos = (uint16_t)this->writeIndex * ChunkSize;
+        this->writeIndex = (this->writeIndex + 1) % Capacity;
+
+        for (uint8_t i = 0; i < ChunkSize; i++)
+        {
+            this->buffer[pos + i] = data[i];
+        }
+    }
+
+    static constexpr uint32_t silence[ChunkSize] = {0};
+    static constexpr uint16_t size = (uint16_t)ChunkSize * Capacity;
+    uint32_t buffer[size];
+
+    uint8_t readIndex;
+    uint8_t writeIndex;
+    bool ready;
+    bool full;
+};
+
+#endif

src/I2s/I2sStm32.cpp

@@ -18,6 +18,7 @@
 #include <Arduino.h>
 #include <tusb.h>
 #include <stm32f0xx.h>
+#include "AudioBuffer.hpp"
 #include "../Comm.hpp"
 
 #define I2S_PIN_WS GPIO_PIN_4
@@ -27,14 +28,20 @@
 I2S_HandleTypeDef hi2s1;
 DMA_HandleTypeDef hdma_spi1_rx;
 
-#define AUDIO_BUFFER_SIZE 192
-static uint16_t buffer[AUDIO_BUFFER_SIZE];
+#define FRAME_SIZE_BYTES 192
+
+#define DMA_BUFFER_SIZE (FRAME_SIZE_BYTES * 2 / sizeof(uint16_t))
+static uint16_t dma_buffer[DMA_BUFFER_SIZE];
+
+#define CHUNK_SIZE (FRAME_SIZE_BYTES / sizeof(uint16_t) / 2)
+#define BUFFER_CAPACITY 10
+AudioBuffer<CHUNK_SIZE, BUFFER_CAPACITY> audioBuffer;
 
 void
 I2sDriver_Start(void)
 {
     __HAL_RCC_DMA1_CLK_ENABLE();
-    HAL_NVIC_SetPriority(DMA1_Channel2_3_IRQn, 0, 0);
+    HAL_NVIC_SetPriority(DMA1_Channel2_3_IRQn, 1, 0);
     HAL_NVIC_EnableIRQ(DMA1_Channel2_3_IRQn);
 
     __HAL_RCC_GPIOA_CLK_ENABLE();
@@ -60,9 +67,6 @@ I2sDriver_Start(void)
 
     __HAL_LINKDMA(&hi2s1, hdmarx, hdma_spi1_rx);
 
-    HAL_NVIC_SetPriority(SPI1_IRQn, 0, 0);
-    HAL_NVIC_EnableIRQ(SPI1_IRQn);
-
     hi2s1.Instance = SPI1;
     hi2s1.Init.Mode = I2S_MODE_SLAVE_RX;
     hi2s1.Init.Standard = I2S_STANDARD_PHILIPS;
@@ -71,13 +75,15 @@ I2sDriver_Start(void)
     hi2s1.Init.AudioFreq = I2S_AUDIOFREQ_48K;
     hi2s1.Init.CPOL = I2S_CPOL_HIGH;
 
+    audioBuffer.clear();
+
     __disable_irq();
     /* TODO: Add timeout */
     while ((GPIOA->IDR & I2S_PIN_WS) == 1);
     while ((GPIOA->IDR & I2S_PIN_WS) == 0);
     /* STM32F072 Errata 2.13.4: Enable I2S when the WS is high */
     HAL_I2S_Init(&hi2s1);
-    HAL_I2S_Receive_DMA(&hi2s1, buffer, AUDIO_BUFFER_SIZE);
+    HAL_I2S_Receive_DMA(&hi2s1, dma_buffer, DMA_BUFFER_SIZE);
     __enable_irq();
 }
 
@@ -87,7 +93,6 @@ I2sDriver_Stop(void)
     HAL_I2S_DMAStop(&hi2s1);
     HAL_I2S_DeInit(&hi2s1);
     __HAL_RCC_SPI1_CLK_DISABLE();
-    HAL_NVIC_DisableIRQ(SPI1_IRQn);
     HAL_DMA_DeInit(hi2s1.hdmarx);
     HAL_GPIO_DeInit(GPIOA, I2S_PIN_WS | I2S_PIN_CK | I2S_PIN_SD);
     HAL_NVIC_DisableIRQ(DMA1_Channel2_3_IRQn);
@@ -100,21 +105,32 @@ DMA1_Channel2_3_IRQHandler(void)
 }
 
 extern "C" void
-SPI1_IRQHandler(void)
+HAL_I2S_RxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
 {
-    HAL_I2S_IRQHandler(&hi2s1);
+    audioBuffer.push((uint32_t*)dma_buffer);
 }
 
 extern "C" void
-HAL_I2S_RxHalfCpltCallback(I2S_HandleTypeDef *hi2s)
+HAL_I2S_RxCpltCallback(I2S_HandleTypeDef *hi2s)
 {
-    tud_audio_write(buffer, AUDIO_BUFFER_SIZE);
+    audioBuffer.push((uint32_t*)(dma_buffer + (DMA_BUFFER_SIZE / 2)));
 }
 
-extern "C" void
-HAL_I2S_RxCpltCallback(I2S_HandleTypeDef *hi2s)
+extern "C" bool
+tud_audio_tx_done_pre_load_cb(uint8_t rhport,
+                              uint8_t itf,
+                              uint8_t ep_in,
+                              uint8_t cur_alt_setting)
 {
-    tud_audio_write(buffer + (AUDIO_BUFFER_SIZE / 2), AUDIO_BUFFER_SIZE);
+    (void) rhport;
+    (void) itf;
+    (void) ep_in;
+    (void) cur_alt_setting;
+
+    const uint32_t *buffer = audioBuffer.pop();
+    tud_audio_write(buffer, FRAME_SIZE_BYTES);
+
+    return true;
 }
 
 #endif /* ARDUINO_ARCH_STM32 */

src/Platform/Stm32/Stm32.cpp

@@ -19,12 +19,15 @@
 #include <stm32f0xx_hal.h>
 #include "../../Comm.hpp"
 
+
 void
 PlatformDriver_Setup(void)
 {
     __HAL_RCC_USB_CLK_ENABLE();
-    HAL_NVIC_SetPriority(USB_IRQn, 2, 0);
+    HAL_NVIC_SetPriority(USB_IRQn, 0, 0);
     HAL_NVIC_EnableIRQ(USB_IRQn);
+    HAL_NVIC_SetPriority(PendSV_IRQn, 1, 0);
+    HAL_NVIC_EnableIRQ(PendSV_IRQn);
     tud_init(0);
 }
 
@@ -46,7 +49,7 @@ USB_IRQHandler(void)
     tud_int_handler(0);
 
     /* Continue in a lower priority interrupt */
-    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
 }
 
 /* The PendSV_Handler will take care of the USB

src/Platform/Stm32/UsbCdcStream.cpp

@@ -77,7 +77,7 @@ UsbCdcStream::write(uint8_t value)
     txWritePos = (txWritePos + 1) % BUFF_SIZE;
 
     /* Continue in an interrupt */
-    SCB->ICSR |= SCB_ICSR_PENDSVSET_Msk;
+    SCB->ICSR = SCB_ICSR_PENDSVSET_Msk;
     return 1;
 }
 

tusb_config.h

@@ -113,6 +113,7 @@ extern "C" {
 #define CFG_TUD_AUDIO_FUNC_1_CTRL_BUF_SZ           64
 
 #define CFG_TUD_AUDIO_ENABLE_EP_IN                 1
+#define CFG_TUD_AUDIO_EP_IN_FLOW_CONTROL           0
 #define CFG_TUD_AUDIO_FUNC_1_N_BYTES_PER_SAMPLE_TX 2
 #define CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX         2
 
@@ -121,7 +122,7 @@ extern "C" {
                                                                      CFG_TUD_AUDIO_FUNC_1_N_CHANNELS_TX)
 
 #define CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX          CFG_TUD_AUDIO_EP_SZ_IN
-#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ       (4 * CFG_TUD_AUDIO_FUNC_1_EP_IN_SZ_MAX)
+#define CFG_TUD_AUDIO_FUNC_1_EP_IN_SW_BUF_SZ       CFG_TUD_AUDIO_EP_SZ_IN
 
 #define CFG_TUD_CDC_RX_BUFSIZE 64
 #define CFG_TUD_CDC_TX_BUFSIZE 64