1 files changed, 158 insertions, 0 deletions

diff --git a/codec2_fft.c b/codec2_fft.c
new file mode 100644
index 0000000..14b8670
--- /dev/null
+++ b/codec2_fft.c
@@ -0,0 +1,158 @@
+/*
+ * codec2_fft.c
+ *
+ *  Created on: 24.09.2016
+ *      Author: danilo
+ */
+
+#include "codec2_fft.h"
+
+#include "debug_alloc.h"
+
+#ifdef USE_KISS_FFT
+#include "_kiss_fft_guts.h"
+
+#else
+#if 0
+// caching constants in RAM did not seem to have an effect on performance
+// TODO: Decide what to with this code
+#define FFT_INIT_CACHE_SIZE 4
+const arm_cfft_instance_f32* fft_init_cache[FFT_INIT_CACHE_SIZE];
+
+static const arm_cfft_instance_f32* arm_fft_instance2ram(const arm_cfft_instance_f32* in)
+{
+
+    arm_cfft_instance_f32* out = malloc(sizeof(arm_cfft_instance_f32));
+
+    if (out) {
+        memcpy(out,in,sizeof(arm_cfft_instance_f32));
+        out->pBitRevTable = malloc(out->bitRevLength * sizeof(uint16_t));
+        out->pTwiddle = malloc(out->fftLen * sizeof(float32_t));
+        memcpy((void*)out->pBitRevTable,in->pBitRevTable,out->bitRevLength * sizeof(uint16_t));
+        memcpy((void*)out->pTwiddle,in->pTwiddle,out->fftLen * sizeof(float32_t));
+    }
+    return out;
+}
+
+
+static const arm_cfft_instance_f32* arm_fft_cache_get(const arm_cfft_instance_f32* romfft)
+{
+    const arm_cfft_instance_f32* retval = NULL;
+    static int used = 0;
+    for (int i = 0; fft_init_cache[i] != NULL && i < used; i++)
+    {
+        if (romfft->fftLen == fft_init_cache[i]->fftLen)
+        {
+            retval = fft_init_cache[i];
+            break;
+        }
+    }
+    if (retval == NULL && used < FFT_INIT_CACHE_SIZE)
+    {
+         retval = arm_fft_instance2ram(romfft);
+         fft_init_cache[used++] = retval;
+    }
+    if (retval == NULL)
+    {
+        retval = romfft;
+    }
+    return retval;
+}
+#endif
+#endif
+
+void codec2_fft_free(codec2_fft_cfg cfg)
+{
+#ifdef USE_KISS_FFT
+    KISS_FFT_FREE(cfg);
+#else
+    FREE(cfg);
+#endif
+}
+
+codec2_fft_cfg codec2_fft_alloc(int nfft, int inverse_fft, void* mem, size_t* lenmem)
+{
+    codec2_fft_cfg retval;
+#ifdef USE_KISS_FFT
+    retval = kiss_fft_alloc(nfft, inverse_fft, mem, lenmem);
+#else
+    retval = MALLOC(sizeof(codec2_fft_struct));
+    retval->inverse  = inverse_fft;
+    switch(nfft)
+    {
+    case 128:
+        retval->instance = &arm_cfft_sR_f32_len128;
+        break;
+    case 256:
+        retval->instance = &arm_cfft_sR_f32_len256;
+        break;
+    case 512:
+        retval->instance = &arm_cfft_sR_f32_len512;
+        break;
+//    case 1024:
+//        retval->instance = &arm_cfft_sR_f32_len1024;
+//        break;
+    default:
+        abort();
+    }
+    // retval->instance = arm_fft_cache_get(retval->instance);
+#endif
+    return retval;
+}
+
+codec2_fftr_cfg codec2_fftr_alloc(int nfft, int inverse_fft, void* mem, size_t* lenmem)
+{
+    codec2_fftr_cfg retval;
+#ifdef USE_KISS_FFT
+    retval = kiss_fftr_alloc(nfft, inverse_fft, mem, lenmem);
+#else
+    retval = MALLOC(sizeof(codec2_fftr_struct));
+    retval->inverse  = inverse_fft;
+    retval->instance = MALLOC(sizeof(arm_rfft_fast_instance_f32));
+    arm_rfft_fast_init_f32(retval->instance,nfft);
+    // memcpy(&retval->instance->Sint,arm_fft_cache_get(&retval->instance->Sint),sizeof(arm_cfft_instance_f32));
+#endif
+    return retval;
+}
+void codec2_fftr_free(codec2_fftr_cfg cfg)
+{
+#ifdef USE_KISS_FFT
+    KISS_FFT_FREE(cfg);
+#else
+    FREE(cfg->instance);
+    FREE(cfg);
+#endif
+}
+
+// there is a little overhead for inplace kiss_fft but this is
+// on the powerful platforms like the Raspberry or even x86 PC based ones
+// not noticeable
+// the reduced usage of RAM and increased performance on STM32 platforms
+// should be worth it.
+void codec2_fft_inplace(codec2_fft_cfg cfg, codec2_fft_cpx* inout)
+{
+
+#ifdef USE_KISS_FFT
+    kiss_fft_cpx in[512];
+    // decide whether to use the local stack based buffer for in
+    // or to allow kiss_fft to allocate RAM
+    // second part is just to play safe since first method
+    // is much faster and uses less RAM
+    if (cfg->nfft <= 512)
+    {
+        memcpy(in,inout,cfg->nfft*sizeof(kiss_fft_cpx));
+        kiss_fft(cfg, in, (kiss_fft_cpx*)inout);
+    }
+    else
+    {
+        kiss_fft(cfg, (kiss_fft_cpx*)inout, (kiss_fft_cpx*)inout);
+    }
+#else
+    arm_cfft_f32(cfg->instance,(float*)inout,cfg->inverse,1);
+    if (cfg->inverse)
+    {
+        arm_scale_f32((float*)inout,cfg->instance->fftLen,(float*)inout,cfg->instance->fftLen*2);
+    }
+
+#endif
+}