From 9d7f2d5123b7e5e72e252cd52d34e8c315d0820f Mon Sep 17 00:00:00 2001 From: Konrad Kosmatka Date: Wed, 24 Jul 2024 18:46:36 +0200 Subject: [PATCH] Add a custom audio flow control implementation --- src/I2s/AudioBuffer.hpp | 145 ++++++++++++++++++++++++++++ src/I2s/I2sStm32.cpp | 46 ++++++--- src/Platform/Stm32/Stm32.cpp | 7 +- src/Platform/Stm32/UsbCdcStream.cpp | 2 +- tusb_config.h | 3 +- 5 files changed, 184 insertions(+), 19 deletions(-) create mode 100644 src/I2s/AudioBuffer.hpp diff --git a/src/I2s/AudioBuffer.hpp b/src/I2s/AudioBuffer.hpp new file mode 100644 index 0000000..b3e1f7d --- /dev/null +++ b/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 + +template +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 diff --git a/src/I2s/I2sStm32.cpp b/src/I2s/I2sStm32.cpp index 85cf344..e6ffde9 100644 --- a/src/I2s/I2sStm32.cpp +++ b/src/I2s/I2sStm32.cpp @@ -18,6 +18,7 @@ #include #include #include +#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 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 */ diff --git a/src/Platform/Stm32/Stm32.cpp b/src/Platform/Stm32/Stm32.cpp index c5c92e5..5e5d3a0 100644 --- a/src/Platform/Stm32/Stm32.cpp +++ b/src/Platform/Stm32/Stm32.cpp @@ -19,12 +19,15 @@ #include #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 diff --git a/src/Platform/Stm32/UsbCdcStream.cpp b/src/Platform/Stm32/UsbCdcStream.cpp index 034a7a4..1f12066 100644 --- a/src/Platform/Stm32/UsbCdcStream.cpp +++ b/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; } diff --git a/tusb_config.h b/tusb_config.h index 726cd4e..a17ea5e 100644 --- a/tusb_config.h +++ b/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