1 files changed, 28 insertions, 15 deletions

diff --git a/codec2.c b/codec2.c
index 840fe21..2697356 100644
--- a/codec2.c
+++ b/codec2.c
@@ -29,6 +29,7 @@
 #include <assert.h>
 #include <stdio.h>
 #include <stdlib.h>
+#include <stdbool.h>
 #include <string.h>
 #include <math.h>
 
@@ -114,12 +115,25 @@ struct CODEC2 * codec2_create(int mode)
     struct CODEC2 *c2;
     int            i,l;
 
-#ifndef CORTEX_M4
-    if (( CODEC2_MODE_ACTIVE(CODEC2_MODE_450, mode)) || 
-        ( CODEC2_MODE_ACTIVE(CODEC2_MODE_450PWB, mode)) ) {
+    // ALL POSSIBLE MODES MUST BE CHECKED HERE!
+    // we test if the desired mode is enabled at compile time
+    // and return NULL if not
+
+    if (false == ( CODEC2_MODE_ACTIVE(CODEC2_MODE_3200, mode) 
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_2400, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_1600, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_1400, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_1300, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_1200, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_700, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_700B, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_700C, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_450, mode)
+                   || CODEC2_MODE_ACTIVE(CODEC2_MODE_450PWB, mode)
+                ) ) 
+    {
         return NULL;
     }  
-#endif
 
     c2 = (struct CODEC2*)MALLOC(sizeof(struct CODEC2));
     if (c2 == NULL)
@@ -221,14 +235,15 @@ struct CODEC2 * codec2_create(int mode)
         int k;
         for(k=0; k<NEWAMP1_K; k++) {
             c2->prev_rate_K_vec_[k] = 0.0;
+            c2->eq[k] = 0.0;
         }
+        c2->eq_en = 0;
         c2->Wo_left = 0.0;
         c2->voicing_left = 0;;
         c2->phase_fft_fwd_cfg = codec2_fft_alloc(NEWAMP1_PHASE_NFFT, 0, NULL, NULL);
         c2->phase_fft_inv_cfg = codec2_fft_alloc(NEWAMP1_PHASE_NFFT, 1, NULL, NULL);
     }
 
-#ifndef CORTEX_M4
     /* newamp2 initialisation */
 
     if ( CODEC2_MODE_ACTIVE(CODEC2_MODE_450, c2->mode)) {
@@ -255,7 +270,6 @@ struct CODEC2 * codec2_create(int mode)
         c2->phase_fft_fwd_cfg = codec2_fft_alloc(NEWAMP2_PHASE_NFFT, 0, NULL, NULL);
         c2->phase_fft_inv_cfg = codec2_fft_alloc(NEWAMP2_PHASE_NFFT, 1, NULL, NULL);
     }
-#endif
 
     c2->fmlfeat = NULL;
 
@@ -302,7 +316,6 @@ struct CODEC2 * codec2_create(int mode)
         c2->decode = codec2_decode_1200;
     }
 
-#ifndef CORTEX_M4
     if ( CODEC2_MODE_ACTIVE(CODEC2_MODE_700, c2->mode))
     {
         c2->encode = codec2_encode_700;
@@ -314,13 +327,13 @@ struct CODEC2 * codec2_create(int mode)
         c2->encode = codec2_encode_700b;
         c2->decode = codec2_decode_700b;
     }
-#endif
+
     if ( CODEC2_MODE_ACTIVE(CODEC2_MODE_700C, c2->mode))
     {
         c2->encode = codec2_encode_700c;
         c2->decode = codec2_decode_700c;
     }
-#ifndef CORTEX_M4
+
     if ( CODEC2_MODE_ACTIVE(CODEC2_MODE_450, c2->mode))
     {
         c2->encode = codec2_encode_450;
@@ -334,7 +347,6 @@ struct CODEC2 * codec2_create(int mode)
         c2->decode = codec2_decode_450pwb;
     }
 
-#endif
     
     return c2;
 }
@@ -1518,7 +1530,6 @@ void codec2_decode_1200(struct CODEC2 *c2, short speech[], const unsigned char *
 }
 
 
-#ifndef CORTEX_M4
 /*---------------------------------------------------------------------------*\
 
   FUNCTION....: codec2_encode_700
@@ -1926,7 +1937,6 @@ void codec2_decode_700b(struct CODEC2 *c2, short speech[], const unsigned char *
     for(i=0; i<LPC_ORD_LOW; i++)
         c2->prev_lsps_dec[i] = lsps[3][i];
 }
-#endif
 
 
 /*---------------------------------------------------------------------------*\
@@ -1987,7 +1997,7 @@ void codec2_encode_700c(struct CODEC2 *c2, unsigned char * bits, short speech[])
                              K,
                              &mean,
                              rate_K_vec_no_mean,
-                             rate_K_vec_no_mean_, &c2->se);
+                             rate_K_vec_no_mean_, &c2->se, c2->eq, c2->eq_en);
     c2->nse += K;
 
 #ifndef CORTEX_M4
@@ -2092,7 +2102,6 @@ float codec2_energy_700c(struct CODEC2 *c2, const unsigned char * bits)
     return POW10F(mean/10.0);
 }
 
-#ifndef CORTEX_M4
 float codec2_energy_450(struct CODEC2 *c2, const unsigned char * bits)
 {
     int     indexes[4];
@@ -2418,7 +2427,6 @@ void codec2_decode_450pwb(struct CODEC2 *c2, short speech[], const unsigned char
    }
 }
 
-#endif
 
 /*---------------------------------------------------------------------------* \
 
@@ -2719,3 +2727,8 @@ float *codec2_enable_user_ratek(struct CODEC2 *codec2_state, int *K) {
 void codec2_700c_post_filter(struct CODEC2 *codec2_state, int en) {
     codec2_state->post_filter_en = en;
 }
+
+void codec2_700c_eq(struct CODEC2 *codec2_state, int en) {
+    codec2_state->eq_en = en;
+    codec2_state->se = 0.0; codec2_state->nse = 0;
+}