From bdd8c21fcfff8041ea94a972903ce42e58c2a5c2 Mon Sep 17 00:00:00 2001 From: ozpv <39195175+ozpv@users.noreply.github.com> Date: Sat, 29 Aug 2026 23:04:49 -0500 Subject: add retain --- Core/Src/Effects/retain.c | 256 +++++++++++++++++++++++++++++++++++++++++++++ Core/Src/Effects/ringmod.c | 31 ++++-- Core/Src/cs4272.c | 45 +++----- Core/Src/main.c | 3 + 4 files changed, 293 insertions(+), 42 deletions(-) create mode 100755 Core/Src/Effects/retain.c (limited to 'Core/Src') diff --git a/Core/Src/Effects/retain.c b/Core/Src/Effects/retain.c new file mode 100755 index 0000000..9b3acbe --- /dev/null +++ b/Core/Src/Effects/retain.c @@ -0,0 +1,256 @@ +/* + * retain.c + * + * Created on: Aug 23, 2026 + * Author: ozpv + */ + +#include "retain.h" +#include "arm_math.h" + +__attribute__((section(".audio_buffers"))) static float input[WINDOW_SIZE]; +static size_t input_position; +static size_t samples_since_frame; + +__attribute__((section(".RAM_D2"))) static float window_function[WINDOW_SIZE]; +__attribute__((section(".RAM_D2"))) static float normalize[HOP_SIZE]; + +__attribute__((section(".audio_buffers"))) static float fft_input[WINDOW_SIZE]; + +__attribute__((section(".audio_buffers"))) static float spectral[WINDOW_SIZE]; + +__attribute__((section(".RAM_D2"))) static float output[WINDOW_SIZE]; + +__attribute__((section(".RAM_D2"))) static float output_history[OVERLAP_SIZE]; +static size_t output_position; + +__attribute__((section(".RAM_D2"))) static float retain_scratch[COMPLEX_BIN_COUNT]; + +static arm_rfft_fast_instance_f32 fft_instance; + +static inline void create_normalize_buffer(float *out, const float *function, const size_t window_size, const size_t hop_size) { + const size_t num_overlaps = window_size / hop_size; + + for (size_t i = 0; i < hop_size; ++i) { + float sum = 0.0f; + + for (size_t j = 0; j < num_overlaps; ++j) { + const float w = function[i + j * hop_size]; + + sum += w * w; + } + + out[i] = sum; + } +} + +void retain_init(void) { + arm_blackman_harris_92db_f32(window_function, WINDOW_SIZE); + + create_normalize_buffer(normalize, window_function, WINDOW_SIZE, HOP_SIZE); + + arm_rfft_fast_init_f32(&fft_instance, WINDOW_SIZE); + + for (size_t i = 0; i < WINDOW_SIZE; ++i) { + input[i] = 0.0f; + fft_input[i] = 0.0f; + output[i] = 0.0f; + spectral[i] = 0.0f; + } + + for (size_t i = 0; i < OVERLAP_SIZE; ++i) { + output_history[i] = 0.0f; + } + + input_position = 0; + samples_since_frame = 0; + output_position = 0; +} + +static inline float select_nth_largest(float *x, const size_t len, const size_t n) { + size_t left = 0; + size_t right = len - 1; + size_t target = n - 1; + + while (left < right) { + size_t mid = left + ((right - left) >> 1); + float a = x[left]; + float b = x[mid]; + float c = x[right]; + float pivot; + + if (a < b) { + if (b < c) { + pivot = b; + } else if (a < c) { + pivot = c; + } else { + pivot = a; + } + } else { + if (a < c) { + pivot = a; + } else if (b < c) { + pivot = c; + } else { + pivot = b; + } + } + + size_t i = left; + size_t j = right; + + while (i <= j) { + while (x[i] > pivot) { + ++i; + } + + while (x[j] < pivot) { + --j; + } + + if (i <= j) { + float t = x[i]; + x[i] = x[j]; + x[j] = t; + + ++i; + --j; + } + } + + if (target <= j) { + right = j; + } else if (target >= i) { + left = i; + } else { + break; + } + } + + return x[target]; +} + +static inline void process(float *spectral, const size_t n) { + if (n == 0) { + for (size_t bin = 1; bin < WINDOW_SIZE / 2; ++bin) { + const size_t i = 2 * bin; + + spectral[i] = 0.0f; + spectral[i + 1] = 0.0f; + } + + return; + } + + if (n >= COMPLEX_BIN_COUNT) { + return; + } + + for (size_t bin = 1; bin < WINDOW_SIZE / 2; ++bin) { + const size_t i = 2 * bin; + + const float re = spectral[i]; + const float im = spectral[i + 1]; + + retain_scratch[bin - 1] = re * re + im * im; + } + + const float threshold = select_nth_largest(retain_scratch, COMPLEX_BIN_COUNT, n); + + for (size_t bin = 1; bin < WINDOW_SIZE / 2; ++bin) { + const size_t i = 2 * bin; + + const float re = spectral[i]; + const float im = spectral[i + 1]; + + const float mag2 = re * re + im * im; + + if (mag2 < threshold) { + spectral[i] = 0.0f; + spectral[i + 1] = 0.0f; + } + } +} + +static inline void process_frame(void) { + /* build a contiguous FFT frame */ + size_t src = input_position; + for (size_t i = 0; i < WINDOW_SIZE; ++i) { + fft_input[i] = input[src]; + + ++src; + + if (src == WINDOW_SIZE) { + src = 0; + } + } + + /* apply window function */ + for (size_t i = 0; i < WINDOW_SIZE; ++i) { + fft_input[i] *= window_function[i]; + } + + /* forward FFT */ + arm_rfft_fast_f32(&fft_instance, fft_input, spectral, 0); + + process(spectral, 5); + + /* inverse FFT */ + arm_rfft_fast_f32(&fft_instance, spectral, output, 1); + + /* synthesis window because of heavy processing */ + for (size_t i = 0; i < WINDOW_SIZE; ++i) { + output[i] *= window_function[i]; + } + + /* overlap add */ + for (size_t i = 0; i < OVERLAP_SIZE; ++i) { + output[i] += output_history[i]; + } + + /* save future overlap */ + for (size_t i = 0; i < OVERLAP_SIZE; ++i) { + output_history[i] = output[i + HOP_SIZE]; + } + + /* normalize output samples */ + for (size_t i = 0; i < HOP_SIZE; ++i) { + output[i] /= normalize[i]; + } + + output_position = 0; + samples_since_frame = 0; +} + +inline void retain( + const int32_t *samples_in, + int32_t *samples_out, + const size_t range_min, + const size_t range_max, + const enum Channel channel +) { + for (size_t i = range_min + channel; i < range_max; i += 2) { + input[input_position] = int24_t_to_f32(samples_in[i]); + + ++input_position; + + if (input_position == WINDOW_SIZE) { + input_position = 0; + } + + ++samples_since_frame; + + if (samples_since_frame == HOP_SIZE) { + process_frame(); + } + + samples_out[i] = f32_to_int24_t(output[output_position]); + + ++output_position; + + if (output_position == HOP_SIZE) { + output_position = 0; + } + } +} diff --git a/Core/Src/Effects/ringmod.c b/Core/Src/Effects/ringmod.c index 1b411dc..be58c11 100755 --- a/Core/Src/Effects/ringmod.c +++ b/Core/Src/Effects/ringmod.c @@ -7,11 +7,17 @@ #include "cs4272.h" #include "ringmod.h" -#include -#include +#include "arm_math.h" /* MF-102S ring modulator */ -void ringmod(float *samples, const size_t range_min, const size_t range_max, const struct RingModParams *params) { +void ringmod( + const struct RingModParams *params, + const int32_t *samples_in, + int32_t *samples_out, + const size_t range_min, + const size_t range_max, + const enum Channel channel +) { static float lfo_phase = 0.0f; static float carrier_phase = 0.0f; @@ -19,23 +25,30 @@ void ringmod(float *samples, const size_t range_min, const size_t range_max, con const float inverse_sample_rate = 1.0f / sample_rate; /* samples are interleaved */ - for (size_t i = range_min; i < range_max; i += 2) { + for (size_t i = range_min + channel; i < range_max; i += 2) { + float sample = int24_t_to_f32(samples_in[i]); + float frequency_lfo = 0.0f; if (params->square_lfo) { frequency_lfo = (lfo_phase < 0.5f) ? params->lfo_amount : -params->lfo_amount; } else { - frequency_lfo = params->lfo_amount * sinf(M_TWOPI * lfo_phase); + frequency_lfo = params->lfo_amount * arm_sin_f32(M_TWOPI * lfo_phase); } - float carrier = sinf(M_TWOPI * carrier_phase); + float carrier = arm_sin_f32(M_TWOPI * carrier_phase); - samples[i] = (samples[i] * (1.0f - params->mix)) + (samples[i] * carrier * params->mix); + float output_sample = (sample * (1.0f - params->mix)) + (sample * carrier * params->mix); + samples_out[i] = f32_to_int24_t(output_sample); lfo_phase += params->lfo_rate * inverse_sample_rate; - lfo_phase = fmod(lfo_phase, 1.0f); + while (lfo_phase >= 1.0f) { + lfo_phase -= 1.0f; + } carrier_phase += (params->frequency + frequency_lfo) * inverse_sample_rate; - carrier_phase = fmod(carrier_phase, 1.0f); + while (carrier_phase >= 1.0f) { + carrier_phase -= 1.0f; + } } } diff --git a/Core/Src/cs4272.c b/Core/Src/cs4272.c index 9a7f0d1..8ba9885 100755 --- a/Core/Src/cs4272.c +++ b/Core/Src/cs4272.c @@ -8,18 +8,10 @@ #include "i2c.h" #include "cs4272.h" #include "ringmod.h" +#include "retain.h" -const struct RingModParams params = { - 0.40f, - 156.0f, - 6.90f, - 0.18f, - true, -}; - -volatile int32_t rx_buffer[BUFFER_SIZE] __attribute__((section(".audio_buffers"))); -volatile int32_t tx_buffer[BUFFER_SIZE] __attribute__((section(".audio_buffers"))); -volatile float samples[BUFFER_SIZE] __attribute__((section(".sdram"))); +__attribute__((section(".audio_buffers"))) int32_t rx_buffer[BUFFER_SIZE]; +__attribute__((section(".audio_buffers"))) int32_t tx_buffer[BUFFER_SIZE]; const uint8_t CS4272_CONFIG[7] = { /* MODE CONTROL 1 @@ -58,35 +50,22 @@ const uint8_t CS4272_CONFIG[7] = { 0b00000010, }; -inline void int32_t_to_float(int32_t *in, float *out, const size_t range_min, const size_t range_max) { - for (size_t i = range_min; i < range_max; ++i) { - out[i] = ((float)(in[i] << 8)) / ((float)INT32_MAX); - } -} - -inline void float_to_int32_t(float *in, int32_t *out, const size_t range_min, const size_t range_max) { - for (size_t i = range_min; i < range_max; ++i) { - out[i] = ((int32_t)(in[i] * ((float)INT32_MAX))) >> 8; - } -} +//const struct RingModParams params = { +// 0.40f, +// 156.0f, +// 16.90f, +// 0.18f, +// false, +//}; void HAL_SAI_RxHalfCpltCallback(SAI_HandleTypeDef *hsai) { - int32_t_to_float((int32_t *)rx_buffer, (float *)samples, 0, BUFFER_SIZE / 2); - - ringmod((float *)samples, 0, BUFFER_SIZE / 2, ¶ms); - - float_to_int32_t((float *)samples, (int32_t *)tx_buffer, 0, BUFFER_SIZE / 2); + retain(rx_buffer, tx_buffer, 0, BUFFER_SIZE / 2, MONO); } void HAL_SAI_RxCpltCallback(SAI_HandleTypeDef *hsai) { - int32_t_to_float((int32_t *)rx_buffer, (float *)samples, BUFFER_SIZE / 2, BUFFER_SIZE); - - ringmod((float *)samples, BUFFER_SIZE / 2, BUFFER_SIZE, ¶ms); - - float_to_int32_t((float *)samples, (int32_t *)tx_buffer, BUFFER_SIZE / 2, BUFFER_SIZE); + retain(rx_buffer, tx_buffer, BUFFER_SIZE / 2, BUFFER_SIZE, MONO); } - HAL_StatusTypeDef CS4272_ReadRegister(const uint8_t addr, uint8_t *data) { return HAL_I2C_Mem_Read(&hi2c4, CS4272_I2C_ADDR, addr, I2C_MEMADD_SIZE_8BIT, data, 1, HAL_MAX_DELAY); } diff --git a/Core/Src/main.c b/Core/Src/main.c index f7fdeac..1b3ef91 100755 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -30,6 +30,7 @@ /* USER CODE BEGIN Includes */ #include "cs4272.h" #include "w9825g6kh6.h" +#include "retain.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -123,6 +124,8 @@ int main(void) FMC_Init(); //SDRAM_Test(); + retain_init(); + __HAL_RCC_SAI1_CLK_ENABLE(); __HAL_SAI_ENABLE(&hsai_BlockA1); -- cgit v1.2.3