From: drowe67 <drowe67@01035d8c-6547-0410-b346-abe4f91aad63>
Date: Mon, 10 Oct 2016 06:24:45 +0000 (+0000)
Subject: Large patch from Danilo to (i) change some #define names to make them codec2-specific... 
X-Git-Url: http://git.whiteaudio.com/gitweb/?a=commitdiff_plain;h=6277d201964b87c6327e8e43a3d54c2edaa7d533;p=freetel-svn-tracking.git

Large patch from Danilo to (i) change some #define names to make them codec2-specific (ii) add an abstraction layer for ffts (iii) add ARM-specific FFTs for stm32 that doesn't help CPU laod but helps memory

git-svn-id: https://svn.code.sf.net/p/freetel/code@2885 01035d8c-6547-0410-b346-abe4f91aad63
---

diff --git a/codec2-dev/CMakeLists.txt b/codec2-dev/CMakeLists.txt
index 41d2b001..36bb52da 100644
--- a/codec2-dev/CMakeLists.txt
+++ b/codec2-dev/CMakeLists.txt
@@ -53,7 +53,7 @@ endif()
 
 # Set default C++ flags.
 set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -Wall")
-set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -g -O2")
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -g -O2 -std=gnu11")
 set(CMAKE_C_FLAGS_DEBUG "${CMAKE_C_FLAGS}")
 set(CMAKE_CXX_FLAGS_DEBUG "${CMAKE_CXX_FLAGS}")
 set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS}")
diff --git a/codec2-dev/src/CMakeLists.txt b/codec2-dev/src/CMakeLists.txt
index df845fed..978f3d62 100644
--- a/codec2-dev/src/CMakeLists.txt
+++ b/codec2-dev/src/CMakeLists.txt
@@ -137,6 +137,7 @@ set(CODEC2_SRCS
     postfilter.c
     sine.c
     codec2.c
+    codec2_fft.c
     cohpsk.c
     fifo.c
     fdmdv.c
@@ -205,16 +206,16 @@ target_link_libraries(c2dec ${CMAKE_REQUIRED_LIBRARIES} codec2)
 add_executable(c2sim c2sim.c ampexp.c phaseexp.c)
 target_link_libraries(c2sim ${CMAKE_REQUIRED_LIBRARIES} codec2)
 
-add_executable(fdmdv_get_test_bits fdmdv_get_test_bits.c fdmdv.c kiss_fft.c)
+add_executable(fdmdv_get_test_bits fdmdv_get_test_bits.c fdmdv.c kiss_fft.c codec2_fft.c kiss_fftr.c)
 target_link_libraries(fdmdv_get_test_bits ${CMAKE_REQUIRED_LIBRARIES})
 
-add_executable(fdmdv_mod fdmdv_mod.c fdmdv.c kiss_fft.c)
+add_executable(fdmdv_mod fdmdv_mod.c fdmdv.c kiss_fft.c codec2_fft.c kiss_fftr.c)
 target_link_libraries(fdmdv_mod ${CMAKE_REQUIRED_LIBRARIES})
 
-add_executable(fdmdv_demod fdmdv_demod.c fdmdv.c kiss_fft.c octave.c modem_stats.c)
+add_executable(fdmdv_demod fdmdv_demod.c fdmdv.c kiss_fft.c octave.c modem_stats.c codec2_fft.c kiss_fftr.c)
 target_link_libraries(fdmdv_demod ${CMAKE_REQUIRED_LIBRARIES})
 
-add_executable(fdmdv_put_test_bits fdmdv_put_test_bits.c fdmdv.c kiss_fft.c)
+add_executable(fdmdv_put_test_bits fdmdv_put_test_bits.c fdmdv.c kiss_fft.c codec2_fft.c kiss_fftr.c)
 target_link_libraries(fdmdv_put_test_bits ${CMAKE_REQUIRED_LIBRARIES})
 
 add_executable(fdmdv_channel fdmdv_channel.c)
diff --git a/codec2-dev/src/c2sim.c b/codec2-dev/src/c2sim.c
index 6cb5d7d5..5af1c8b5 100644
--- a/codec2-dev/src/c2sim.c
+++ b/codec2-dev/src/c2sim.c
@@ -68,12 +68,12 @@ int main(int argc, char *argv[])
     short buf[N_SAMP];	/* input/output buffer                   */
     float buf_float[N_SAMP];
     float buf_float_bpf[N_SAMP];
-    float Sn[M];	/* float input speech samples            */
+    float Sn[M_PITCH];	/* float input speech samples            */
     float Sn_pre[N_SAMP];	/* pre-emphasised input speech samples   */
     COMP  Sw[FFT_ENC];	/* DFT of Sn[]                           */
-    kiss_fft_cfg  fft_fwd_cfg;
-    kiss_fft_cfg  fft_inv_cfg;
-    float w[M];	        /* time domain hamming window            */
+    codec2_fft_cfg  fft_fwd_cfg;
+    codec2_fft_cfg  fft_inv_cfg;
+    float w[M_PITCH];	        /* time domain hamming window            */
     COMP  W[FFT_ENC];	/* DFT of w[]                            */
     MODEL model;
     float Pn[2*N_SAMP];	/* trapezoidal synthesis window          */
@@ -192,7 +192,7 @@ int main(int argc, char *argv[])
     int num_opts=sizeof(long_options)/sizeof(struct option);
     COMP Aw[FFT_ENC];
 
-    for(i=0; i<M; i++) {
+    for(i=0; i<M_PITCH; i++) {
 	Sn[i] = 1.0;
 	Sn_pre[i] = 1.0;
     }
@@ -214,7 +214,7 @@ int main(int argc, char *argv[])
     e = prev_e = 1;
     hpf_states[0] = hpf_states[1] = 0.0;
 
-    nlp_states = nlp_create(M);
+    nlp_states = nlp_create(M_PITCH);
 
     if (argc < 2) {
         print_help(long_options, num_opts, argv);
@@ -397,8 +397,8 @@ int main(int argc, char *argv[])
 
     /* Initialise ------------------------------------------------------------*/
 
-    fft_fwd_cfg = kiss_fft_alloc(FFT_ENC, 0, NULL, NULL); /* fwd FFT,used in several places   */
-    fft_inv_cfg = kiss_fft_alloc(FFT_DEC, 1, NULL, NULL); /* inverse FFT, used just for synth */
+    fft_fwd_cfg = codec2_fft_alloc(FFT_ENC, 0, NULL, NULL); /* fwd FFT,used in several places   */
+    fft_inv_cfg = codec2_fft_alloc(FFT_DEC, 1, NULL, NULL); /* inverse FFT, used just for synth */
     make_analysis_window(fft_fwd_cfg, w, W);
     make_synthesis_window(Pn);
     quantise_init();
@@ -465,11 +465,11 @@ int main(int argc, char *argv[])
 
         /* shift buffer of input samples, and insert new samples */
 
-	for(i=0; i<M-N_SAMP; i++) {
+	for(i=0; i<M_PITCH-N_SAMP; i++) {
 	    Sn[i] = Sn[i+N_SAMP];
 	}
 	for(i=0; i<N_SAMP; i++) {
-	    Sn[i+M-N_SAMP] = buf_float[i];
+	    Sn[i+M_PITCH-N_SAMP] = buf_float[i];
         }
 
 	/*------------------------------------------------------------*\
@@ -517,7 +517,7 @@ int main(int argc, char *argv[])
 	\*------------------------------------------------------------*/
 
 	if (phase0) {
-	    float Wn[M];		        /* windowed speech samples */
+	    float Wn[M_PITCH];		        /* windowed speech samples */
 	    float Rk[order+1];                  /* autocorrelation coeffs  */
             COMP a[FFT_ENC];
 
@@ -527,14 +527,18 @@ int main(int argc, char *argv[])
 
 	    /* find aks here, these are overwritten if LPC modelling is enabled */
 
-            for(i=0; i<M; i++)
+            for(i=0; i<M_PITCH; i++)
                 Wn[i] = Sn[i]*w[i];
-	    autocorrelate(Wn,Rk,M,order);
+	    autocorrelate(Wn,Rk,M_PITCH,order);
 	    levinson_durbin(Rk,ak,order);
 
 	    /* determine voicing */
 
+	    #if 0
 	    snr = est_voicing_mbe(&model, Sw, W, Sw_, Ew);
+            #else
+	    snr = est_voicing_mbe(&model, Sw, W);
+            #endif
 
 	    if (dump_pitch_e)
 		fprintf(fjvm, "%f %f %d ", model.Wo, snr, model.voiced);
@@ -591,7 +595,7 @@ int main(int argc, char *argv[])
 	    if (e < 0.0) {
 		int i;
 		FILE*f=fopen("x.txt","wt");
-		for(i=0; i<M; i++)
+		for(i=0; i<M_PITCH; i++)
 		    fprintf(f,"%f\n", Sn[i]);
 		fclose(f);
 		printf("e = %f frames = %d\n", e, frames);
diff --git a/codec2-dev/src/codec2.c b/codec2-dev/src/codec2.c
index 0111f7f0..674aa621 100644
--- a/codec2-dev/src/codec2.c
+++ b/codec2-dev/src/codec2.c
@@ -33,6 +33,7 @@
 #include <math.h>
 
 #include "defines.h"
+#include "codec2_fft.h"
 #include "sine.h"
 #include "nlp.h"
 #include "dump.h"
@@ -117,16 +118,16 @@ struct CODEC2 * codec2_create(int mode)
 	return NULL;
 
     c2->mode = mode;
-    for(i=0; i<M; i++)
+    for(i=0; i<M_PITCH; i++)
 	c2->Sn[i] = 1.0;
     c2->hpf_states[0] = c2->hpf_states[1] = 0.0;
     for(i=0; i<2*N_SAMP; i++)
 	c2->Sn_[i] = 0;
-    c2->fft_fwd_cfg = kiss_fft_alloc(FFT_ENC, 0, NULL, NULL);
-    c2->fftr_fwd_cfg = kiss_fftr_alloc(FFT_ENC, 0, NULL, NULL);
+    c2->fft_fwd_cfg = codec2_fft_alloc(FFT_ENC, 0, NULL, NULL);
+    c2->fftr_fwd_cfg = codec2_fftr_alloc(FFT_ENC, 0, NULL, NULL);
     make_analysis_window(c2->fft_fwd_cfg, c2->w,c2->W);
     make_synthesis_window(c2->Pn);
-    c2->fft_inv_cfg = kiss_fft_alloc(FFT_DEC, 1, NULL, NULL);
+    c2->fftr_inv_cfg = codec2_fftr_alloc(FFT_DEC, 1, NULL, NULL);
     quantise_init();
     c2->prev_Wo_enc = 0.0;
     c2->bg_est = 0.0;
@@ -143,7 +144,7 @@ struct CODEC2 * codec2_create(int mode)
     }
     c2->prev_e_dec = 1;
 
-    c2->nlp = nlp_create(M);
+    c2->nlp = nlp_create(M_PITCH);
     if (c2->nlp == NULL) {
 	free (c2);
 	return NULL;
@@ -186,9 +187,9 @@ void codec2_destroy(struct CODEC2 *c2)
     assert(c2 != NULL);
     free(c2->bpf_buf);
     nlp_destroy(c2->nlp);
-    KISS_FFT_FREE(c2->fft_fwd_cfg);
-    KISS_FFT_FREE(c2->fftr_fwd_cfg);
-    KISS_FFT_FREE(c2->fft_inv_cfg);
+    codec2_fft_free(c2->fft_fwd_cfg);
+    codec2_fftr_free(c2->fftr_fwd_cfg);
+    codec2_fftr_free(c2->fftr_inv_cfg);
     free(c2);
 }
 
@@ -1855,7 +1856,7 @@ void synthesise_one_frame(struct CODEC2 *c2, short speech[], MODEL *model, COMP
 
     PROFILE_SAMPLE_AND_LOG(synth_start, pf_start, "    postfilter");
 
-    synthesise(c2->fft_inv_cfg, c2->Sn_, model, c2->Pn, 1);
+    synthesise(c2->fftr_inv_cfg, c2->Sn_, model, c2->Pn, 1);
 
     PROFILE_SAMPLE_AND_LOG2(synth_start, "    synth");
 
@@ -1886,18 +1887,16 @@ void synthesise_one_frame(struct CODEC2 *c2, short speech[], MODEL *model, COMP
 void analyse_one_frame(struct CODEC2 *c2, MODEL *model, short speech[])
 {
     COMP    Sw[FFT_ENC];
-    COMP    Sw_[FFT_ENC];
-    COMP    Ew[FFT_ENC];
     float   pitch;
     int     i;
     PROFILE_VAR(dft_start, nlp_start, model_start, two_stage, estamps);
 
     /* Read input speech */
 
-    for(i=0; i<M-N_SAMP; i++)
+    for(i=0; i<M_PITCH-N_SAMP; i++)
       c2->Sn[i] = c2->Sn[i+N_SAMP];
     for(i=0; i<N_SAMP; i++)
-      c2->Sn[i+M-N_SAMP] = speech[i];
+      c2->Sn[i+M_PITCH-N_SAMP] = speech[i];
 
     PROFILE_SAMPLE(dft_start);
     dft_speech(c2->fft_fwd_cfg, Sw, c2->Sn, c2->w);
@@ -1917,7 +1916,7 @@ void analyse_one_frame(struct CODEC2 *c2, MODEL *model, short speech[])
     PROFILE_SAMPLE_AND_LOG(two_stage, model_start, "    two_stage");
     estimate_amplitudes(model, Sw, c2->W, 0);
     PROFILE_SAMPLE_AND_LOG(estamps, two_stage, "    est_amps");
-    est_voicing_mbe(model, Sw, c2->W, Sw_, Ew);
+    est_voicing_mbe(model, Sw, c2->W);
     c2->prev_Wo_enc = model->Wo;
     PROFILE_SAMPLE_AND_LOG2(estamps, "    est_voicing");
     #ifdef DUMP
diff --git a/codec2-dev/src/codec2_fft.c b/codec2-dev/src/codec2_fft.c
new file mode 100644
index 00000000..3a5eb475
--- /dev/null
+++ b/codec2-dev/src/codec2_fft.c
@@ -0,0 +1,151 @@
+/*
+ * codec2_fft.c
+ *
+ *  Created on: 24.09.2016
+ *      Author: danilo
+ */
+
+#include "codec2_fft.h"
+#ifdef USE_KISS_FFT
+#include "_kiss_fft_guts.h"
+
+#else
+#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
+
+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(inout,cfg->instance->fftLen,inout,cfg->instance->fftLen*2);
+    }
+
+#endif
+}
diff --git a/codec2-dev/src/codec2_fft.h b/codec2-dev/src/codec2_fft.h
new file mode 100644
index 00000000..d421c474
--- /dev/null
+++ b/codec2-dev/src/codec2_fft.h
@@ -0,0 +1,110 @@
+/*
+ * codec2_fft.h
+ *
+ *  Created on: 17.09.2016
+ *      Author: danilo
+ */
+
+#ifndef DRIVERS_FREEDV_CODEC2_FFT_H_
+#define DRIVERS_FREEDV_CODEC2_FFT_H_
+
+#include <assert.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#ifdef ARM_MATH_CM4
+  #include "stm32f4xx.h"
+  #include "core_cm4.h"
+  #include "arm_math.h"
+  #include "arm_const_structs.h"
+#endif
+
+#include "defines.h"
+#include "comp.h"
+
+#ifndef ARM_MATH_CM4
+    #define USE_KISS_FFT
+#endif
+// #define USE_KISS_FFT
+
+
+typedef COMP    codec2_fft_cpx;
+#include "kiss_fftr.h"
+
+#ifdef USE_KISS_FFT
+    #include "kiss_fft.h"
+    typedef kiss_fftr_cfg codec2_fftr_cfg;
+    typedef kiss_fft_cfg codec2_fft_cfg;
+    typedef kiss_fft_scalar codec2_fft_scalar;
+#else
+  typedef float32_t codec2_fft_scalar;
+  typedef struct {
+      arm_rfft_fast_instance_f32* instance;
+      int inverse;
+  } codec2_fftr_struct;
+
+  typedef codec2_fftr_struct* codec2_fftr_cfg;
+
+  typedef struct {
+        const arm_cfft_instance_f32* instance;
+        int inverse;
+    } codec2_fft_struct;
+  typedef codec2_fft_struct* codec2_fft_cfg;
+#endif
+
+
+
+static inline void codec2_fftr(codec2_fftr_cfg cfg, codec2_fft_scalar* in, codec2_fft_cpx* out)
+{
+
+#ifdef USE_KISS_FFT
+      kiss_fftr(cfg, in, (kiss_fft_cpx*)out);
+#else
+    arm_rfft_fast_f32(cfg->instance,in,(float*)out,cfg->inverse);
+    out->imag = 0; // remove out[FFT_ENC/2]->real stored in out[0].imag
+#endif
+}
+
+static inline void codec2_fftri(codec2_fftr_cfg cfg, codec2_fft_cpx* in, codec2_fft_scalar* out)
+{
+#ifdef USE_KISS_FFT
+      kiss_fftri(cfg, (kiss_fft_cpx*)in, out);
+#else
+    arm_rfft_fast_f32(cfg->instance,(float*)in,out,cfg->inverse);
+    // arm_scale_f32(out,cfg->instance->fftLenRFFT,out,cfg->instance->fftLenRFFT);
+#endif
+
+}
+
+codec2_fft_cfg codec2_fft_alloc(int nfft, int inverse_fft, void* mem, size_t* lenmem);
+codec2_fftr_cfg codec2_fftr_alloc(int nfft, int inverse_fft, void* mem, size_t* lenmem);
+void codec2_fft_free(codec2_fft_cfg cfg);
+void codec2_fftr_free(codec2_fftr_cfg cfg);
+
+
+static inline void codec2_fft(codec2_fft_cfg cfg, codec2_fft_cpx* in, codec2_fft_cpx* out)
+{
+
+#ifdef USE_KISS_FFT
+      kiss_fft(cfg, (kiss_fft_cpx*)in, (kiss_fft_cpx*)out);
+#else
+    memcpy(out,in,cfg->instance->fftLen*2*sizeof(float));
+    arm_cfft_f32(cfg->instance,(float*)out,cfg->inverse,0);
+    // TODO: this is not nice, but for now required to keep changes minimal
+    // however, since main goal is to reduce the memory usage
+    // we should convert to an in place interface
+    // on PC like platforms the overhead of using the "inplace" kiss_fft calls
+    // is neglectable compared to the gain in memory usage on STM32 platforms
+    if (cfg->inverse)
+    {
+        arm_scale_f32((float*)out,cfg->instance->fftLen,(float*)out,cfg->instance->fftLen*2);
+    }
+#endif
+}
+
+void codec2_fft_inplace(codec2_fft_cfg cfg, codec2_fft_cpx* inout);
+
+
+#endif
diff --git a/codec2-dev/src/codec2_internal.h b/codec2-dev/src/codec2_internal.h
index 57f27cbc..cb516cc4 100644
--- a/codec2-dev/src/codec2_internal.h
+++ b/codec2-dev/src/codec2_internal.h
@@ -29,20 +29,22 @@
 #ifndef __CODEC2_INTERNAL__
 #define __CODEC2_INTERNAL__
 
+#include "codec2_fft.h"
+
 struct CODEC2 {
     int           mode;
-    kiss_fft_cfg  fft_fwd_cfg;             /* forward FFT config                        */
-    kiss_fftr_cfg fftr_fwd_cfg;            /* forward real FFT config                   */
-    float         w[M];	                   /* time domain hamming window                */
+    codec2_fft_cfg  fft_fwd_cfg;             /* forward FFT config                        */
+    codec2_fftr_cfg fftr_fwd_cfg;            /* forward real FFT config                   */
+    float         w[M_PITCH];	                   /* time domain hamming window                */
     COMP          W[FFT_ENC];	           /* DFT of w[]                                */
     float         Pn[2*N_SAMP];	           /* trapezoidal synthesis window              */
     float        *bpf_buf;                 /* buffer for band pass filter               */
-    float         Sn[M];                   /* input speech                              */
+    float         Sn[M_PITCH];                   /* input speech                              */
     float         hpf_states[2];           /* high pass filter states                   */
     void         *nlp;                     /* pitch predictor states                    */
     int           gray;                    /* non-zero for gray encoding                */
 
-    kiss_fft_cfg  fft_inv_cfg;             /* inverse FFT config                        */
+    codec2_fftr_cfg  fftr_inv_cfg;             /* inverse FFT config                        */
     float         Sn_[2*N_SAMP];	           /* synthesised output speech                 */
     float         ex_phase;                /* excitation model phase track              */
     float         bg_est;                  /* background noise estimate for post filter */
diff --git a/codec2-dev/src/defines.h b/codec2-dev/src/defines.h
index 67e65c4b..9ec7235d 100644
--- a/codec2-dev/src/defines.h
+++ b/codec2-dev/src/defines.h
@@ -55,7 +55,7 @@
 
 /* Pitch estimation defines */
 
-#define M        320		/* pitch analysis frame size            */
+#define M_PITCH        320		/* pitch analysis frame size            */
 #define P_MIN    20		/* minimum pitch                        */
 #define P_MAX    160		/* maximum pitch                        */
 
diff --git a/codec2-dev/src/dump.h b/codec2-dev/src/dump.h
index 58c93102..f4339ca2 100644
--- a/codec2-dev/src/dump.h
+++ b/codec2-dev/src/dump.h
@@ -28,8 +28,7 @@
 
 #include "defines.h"
 #include "comp.h"
-#include "kiss_fft.h"
-#include "kiss_fftr.h"
+#include "codec2_fft.h"
 #include "codec2_internal.h"
 
 void dump_on(char filename_prefix[]);
diff --git a/codec2-dev/src/fdmdv.c b/codec2-dev/src/fdmdv.c
index eb2774fd..ee075016 100644
--- a/codec2-dev/src/fdmdv.c
+++ b/codec2-dev/src/fdmdv.c
@@ -51,7 +51,7 @@
 #include "rxdec_coeff.h"
 #include "test_bits.h"
 #include "pilot_coeff.h"
-#include "kiss_fft.h"
+#include "codec2_fft.h"
 #include "hanning.h"
 #include "os.h"
 #include "machdep.h"
@@ -159,7 +159,7 @@ struct FDMDV * fdmdv_create(int Nc)
 
     /* freq Offset estimation states */
 
-    f->fft_pilot_cfg = kiss_fft_alloc (MPILOTFFT, 0, NULL, NULL);
+    f->fft_pilot_cfg = codec2_fft_alloc (MPILOTFFT, 0, NULL, NULL);
     assert(f->fft_pilot_cfg != NULL);
 
     for(i=0; i<NPILOTBASEBAND; i++) {
@@ -218,7 +218,7 @@ struct FDMDV * fdmdv_create(int Nc)
 void fdmdv_destroy(struct FDMDV *fdmdv)
 {
     assert(fdmdv != NULL);
-    KISS_FFT_FREE(fdmdv->fft_pilot_cfg);
+    codec2_fft_free(fdmdv->fft_pilot_cfg);
     free(fdmdv->rx_test_bits_mem);
     free(fdmdv);
 }
@@ -713,12 +713,11 @@ void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq)
 \*---------------------------------------------------------------------------*/
 
 void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[],
-		   COMP pilot_lpf[], kiss_fft_cfg fft_pilot_cfg, COMP S[], int nin,
+		   COMP pilot_lpf[], codec2_fft_cfg fft_pilot_cfg, COMP S[], int nin,
                    int do_fft)
 {
     int   i,j,k;
     int   mpilot;
-    COMP  s[MPILOTFFT];
     float mag, imax;
     int   ix;
     float r;
@@ -770,16 +769,12 @@ void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[],
     if (do_fft) {
 
         /* decimate to improve DFT resolution, window and DFT */
-
         mpilot = FS/(2*200);  /* calc decimation rate given new sample rate is twice LPF freq */
-        for(i=0; i<MPILOTFFT; i++) {
-            s[i].real = 0.0; s[i].imag = 0.0;
-        }
         for(i=0,j=0; i<NPILOTLPF; i+=mpilot,j++) {
-            s[j] = fcmult(hanning[i], pilot_lpf[i]);
+            S[j] = fcmult(hanning[i], pilot_lpf[i]);
         }
 
-        kiss_fft(fft_pilot_cfg, (kiss_fft_cpx *)s, (kiss_fft_cpx *)S);
+        codec2_fft_inplace(fft_pilot_cfg, S);
 
         /* peak pick and convert to Hz */
 
diff --git a/codec2-dev/src/fdmdv_internal.h b/codec2-dev/src/fdmdv_internal.h
index 0946db9e..3f830bad 100644
--- a/codec2-dev/src/fdmdv_internal.h
+++ b/codec2-dev/src/fdmdv_internal.h
@@ -31,7 +31,7 @@
 
 #include "comp.h"
 #include "codec2_fdmdv.h"
-#include "kiss_fft.h"
+#include "codec2_fft.h"
 
 /*---------------------------------------------------------------------------*\
 
@@ -107,7 +107,7 @@ struct FDMDV {
 
     /* freq offset estimation states */
 
-    kiss_fft_cfg fft_pilot_cfg;
+    codec2_fft_cfg fft_pilot_cfg;
     COMP pilot_baseband1[NPILOTBASEBAND];
     COMP pilot_baseband2[NPILOTBASEBAND];
     COMP pilot_lpf1[NPILOTLPF];
@@ -171,7 +171,7 @@ void tx_filter_and_upconvert(COMP tx_fdm[], int Nc, COMP tx_symbols[],
 void generate_pilot_fdm(COMP *pilot_fdm, int *bit, float *symbol, float *filter_mem, COMP *phase, COMP *freq);
 void generate_pilot_lut(COMP pilot_lut[], COMP *pilot_freq);
 float rx_est_freq_offset(struct FDMDV *f, COMP rx_fdm[], int nin, int do_fft);
-void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[], COMP pilot_lpf[], kiss_fft_cfg fft_pilot_cfg, COMP S[], int nin, int do_fft);
+void lpf_peak_pick(float *foff, float *max, COMP pilot_baseband[], COMP pilot_lpf[], codec2_fft_cfg fft_pilot_cfg, COMP S[], int nin, int do_fft);
 void fdm_downconvert(COMP rx_baseband[NC+1][M_FAC+M_FAC/P], int Nc, COMP rx_fdm[], COMP phase_rx[], COMP freq[], int nin);
 void rxdec_filter(COMP rx_fdm_filter[], COMP rx_fdm[], COMP rxdec_lpf_mem[], int nin);
 void rx_filter(COMP rx_filt[NC+1][P+1], int Nc, COMP rx_baseband[NC+1][M_FAC+M_FAC/P], COMP rx_filter_memory[NC+1][NFILTER], int nin);
diff --git a/codec2-dev/src/interp.c b/codec2-dev/src/interp.c
index 8c56305f..1bb74c05 100644
--- a/codec2-dev/src/interp.c
+++ b/codec2-dev/src/interp.c
@@ -148,7 +148,7 @@ float sample_log_amp(MODEL *model, float w)
 \*---------------------------------------------------------------------------*/
 
 void interpolate_lsp(
-  kiss_fft_cfg  fft_fwd_cfg,
+  codec2_fft_cfg  fft_fwd_cfg,
   MODEL *interp,    /* interpolated model params                     */
   MODEL *prev,      /* previous frames model params                  */
   MODEL *next,      /* next frames model params                      */
@@ -282,8 +282,8 @@ void interp_Wo2(
 
 float interp_energy(float prev_e, float next_e)
 {
-    return powf(10.0, (log10f(prev_e) + log10f(next_e))/2.0);
-
+    //return powf(10.0, (log10f(prev_e) + log10f(next_e))/2.0);
+    return sqrtf(prev_e * next_e); //looks better is math. identical and faster math
 }
 
 
diff --git a/codec2-dev/src/nlp.c b/codec2-dev/src/nlp.c
index 6c9b61b6..3321375c 100644
--- a/codec2-dev/src/nlp.c
+++ b/codec2-dev/src/nlp.c
@@ -28,7 +28,7 @@
 #include "defines.h"
 #include "nlp.h"
 #include "dump.h"
-#include "kiss_fft.h"
+#include "codec2_fft.h"
 #undef PROFILE
 #include "machdep.h"
 
@@ -121,7 +121,7 @@ typedef struct {
     float         sq[PMAX_M];	     /* squared speech samples       */
     float         mem_x,mem_y;       /* memory for notch filter      */
     float         mem_fir[NLP_NTAP]; /* decimation FIR filter memory */
-    kiss_fft_cfg  fft_cfg;           /* kiss FFT config              */
+    codec2_fft_cfg  fft_cfg;           /* kiss FFT config              */
 } NLP;
 
 #ifdef POST_PROCESS_MBE
@@ -165,7 +165,7 @@ int    m			/* analysis window size */
     for(i=0; i<NLP_NTAP; i++)
 	nlp->mem_fir[i] = 0.0;
 
-    nlp->fft_cfg = kiss_fft_alloc (PE_FFT_SIZE, 0, NULL, NULL);
+    nlp->fft_cfg = codec2_fft_alloc (PE_FFT_SIZE, 0, NULL, NULL);
     assert(nlp->fft_cfg != NULL);
 
     return (void*)nlp;
@@ -185,7 +185,7 @@ void nlp_destroy(void *nlp_state)
     assert(nlp_state != NULL);
     nlp = (NLP*)nlp_state;
 
-    KISS_FFT_FREE(nlp->fft_cfg);
+    codec2_fft_free(nlp->fft_cfg);
     free(nlp_state);
 }
 
@@ -233,8 +233,7 @@ float nlp(
 {
     NLP   *nlp;
     float  notch;		    /* current notch filter output    */
-    COMP   fw[PE_FFT_SIZE];	    /* DFT of squared signal (input)  */
-    COMP   Fw[PE_FFT_SIZE];	    /* DFT of squared signal (output) */
+    COMP   Fw[PE_FFT_SIZE];	    /* DFT of squared signal (input/output) */
     float  gmax;
     int    gmax_bin;
     int    m, i,j;
@@ -285,18 +284,20 @@ float nlp(
     /* Decimate and DFT */
 
     for(i=0; i<PE_FFT_SIZE; i++) {
-	fw[i].real = 0.0;
-	fw[i].imag = 0.0;
+	Fw[i].real = 0.0;
+	Fw[i].imag = 0.0;
     }
     for(i=0; i<m/DEC; i++) {
-	fw[i].real = nlp->sq[i*DEC]*nlp->w[i];
+	Fw[i].real = nlp->sq[i*DEC]*nlp->w[i];
     }
     PROFILE_SAMPLE_AND_LOG(window, filter, "      window");
     #ifdef DUMP
     dump_dec(Fw);
     #endif
 
-    kiss_fft(nlp->fft_cfg, (kiss_fft_cpx *)fw, (kiss_fft_cpx *)Fw);
+    // FIXME: check if this can be converted to a real fft
+    // since all imag inputs are 0
+    codec2_fft_inplace(nlp->fft_cfg, Fw);
     PROFILE_SAMPLE_AND_LOG(fft, window, "      fft");
 
     for(i=0; i<PE_FFT_SIZE; i++)
diff --git a/codec2-dev/src/phase.c b/codec2-dev/src/phase.c
index 60037e18..65c81741 100644
--- a/codec2-dev/src/phase.c
+++ b/codec2-dev/src/phase.c
@@ -129,7 +129,7 @@
 \*---------------------------------------------------------------------------*/
 
 void phase_synth_zero_order(
-    kiss_fft_cfg fft_fwd_cfg,
+    codec2_fft_cfg fft_fwd_cfg,
     MODEL *model,
     float *ex_phase,            /* excitation phase of fundamental */
     COMP   A[]
diff --git a/codec2-dev/src/phase.h b/codec2-dev/src/phase.h
index 174582a6..65cd1e57 100644
--- a/codec2-dev/src/phase.h
+++ b/codec2-dev/src/phase.h
@@ -28,10 +28,10 @@
 #ifndef __PHASE__
 #define __PHASE__
 
-#include "kiss_fft.h"
+#include "codec2_fft.h"
 #include "comp.h"
 
-void phase_synth_zero_order(kiss_fft_cfg fft_dec_cfg,
+void phase_synth_zero_order(codec2_fft_cfg fft_dec_cfg,
 			    MODEL *model,
                             float *ex_phase,
                             COMP   A[]);
diff --git a/codec2-dev/src/quantise.c b/codec2-dev/src/quantise.c
index 23a8868f..e57ffa30 100644
--- a/codec2-dev/src/quantise.c
+++ b/codec2-dev/src/quantise.c
@@ -36,7 +36,7 @@
 #include "quantise.h"
 #include "lpc.h"
 #include "lsp.h"
-#include "kiss_fft.h"
+#include "codec2_fft.h"
 #undef PROFILE
 #include "machdep.h"
 
@@ -846,7 +846,7 @@ void force_min_lsp_dist(float lsp[], int order)
 
 \*---------------------------------------------------------------------------*/
 
-void lpc_post_filter(kiss_fftr_cfg fftr_fwd_cfg, float Pw[], float ak[],
+void lpc_post_filter(codec2_fftr_cfg fftr_fwd_cfg, float Pw[], float ak[],
                      int order, int dump, float beta, float gamma, int bass_boost, float E)
 {
     int   i;
@@ -873,7 +873,7 @@ void lpc_post_filter(kiss_fftr_cfg fftr_fwd_cfg, float Pw[], float ak[],
 	x[i] = ak[i] * coeff;
         coeff *= gamma;
     }
-    kiss_fftr(fftr_fwd_cfg, (kiss_fft_scalar *)x, (kiss_fft_cpx *)Ww);
+    codec2_fftr(fftr_fwd_cfg, x, Ww);
 
     PROFILE_SAMPLE_AND_LOG(tfft2, taw, "        fft2");
 
@@ -956,7 +956,7 @@ void lpc_post_filter(kiss_fftr_cfg fftr_fwd_cfg, float Pw[], float ak[],
 \*---------------------------------------------------------------------------*/
 
 void aks_to_M2(
-  kiss_fftr_cfg  fftr_fwd_cfg,
+  codec2_fftr_cfg  fftr_fwd_cfg,
   float         ak[],	     /* LPC's */
   int           order,
   MODEL        *model,	     /* sinusoidal model parameters for this frame */
@@ -993,7 +993,7 @@ void aks_to_M2(
 
       for(i=0; i<=order; i++)
           a[i] = ak[i];
-      kiss_fftr(fftr_fwd_cfg, (kiss_fft_scalar *)a, (kiss_fft_cpx *)Aw);
+      codec2_fftr(fftr_fwd_cfg, a, Aw);
   }
   PROFILE_SAMPLE_AND_LOG(tfft, tstart, "      fft");
 
@@ -1047,6 +1047,16 @@ void aks_to_M2(
   for(m=1; m<=model->L; m++) {
       am = (int)((m - 0.5)*model->Wo/r + 0.5);
       bm = (int)((m + 0.5)*model->Wo/r + 0.5);
+
+      // FIXME: With arm_rfft_fast_f32 we have to use this
+      // otherwise sometimes a to high bm is calculated
+      // which causes trouble later in the calculation
+      // chain
+      // it seems for some reason model->Wo is calculated somewhat too high
+      if (bm>FFT_ENC/2)
+      {
+          bm = FFT_ENC/2;
+      }
       Em = 0.0;
 
       for(i=am; i<bm; i++)
@@ -1070,7 +1080,6 @@ void aks_to_M2(
           if (Am < model->A[m])
               Am *= 1.4;
       }
-
       model->A[m] = Am;
   }
   *snr = 10.0*log10f(signal/noise);
@@ -1274,12 +1283,12 @@ float speech_to_uq_lsps(float lsp[],
 )
 {
     int   i, roots;
-    float Wn[M];
+    float Wn[M_PITCH];
     float R[order+1];
     float e, E;
 
     e = 0.0;
-    for(i=0; i<M; i++) {
+    for(i=0; i<M_PITCH; i++) {
 	Wn[i] = Sn[i]*w[i];
 	e += Wn[i]*Wn[i];
     }
@@ -1292,7 +1301,7 @@ float speech_to_uq_lsps(float lsp[],
 	return 0.0;
     }
 
-    autocorrelate(Wn, R, M, order);
+    autocorrelate(Wn, R, M_PITCH, order);
     levinson_durbin(R, ak, order);
 
     E = 0.0;
@@ -1932,7 +1941,7 @@ float decode_energy(int index, int bits)
 
 \*---------------------------------------------------------------------------*/
 
-float decode_amplitudes(kiss_fft_cfg  fft_fwd_cfg,
+float decode_amplitudes(codec2_fft_cfg  fft_fwd_cfg,
 			MODEL *model,
 			float  ak[],
 		        int    lsp_indexes[],
diff --git a/codec2-dev/src/quantise.h b/codec2-dev/src/quantise.h
index 2036c704..bc8c056a 100644
--- a/codec2-dev/src/quantise.h
+++ b/codec2-dev/src/quantise.h
@@ -26,8 +26,7 @@
 #ifndef __QUANTISE__
 #define __QUANTISE__
 
-#include "kiss_fft.h"
-#include "kiss_fftr.h"
+#include "codec2_fft.h"
 #include "comp.h"
 
 #define WO_BITS     7
@@ -57,7 +56,7 @@
 void quantise_init();
 float lpc_model_amplitudes(float Sn[], float w[], MODEL *model, int order,
 			   int lsp,float ak[]);
-void aks_to_M2(kiss_fftr_cfg fftr_fwd_cfg, float ak[], int order, MODEL *model,
+void aks_to_M2(codec2_fftr_cfg fftr_fwd_cfg, float ak[], int order, MODEL *model,
 	       float E, float *snr, int dump, int sim_pf,
                int pf, int bass_boost, float beta, float gamma, COMP Aw[]);
 
diff --git a/codec2-dev/src/sine.c b/codec2-dev/src/sine.c
index 138486da..10c35e0a 100644
--- a/codec2-dev/src/sine.c
+++ b/codec2-dev/src/sine.c
@@ -66,7 +66,7 @@ void hs_pitch_refinement(MODEL *model, COMP Sw[], float pmin, float pmax,
 
 \*---------------------------------------------------------------------------*/
 
-void make_analysis_window(kiss_fft_cfg fft_fwd_cfg, float w[], COMP W[])
+void make_analysis_window(codec2_fft_cfg fft_fwd_cfg, float w[], COMP W[])
 {
   float m;
   COMP  wshift[FFT_ENC];
@@ -85,20 +85,20 @@ void make_analysis_window(kiss_fft_cfg fft_fwd_cfg, float w[], COMP W[])
   */
 
   m = 0.0;
-  for(i=0; i<M/2-NW/2; i++)
+  for(i=0; i<M_PITCH/2-NW/2; i++)
     w[i] = 0.0;
-  for(i=M/2-NW/2,j=0; i<M/2+NW/2; i++,j++) {
+  for(i=M_PITCH/2-NW/2,j=0; i<M_PITCH/2+NW/2; i++,j++) {
     w[i] = 0.5 - 0.5*cosf(TWO_PI*j/(NW-1));
     m += w[i]*w[i];
   }
-  for(i=M/2+NW/2; i<M; i++)
+  for(i=M_PITCH/2+NW/2; i<M_PITCH; i++)
     w[i] = 0.0;
 
   /* Normalise - makes freq domain amplitude estimation straight
      forward */
 
   m = 1.0/sqrtf(m*FFT_ENC);
-  for(i=0; i<M; i++) {
+  for(i=0; i<M_PITCH; i++) {
     w[i] *= m;
   }
 
@@ -128,11 +128,11 @@ void make_analysis_window(kiss_fft_cfg fft_fwd_cfg, float w[], COMP W[])
     wshift[i].imag = 0.0;
   }
   for(i=0; i<NW/2; i++)
-    wshift[i].real = w[i+M/2];
-  for(i=FFT_ENC-NW/2,j=M/2-NW/2; i<FFT_ENC; i++,j++)
+    wshift[i].real = w[i+M_PITCH/2];
+  for(i=FFT_ENC-NW/2,j=M_PITCH/2-NW/2; i<FFT_ENC; i++,j++)
    wshift[i].real = w[j];
 
-  kiss_fft(fft_fwd_cfg, (kiss_fft_cpx *)wshift, (kiss_fft_cpx *)W);
+  codec2_fft(fft_fwd_cfg, wshift, W);
 
   /*
       Re-arrange W[] to be symmetrical about FFT_ENC/2.  Makes later
@@ -199,14 +199,40 @@ float hpf(float x, float states[])
 
 \*---------------------------------------------------------------------------*/
 
-void dft_speech(kiss_fft_cfg fft_fwd_cfg, COMP Sw[], float Sn[], float w[])
+// TODO: we can either go for a faster FFT using fftr and some stack usage
+// or we can reduce stack usage to almost zero on STM32 by switching to fft_inplace
+#if 1
+void dft_speech(codec2_fft_cfg fft_fwd_cfg, COMP Sw[], float Sn[], float w[])
 {
-  int  i;
-  COMP sw[FFT_ENC];
+    int  i;
+  for(i=0; i<FFT_ENC; i++) {
+    Sw[i].real = 0.0;
+    Sw[i].imag = 0.0;
+  }
+
+  /* Centre analysis window on time axis, we need to arrange input
+     to FFT this way to make FFT phases correct */
+
+  /* move 2nd half to start of FFT input vector */
+
+  for(i=0; i<NW/2; i++)
+    Sw[i].real = Sn[i+M_PITCH/2]*w[i+M_PITCH/2];
+
+  /* move 1st half to end of FFT input vector */
+
+  for(i=0; i<NW/2; i++)
+    Sw[FFT_ENC-NW/2+i].real = Sn[i+M_PITCH/2-NW/2]*w[i+M_PITCH/2-NW/2];
+
+  codec2_fft_inplace(fft_fwd_cfg, Sw);
+}
+#else
+void dft_speech(codec2_fftr_cfg fftr_fwd_cfg, COMP Sw[], float Sn[], float w[])
+{
+    int  i;
+  float sw[FFT_ENC];
 
   for(i=0; i<FFT_ENC; i++) {
-    sw[i].real = 0.0;
-    sw[i].imag = 0.0;
+    sw[i] = 0.0;
   }
 
   /* Centre analysis window on time axis, we need to arrange input
@@ -215,15 +241,17 @@ void dft_speech(kiss_fft_cfg fft_fwd_cfg, COMP Sw[], float Sn[], float w[])
   /* move 2nd half to start of FFT input vector */
 
   for(i=0; i<NW/2; i++)
-    sw[i].real = Sn[i+M/2]*w[i+M/2];
+    sw[i] = Sn[i+M_PITCH/2]*w[i+M_PITCH/2];
 
   /* move 1st half to end of FFT input vector */
 
   for(i=0; i<NW/2; i++)
-    sw[FFT_ENC-NW/2+i].real = Sn[i+M/2-NW/2]*w[i+M/2-NW/2];
+    sw[FFT_ENC-NW/2+i] = Sn[i+M_PITCH/2-NW/2]*w[i+M_PITCH/2-NW/2];
 
-  kiss_fft(fft_fwd_cfg, (kiss_fft_cpx *)sw, (kiss_fft_cpx *)Sw);
+  codec2_fftr(fftr_fwd_cfg, sw, Sw);
 }
+#endif
+
 
 /*---------------------------------------------------------------------------*\
 
@@ -388,12 +416,10 @@ void estimate_amplitudes(MODEL *model, COMP Sw[], COMP W[], int est_phase)
 float est_voicing_mbe(
     MODEL *model,
     COMP   Sw[],
-    COMP   W[],
-    COMP   Sw_[],         /* DFT of all voiced synthesised signal  */
-                          /* useful for debugging/dump file        */
-    COMP   Ew[])          /* DFT of error                          */
+    COMP   W[]
+    )          /* DFT of error                          */
 {
-    int   i,l,al,bl,m;    /* loop variables */
+    int   l,al,bl,m;    /* loop variables */
     COMP  Am;             /* amplitude sample for this band */
     int   offset;         /* centers Hw[] about current harmonic */
     float den;            /* denominator of Am expression */
@@ -402,17 +428,14 @@ float est_voicing_mbe(
     float sig, snr;
     float elow, ehigh, eratio;
     float sixty;
+    COMP   Ew;
+    Ew.real = 0;
+    Ew.imag = 0;
 
     sig = 1E-4;
     for(l=1; l<=model->L/4; l++) {
 	sig += model->A[l]*model->A[l];
     }
-    for(i=0; i<FFT_ENC; i++) {
-	Sw_[i].real = 0.0;
-	Sw_[i].imag = 0.0;
-	Ew[i].real = 0.0;
-	Ew[i].imag = 0.0;
-    }
 
     Wo = model->Wo;
     error = 1E-4;
@@ -442,12 +465,10 @@ float est_voicing_mbe(
 
         offset = FFT_ENC/2 - l*Wo*FFT_ENC/TWO_PI + 0.5;
         for(m=al; m<bl; m++) {
-	    Sw_[m].real = Am.real*W[offset+m].real;
-	    Sw_[m].imag = Am.imag*W[offset+m].real;
-	    Ew[m].real = Sw[m].real - Sw_[m].real;
-	    Ew[m].imag = Sw[m].imag - Sw_[m].imag;
-	    error += Ew[m].real*Ew[m].real;
-	    error += Ew[m].imag*Ew[m].imag;
+	    Ew.real = Sw[m].real - Am.real*W[offset+m].real;
+	    Ew.imag = Sw[m].imag - Am.imag*W[offset+m].real;
+	    error += Ew.real*Ew.real;
+	    error += Ew.imag*Ew.imag;
 	}
     }
 
@@ -549,7 +570,7 @@ void make_synthesis_window(float Pn[])
 \*---------------------------------------------------------------------------*/
 
 void synthesise(
-  kiss_fft_cfg fft_inv_cfg,
+  codec2_fftr_cfg fftr_inv_cfg,
   float  Sn_[],		/* time domain synthesised signal              */
   MODEL *model,		/* ptr to model parameters for this frame      */
   float  Pn[],		/* time domain Parzen window                   */
@@ -557,8 +578,8 @@ void synthesise(
 )
 {
     int   i,l,j,b;	/* loop variables */
-    COMP  Sw_[FFT_DEC];	/* DFT of synthesised signal */
-    COMP  sw_[FFT_DEC];	/* synthesised signal */
+    COMP  Sw_[FFT_DEC/2+1];	/* DFT of synthesised signal */
+    float sw_[FFT_DEC];	/* synthesised signal */
 
     if (shift) {
 	/* Update memories */
@@ -568,7 +589,7 @@ void synthesise(
 	Sn_[N_SAMP-1] = 0.0;
     }
 
-    for(i=0; i<FFT_DEC; i++) {
+    for(i=0; i<FFT_DEC/2+1; i++) {
 	Sw_[i].real = 0.0;
 	Sw_[i].imag = 0.0;
     }
@@ -591,21 +612,19 @@ void synthesise(
 #ifdef FFT_SYNTHESIS
     /* Now set up frequency domain synthesised speech */
     for(l=1; l<=model->L; l++) {
-    //for(l=model->L/2; l<=model->L; l++) {
-    //for(l=1; l<=model->L/4; l++) {
-	b = (int)(l*model->Wo*FFT_DEC/TWO_PI + 0.5);
-	if (b > ((FFT_DEC/2)-1)) {
-		b = (FFT_DEC/2)-1;
-	}
-	Sw_[b].real = model->A[l]*cosf(model->phi[l]);
-	Sw_[b].imag = model->A[l]*sinf(model->phi[l]);
-	Sw_[FFT_DEC-b].real = Sw_[b].real;
-	Sw_[FFT_DEC-b].imag = -Sw_[b].imag;
+        //for(l=model->L/2; l<=model->L; l++) {
+        //for(l=1; l<=model->L/4; l++) {
+        b = (int)(l*model->Wo*FFT_DEC/TWO_PI + 0.5);
+        if (b > ((FFT_DEC/2)-1)) {
+            b = (FFT_DEC/2)-1;
+        }
+        Sw_[b].real = model->A[l]*cosf(model->phi[l]);
+        Sw_[b].imag = model->A[l]*sinf(model->phi[l]);
     }
 
     /* Perform inverse DFT */
 
-    kiss_fft(fft_inv_cfg, (kiss_fft_cpx *)Sw_, (kiss_fft_cpx *)sw_);
+    codec2_fftri(fftr_inv_cfg, Sw_,sw_);
 #else
     /*
        Direct time domain synthesis using the cos() function.  Works
@@ -613,28 +632,32 @@ void synthesise(
        still used to handle overlap-add between adjacent frames.  This
        could be simplified as we don't need to synthesise where Pn[]
        is zero.
-    */
+     */
     for(l=1; l<=model->L; l++) {
-	for(i=0,j=-N_SAMP+1; i<N_SAMP-1; i++,j++) {
-	    Sw_[FFT_DEC-N_SAMP+1+i].real += 2.0*model->A[l]*cosf(j*model->Wo*l + model->phi[l]);
-	}
- 	for(i=N_SAMP-1,j=0; i<2*N_SAMP; i++,j++)
-	    Sw_[j].real += 2.0*model->A[l]*cosf(j*model->Wo*l + model->phi[l]);
+        for(i=0,j=-N_SAMP+1; i<N_SAMP-1; i++,j++) {
+            Sw_[FFT_DEC-N_SAMP+1+i].real += 2.0*model->A[l]*cosf(j*model->Wo*l + model->phi[l]);
+        }
+        for(i=N_SAMP-1,j=0; i<2*N_SAMP; i++,j++)
+            Sw_[j].real += 2.0*model->A[l]*cosf(j*model->Wo*l + model->phi[l]);
     }
 #endif
 
     /* Overlap add to previous samples */
-
+#ifdef USE_KISS_FFT
+#define    FFTI_FACTOR ((float)1.0)
+#else
+#define    FFTI_FACTOR ((float32_t)FFT_DEC)
+#endif
     for(i=0; i<N_SAMP-1; i++) {
-	Sn_[i] += sw_[FFT_DEC-N_SAMP+1+i].real*Pn[i];
+        Sn_[i] += sw_[FFT_DEC-N_SAMP+1+i]*Pn[i] * FFTI_FACTOR;
     }
 
     if (shift)
-	for(i=N_SAMP-1,j=0; i<2*N_SAMP; i++,j++)
-	    Sn_[i] = sw_[j].real*Pn[i];
+        for(i=N_SAMP-1,j=0; i<2*N_SAMP; i++,j++)
+            Sn_[i] = sw_[j]*Pn[i] * FFTI_FACTOR;
     else
-	for(i=N_SAMP-1,j=0; i<2*N_SAMP; i++,j++)
-	    Sn_[i] += sw_[j].real*Pn[i];
+        for(i=N_SAMP-1,j=0; i<2*N_SAMP; i++,j++)
+            Sn_[i] += sw_[j]*Pn[i] * FFTI_FACTOR;
 }
 
 
diff --git a/codec2-dev/src/sine.h b/codec2-dev/src/sine.h
index 1b1ae078..bbd24711 100644
--- a/codec2-dev/src/sine.h
+++ b/codec2-dev/src/sine.h
@@ -30,16 +30,16 @@
 
 #include "defines.h"
 #include "comp.h"
-#include "kiss_fft.h"
+#include "codec2_fft.h"
 
-void make_analysis_window(kiss_fft_cfg fft_fwd_cfg, float w[], COMP W[]);
+void make_analysis_window(codec2_fft_cfg fft_fwd_cfg, float w[], COMP W[]);
 float hpf(float x, float states[]);
-void dft_speech(kiss_fft_cfg fft_fwd_cfg, COMP Sw[], float Sn[], float w[]);
+void dft_speech(codec2_fft_cfg fft_fwd_cfg, COMP Sw[], float Sn[], float w[]);
 void two_stage_pitch_refinement(MODEL *model, COMP Sw[]);
 void estimate_amplitudes(MODEL *model, COMP Sw[], COMP W[], int est_phase);
-float est_voicing_mbe(MODEL *model, COMP Sw[], COMP W[], COMP Sw_[],COMP Ew[]);
+float est_voicing_mbe(MODEL *model, COMP Sw[], COMP W[]);
 void make_synthesis_window(float Pn[]);
-void synthesise(kiss_fft_cfg fft_inv_cfg, float Sn_[], MODEL *model, float Pn[], int shift);
+void synthesise(codec2_fftr_cfg fftr_inv_cfg, float Sn_[], MODEL *model, float Pn[], int shift);
 
 #define CODEC2_RAND_MAX 32767
 int codec2_rand(void);
diff --git a/codec2-dev/stm32/Makefile b/codec2-dev/stm32/Makefile
index bcd6dbb0..2911a298 100644
--- a/codec2-dev/stm32/Makefile
+++ b/codec2-dev/stm32/Makefile
@@ -11,13 +11,14 @@ FLOAT_TYPE=hard
 
 CROSS_COMPILE ?= arm-none-eabi-
 CC=$(BINPATH)$(CROSS_COMPILE)gcc
+AS=$(BINPATH)$(CROSS_COMPILE)as
 OBJCOPY=$(BINPATH)$(CROSS_COMPILE)objcopy
 SIZE=$(BINPATH)$(CROSS_COMPILE)size
 SUDO ?= sudo
 
 ###################################################
 
-CFLAGS  = -std=gnu99 -O0 -g -Wall -Tstm32_flash.ld -DSTM32F40_41xxx -DCORTEX_M4
+CFLAGS  = -std=gnu11 -O0 -g -Wall -Tstm32_flash.ld -DSTM32F40_41xxx -DCORTEX_M4
 CFLAGS += -mlittle-endian -mthumb -mthumb-interwork -nostartfiles -mcpu=cortex-m4
 
 ifeq ($(FLOAT_TYPE), hard)
@@ -368,7 +369,8 @@ $(CMSIS)/DSP_Lib/Source/TransformFunctions/arm_rfft_init_q31.c\
 $(CMSIS)/DSP_Lib/Source/TransformFunctions/arm_rfft_q15.c\
 $(CMSIS)/DSP_Lib/Source/TransformFunctions/arm_rfft_q31.c\
 
-CMSIS_OBJS = $(CMSIS_SRCS:.c=.o)
+
+CMSIS_OBJS = $(CMSIS_SRCS:.c=.o) $(CMSIS)/DSP_Lib/Source/TransformFunctions/arm_bitreversal2.o
 
 ###################################################
 
@@ -381,6 +383,7 @@ $(CODEC2_SRC)/nlp.c \
 $(CODEC2_SRC)/postfilter.c \
 $(CODEC2_SRC)/sine.c \
 $(CODEC2_SRC)/codec2.c \
+$(CODEC2_SRC)/codec2_fft.c \
 $(CODEC2_SRC)/kiss_fft.c \
 $(CODEC2_SRC)/kiss_fftr.c \
 $(CODEC2_SRC)/interp.c \
@@ -433,7 +436,7 @@ LIBPATHS =
 
 # Libraries to link
 
-LIBS = -lg -lnosys -lm
+LIBS = -lg -lnosys -lm 
 #Uncomment for standard arm semihosting
 #LIBS = -lg -lrdimon -lm --specs=rdimon.specs
 
diff --git a/codec2-dev/stm32/src/fdmdv_profile.c b/codec2-dev/stm32/src/fdmdv_profile.c
index 2bb72b7f..a663e604 100644
--- a/codec2-dev/stm32/src/fdmdv_profile.c
+++ b/codec2-dev/stm32/src/fdmdv_profile.c
@@ -56,12 +56,12 @@ static COMP channel[CHANNEL_BUF_SIZE];
 static void channel_in(COMP tx_fdm[], int nout) {
     int i;
 
-    /* add M tx samples to end of buffer */
+    /* add M_PITCH tx samples to end of buffer */
 
     assert((channel_count + nout) < CHANNEL_BUF_SIZE);
     for(i=0; i<nout; i++)
         channel[channel_count+i] = tx_fdm[i];
-    channel_count += M;
+    channel_count += M_PITCH;
 }
 
 static void channel_out(COMP rx_fdm[], int nin) {
diff --git a/codec2-dev/unittest/tlspsens.c b/codec2-dev/unittest/tlspsens.c
index f25b7fc2..98874f72 100644
--- a/codec2-dev/unittest/tlspsens.c
+++ b/codec2-dev/unittest/tlspsens.c
@@ -43,7 +43,7 @@ float run_a_test(char raw_file_name[], int bit_to_corrupt)
     FILE   *fin;
     short   buf[N_SAMP];
     struct  CODEC2 *c2;
-    kiss_fft_cfg  fft_fwd_cfg;
+    codec2_fftr_cfg  fftr_fwd_cfg;
     MODEL   model;
     float   ak[LPC_ORD+1];
     float   lsps[LPC_ORD], e;
@@ -53,7 +53,7 @@ float run_a_test(char raw_file_name[], int bit_to_corrupt)
     COMP    Aw[FFT_ENC];
 
     c2 = codec2_create(CODEC2_MODE_2400);
-    fft_fwd_cfg = kiss_fft_alloc(FFT_ENC, 0, NULL, NULL);
+    fftr_fwd_cfg = codec2_fftr_alloc(FFT_ENC, 0, NULL, NULL);
 
     fin = fopen(raw_file_name, "rb");
     assert(fin != NULL);
@@ -94,7 +94,7 @@ float run_a_test(char raw_file_name[], int bit_to_corrupt)
 	check_lsp_order(lsps, LPC_ORD);
 	bw_expand_lsps(lsps, LPC_ORD, 50.0, 100.0);
 	lsp_to_lpc(lsps, ak, LPC_ORD);
-	aks_to_M2(fft_fwd_cfg, ak, LPC_ORD, &model, e, &snr, 0, 0, 1, 1, LPCPF_BETA, LPCPF_GAMMA, Aw);
+	aks_to_M2(fftr_fwd_cfg, ak, LPC_ORD, &model, e, &snr, 0, 0, 1, 1, LPCPF_BETA, LPCPF_GAMMA, Aw);
 	snr_sum += snr;
 	frames++;
     }