add retainHEADmain

4 files changed, 293 insertions, 42 deletions

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 <math.h>

-#include <float.h>

+#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, &params);

-

-    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, &params);

-

-    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);