Update SILK code using the CELT range coder
[opus.git] / src_common / SKP_Silk_NSQ.c
similarity index 66%
rename from src/SKP_Silk_NSQ.c
rename to src_common/SKP_Silk_NSQ.c
index 19cb8ea..12dd887 100644 (file)
@@ -36,15 +36,15 @@ SKP_INLINE void SKP_Silk_nsq_scale_states(
     SKP_int32           sLTP_Q16[],         /* O LTP state matching scaled input    */\r
     SKP_int             subfr,              /* I subframe number                    */\r
     const SKP_int       LTP_scale_Q14,      /* I                                    */\r
-    const SKP_int32     Gains_Q16[ NB_SUBFR ], /* I                                 */\r
-    const SKP_int       pitchL[ NB_SUBFR ]  /* I                                    */\r
+    const SKP_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                             */\r
+    const SKP_int       pitchL[ MAX_NB_SUBFR ]  /* I                                */\r
 );\r
 \r
 SKP_INLINE void SKP_Silk_noise_shape_quantizer(\r
     SKP_Silk_nsq_state  *NSQ,               /* I/O  NSQ state                       */\r
     SKP_int             sigtype,            /* I    Signal type                     */\r
     const SKP_int32     x_sc_Q10[],         /* I                                    */\r
-    SKP_int             q[],                /* O                                    */\r
+    SKP_int8            q[],                /* O                                    */\r
     SKP_int16           xq[],               /* O                                    */\r
     SKP_int32           sLTP_Q16[],         /* I/O  LTP state                       */\r
     const SKP_int16     a_Q12[],            /* I    Short term prediction coefs     */\r
@@ -59,7 +59,14 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
     SKP_int             offset_Q10,         /* I                                    */\r
     SKP_int             length,             /* I    Input length                    */\r
     SKP_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */\r
-    SKP_int             predictLPCOrder     /* I    Prediction filter order         */\r
+    SKP_int             predictLPCOrder,    /* I    Prediction filter order         */\r
+    SKP_int32           warping_Q16         /* I                                    */\r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+    ,SKP_float          q_in_env[],         /* O                                    */\r
+    SKP_float           q_exc[],            /* O                                    */\r
+    SKP_float           q_LPC_exc[],        /* O                                    */\r
+    SKP_float           q_exc_prev[]        /* O                                    */\r
+#endif\r
 );\r
 \r
 void SKP_Silk_NSQ(\r
@@ -67,15 +74,15 @@ void SKP_Silk_NSQ(
     SKP_Silk_encoder_control        *psEncCtrlC,                                /* I    Encoder Control                     */\r
     SKP_Silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                           */\r
     const SKP_int16                 x[],                                        /* I    prefiltered input signal            */\r
-    SKP_int                         q[],                                        /* O    quantized qulse signal              */\r
+    SKP_int8                        q[],                                        /* O    quantized qulse signal              */\r
     const SKP_int                   LSFInterpFactor_Q2,                         /* I    LSF interpolation factor in Q2      */\r
     const SKP_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefficients  */\r
-    const SKP_int16                 LTPCoef_Q14[ LTP_ORDER * NB_SUBFR ],        /* I    Long term prediction coefficients   */\r
-    const SKP_int16                 AR2_Q13[ NB_SUBFR * SHAPE_LPC_ORDER_MAX ],  /* I                                        */\r
-    const SKP_int                   HarmShapeGain_Q14[ NB_SUBFR ],              /* I                                        */\r
-    const SKP_int                   Tilt_Q14[ NB_SUBFR ],                       /* I    Spectral tilt                       */\r
-    const SKP_int32                 LF_shp_Q14[ NB_SUBFR ],                     /* I                                        */\r
-    const SKP_int32                 Gains_Q16[ NB_SUBFR ],                      /* I                                        */\r
+    const SKP_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],        /* I    Long term prediction coefficients   */\r
+    const SKP_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ],  /* I                                    */\r
+    const SKP_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],              /* I                                    */\r
+    const SKP_int                   Tilt_Q14[ MAX_NB_SUBFR ],                       /* I    Spectral tilt                   */\r
+    const SKP_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                     /* I                                    */\r
+    const SKP_int32                 Gains_Q16[ MAX_NB_SUBFR ],                      /* I                                    */\r
     const SKP_int                   Lambda_Q10,                                 /* I                                        */\r
     const SKP_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */\r
 )\r
@@ -88,16 +95,27 @@ void SKP_Silk_NSQ(
     SKP_int32   HarmShapeFIRPacked_Q14;\r
     SKP_int     offset_Q10;\r
     SKP_int32   FiltState[ MAX_LPC_ORDER ];\r
-    SKP_int32   x_sc_Q10[ MAX_FRAME_LENGTH / NB_SUBFR ];\r
-\r
-    subfr_length = psEncC->frame_length / NB_SUBFR;\r
+    SKP_int32   x_sc_Q10[ MAX_FRAME_LENGTH / MAX_NB_SUBFR ];\r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+    /* Date buffers for analysing */\r
+    SKP_float   q_in_env_buf[  MAX_FRAME_LENGTH ];\r
+    SKP_float   q_exc_buf[     MAX_FRAME_LENGTH ];\r
+    SKP_float   q_LPC_exc_buf[ MAX_FRAME_LENGTH ];\r
+    SKP_float   q_exc_prev_buf[     MAX_FRAME_LENGTH ];\r
+    SKP_float   *q_in_env, *q_exc, *q_LPC_exc, *q_exc_prev;\r
+\r
+    q_in_env   = q_in_env_buf;\r
+    q_exc      = q_exc_buf;\r
+    q_LPC_exc  = q_LPC_exc_buf;\r
+    q_exc_prev = q_exc_prev_buf;\r
+#endif\r
+\r
+    subfr_length = psEncC->frame_length / MAX_NB_SUBFR;\r
 \r
     NSQ->rand_seed  =  psEncCtrlC->Seed;\r
     /* Set unvoiced lag to the previous one, overwrite later for voiced */\r
     lag             = NSQ->lagPrev;\r
 \r
-    SKP_assert( NSQ->prev_inv_gain_Q16 != 0 );\r
-\r
     offset_Q10 = SKP_Silk_Quantization_Offsets_Q10[ psEncCtrlC->sigtype ][ psEncCtrlC->QuantOffsetType ];\r
 \r
     if( LSFInterpFactor_Q2 == ( 1 << 2 ) ) {\r
@@ -107,13 +125,13 @@ void SKP_Silk_NSQ(
     }\r
 \r
     /* Setup pointers to start of sub frame */\r
-    NSQ->sLTP_shp_buf_idx = psEncC->frame_length;\r
-    NSQ->sLTP_buf_idx     = psEncC->frame_length;\r
-    pxq                   = &NSQ->xq[ psEncC->frame_length ];\r
-    for( k = 0; k < NB_SUBFR; k++ ) {\r
+    NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;\r
+    NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;\r
+    pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];\r
+    for( k = 0; k < psEncC->nb_subfr; k++ ) {\r
         A_Q12      = &PredCoef_Q12[ (( k >> 1 ) | ( 1 - LSF_interpolation_flag )) * MAX_LPC_ORDER ];\r
         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER ];\r
-        AR_shp_Q13 = &AR2_Q13[     k * SHAPE_LPC_ORDER_MAX ];\r
+        AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];\r
 \r
         /* Noise shape parameters */\r
         SKP_assert( HarmShapeGain_Q14[ k ] >= 0 );\r
@@ -129,15 +147,15 @@ void SKP_Silk_NSQ(
             if( ( k & ( 3 - SKP_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {\r
                 /* Rewhiten with new A coefs */\r
                 \r
-                start_idx = psEncC->frame_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;\r
-                start_idx = SKP_LIMIT( start_idx, 0, psEncC->frame_length - psEncC->predictLPCOrder ); /* Limit */\r
+                start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;\r
+                start_idx = SKP_LIMIT_int( start_idx, 0, psEncC->ltp_mem_length - psEncC->predictLPCOrder ); /* Limit */\r
                 \r
                 SKP_memset( FiltState, 0, psEncC->predictLPCOrder * sizeof( SKP_int32 ) );\r
-                SKP_Silk_MA_Prediction( &NSQ->xq[ start_idx + k * ( psEncC->frame_length >> 2 ) ], \r
-                    A_Q12, FiltState, sLTP + start_idx, psEncC->frame_length - start_idx, psEncC->predictLPCOrder );\r
+                SKP_Silk_MA_Prediction( &NSQ->xq[ start_idx + k * psEncC->subfr_length ], \r
+                    A_Q12, FiltState, sLTP + start_idx, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder );\r
 \r
                 NSQ->rewhite_flag = 1;\r
-                NSQ->sLTP_buf_idx = psEncC->frame_length;\r
+                NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;\r
             }\r
         }\r
         \r
@@ -146,22 +164,36 @@ void SKP_Silk_NSQ(
 \r
         SKP_Silk_noise_shape_quantizer( NSQ, psEncCtrlC->sigtype, x_sc_Q10, q, pxq, sLTP_Q16, A_Q12, B_Q14, \r
             AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], Lambda_Q10, \r
-            offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder\r
+            offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder, psEncCtrlC->warping_Q16\r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+            , q_in_env, q_exc, q_LPC_exc, q_exc_prev\r
+#endif\r
         );\r
 \r
         x          += psEncC->subfr_length;\r
         q          += psEncC->subfr_length;\r
         pxq        += psEncC->subfr_length;\r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+        q_in_env   += psEncC->subfr_length;\r
+        q_exc      += psEncC->subfr_length;\r
+        q_LPC_exc  += psEncC->subfr_length;\r
+        q_exc_prev += subfr_length;\r
+#endif\r
     }\r
 \r
-    /* Save scalars for this layer */\r
-    NSQ->sLF_AR_shp_Q12             = NSQ->sLF_AR_shp_Q12;\r
-    NSQ->prev_inv_gain_Q16          = NSQ->prev_inv_gain_Q16;\r
-    NSQ->lagPrev                        = psEncCtrlC->pitchL[ NB_SUBFR - 1 ];\r
+    /* Update lagPrev for next frame */\r
+    NSQ->lagPrev = psEncCtrlC->pitchL[ psEncC->nb_subfr - 1 ];\r
     /* Save quantized speech and noise shaping signals */\r
-    SKP_memcpy( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int16 ) );\r
-    SKP_memcpy( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int32 ) );\r
-\r
+    SKP_memcpy( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( SKP_int16 ) );\r
+    SKP_memcpy( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( SKP_int32 ) );\r
+\r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+    DEBUG_STORE_DATA( q_in_env.dat,  q_in_env_buf,  psEncC->frame_length * sizeof( SKP_float ) );\r
+    DEBUG_STORE_DATA( q_exc.dat,     q_exc_buf,     psEncC->frame_length * sizeof( SKP_float ) );\r
+    DEBUG_STORE_DATA( q_lpc_exc.dat, q_LPC_exc_buf, psEncC->frame_length * sizeof( SKP_float ) );\r
+    DEBUG_STORE_DATA( xq.dat,        &NSQ->xq[ psEncC->ltp_mem_length - psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int16 ) );\r
+    DEBUG_STORE_DATA( q.dat,         &q[ -psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int8 ) );\r
+#endif\r
 }\r
 \r
 /***********************************/\r
@@ -171,7 +203,7 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
     SKP_Silk_nsq_state  *NSQ,               /* I/O  NSQ state                       */\r
     SKP_int             sigtype,            /* I    Signal type                     */\r
     const SKP_int32     x_sc_Q10[],         /* I                                    */\r
-    SKP_int             q[],                /* O                                    */\r
+    SKP_int8            q[],                /* O                                    */\r
     SKP_int16           xq[],               /* O                                    */\r
     SKP_int32           sLTP_Q16[],         /* I/O  LTP state                       */\r
     const SKP_int16     a_Q12[],            /* I    Short term prediction coefs     */\r
@@ -186,19 +218,23 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
     SKP_int             offset_Q10,         /* I                                    */\r
     SKP_int             length,             /* I    Input length                    */\r
     SKP_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */\r
-    SKP_int             predictLPCOrder     /* I    Prediction filter order         */\r
+    SKP_int             predictLPCOrder,    /* I    Prediction filter order         */\r
+    SKP_int             warping_Q16         /* I                                    */\r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+    ,SKP_float          q_in_env[],         /* O                                    */\r
+    SKP_float           q_exc[],            /* O                                    */\r
+    SKP_float           q_LPC_exc[],        /* O                                    */\r
+    SKP_float           q_exc_prev[]        /* O                                    */\r
+#endif\r
 )\r
 {\r
     SKP_int     i, j;\r
     SKP_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14;\r
     SKP_int32   n_LF_Q10, r_Q10, q_Q0, q_Q10;\r
     SKP_int32   thr1_Q10, thr2_Q10, thr3_Q10;\r
-    SKP_int32   Atmp, dither;\r
-    SKP_int32   exc_Q10, LPC_exc_Q10, xq_Q10;\r
-    SKP_int32   tmp, sLF_AR_shp_Q10;\r
-    SKP_int32   *psLPC_Q14;\r
-    SKP_int32   *shp_lag_ptr, *pred_lag_ptr;\r
-    SKP_int32   a_Q12_tmp[ MAX_LPC_ORDER / 2 ], AR_shp_Q13_tmp[ MAX_LPC_ORDER / 2 ];\r
+    SKP_int32   dither, exc_Q10, LPC_exc_Q10, xq_Q10;\r
+    SKP_int32   tmp1, tmp2, sLF_AR_shp_Q10;\r
+    SKP_int32   *psLPC_Q14, *shp_lag_ptr, *pred_lag_ptr;\r
 \r
     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];\r
     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
@@ -211,11 +247,7 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
     thr2_Q10 = SKP_SUB_RSHIFT32( -512,  Lambda_Q10, 1);\r
     thr2_Q10 = SKP_ADD_RSHIFT32( thr2_Q10, SKP_SMULBB( offset_Q10, Lambda_Q10 ), 10 );\r
     thr3_Q10 = SKP_ADD_RSHIFT32(  512,  Lambda_Q10, 1);\r
-    \r
-    /* Preload LPC coeficients to array on stack. Gives small performance gain */\r
-    SKP_memcpy( a_Q12_tmp, a_Q12, predictLPCOrder * sizeof( SKP_int16 ) );\r
-    SKP_memcpy( AR_shp_Q13_tmp, AR_shp_Q13, shapingLPCOrder * sizeof( SKP_int16 ) );\r
-    \r
+\r
     for( i = 0; i < length; i++ ) {\r
         /* Generate dither */\r
         NSQ->rand_seed = SKP_RAND( NSQ->rand_seed );\r
@@ -228,30 +260,20 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
         SKP_assert( ( (SKP_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */\r
         SKP_assert( predictLPCOrder >= 10 );            /* check that unrolling works */\r
 \r
-        /* NOTE: the code below loads two int16 values in an int32, and multiplies each using the   */\r
-        /* SMLAWB and SMLAWT instructions. On a big-endian CPU the two int16 variables would be     */\r
-        /* loaded in reverse order and the code will give the wrong result. In that case swapping   */\r
-        /* the SMLAWB and SMLAWT instructions should solve the problem.                             */\r
         /* Partially unrolled */\r
-        Atmp = a_Q12_tmp[ 0 ];      /* read two coefficients at once */\r
-        LPC_pred_Q10 = SKP_SMULWB(               psLPC_Q14[ 0  ], Atmp );\r
-        LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -1 ], Atmp );\r
-        Atmp = a_Q12_tmp[ 1 ];\r
-        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], Atmp );\r
-        LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -3 ], Atmp );\r
-        Atmp = a_Q12_tmp[ 2 ];\r
-        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], Atmp );\r
-        LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -5 ], Atmp );\r
-        Atmp = a_Q12_tmp[ 3 ];\r
-        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], Atmp );\r
-        LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -7 ], Atmp );\r
-        Atmp = a_Q12_tmp[ 4 ];\r
-        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], Atmp );\r
-        LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -9 ], Atmp );\r
-        for( j = 10; j < predictLPCOrder; j += 2 ) {\r
-            Atmp = a_Q12_tmp[ j >> 1 ];     /* read two coefficients at once */\r
-            LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j     ], Atmp );\r
-            LPC_pred_Q10 = SKP_SMLAWT( LPC_pred_Q10, psLPC_Q14[ -j - 1 ], Atmp );\r
+        LPC_pred_Q10 = SKP_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );\r
+        LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );\r
+\r
+        for( j = 10; j < predictLPCOrder; j ++ ) {\r
+            LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );\r
         }\r
 \r
         /* Long-term prediction */\r
@@ -268,41 +290,34 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
         }\r
 \r
         /* Noise shape feedback */\r
-        SKP_assert( ( shapingLPCOrder       & 1 ) == 0 );   /* check that order is even */\r
-        SKP_assert( ( (SKP_int64)AR_shp_Q13 & 3 ) == 0 );   /* check that array starts at 4-byte aligned address */\r
-        SKP_assert( shapingLPCOrder >= 12 );                /* check that unrolling works */\r
-\r
-        /* Partially unrolled */\r
-        Atmp = AR_shp_Q13_tmp[ 0 ];     /* read two coefficients at once */\r
-        n_AR_Q10 = SKP_SMULWB(           psLPC_Q14[ 0  ], Atmp );\r
-        n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -1 ], Atmp );\r
-        Atmp = AR_shp_Q13_tmp[ 1 ];\r
-        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -2 ], Atmp );\r
-        n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -3 ], Atmp );\r
-        Atmp = AR_shp_Q13_tmp[ 2 ];\r
-        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -4 ], Atmp );\r
-        n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -5 ], Atmp );\r
-        Atmp = AR_shp_Q13_tmp[ 3 ];\r
-        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -6 ], Atmp );\r
-        n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -7 ], Atmp );\r
-        Atmp = AR_shp_Q13_tmp[ 4 ];\r
-        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -8 ], Atmp );\r
-        n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -9 ], Atmp );\r
-        Atmp = AR_shp_Q13_tmp[ 5 ];\r
-        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -10 ], Atmp );\r
-        n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -11 ], Atmp );\r
-        for( j = 12; j < shapingLPCOrder; j += 2 ) {\r
-            Atmp = AR_shp_Q13_tmp[ j >> 1 ];        /* read two coefficients at once */\r
-            n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, psLPC_Q14[ -j     ], Atmp );\r
-            n_AR_Q10 = SKP_SMLAWT( n_AR_Q10, psLPC_Q14[ -j - 1 ], Atmp );\r
+        SKP_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */\r
+        /* Output of lowpass section */\r
+        tmp2 = SKP_SMLAWB( psLPC_Q14[ 0 ], NSQ->sAR2_Q14[ 0 ], warping_Q16 );\r
+        /* Output of allpass section */\r
+        tmp1 = SKP_SMLAWB( NSQ->sAR2_Q14[ 0 ], NSQ->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );\r
+        NSQ->sAR2_Q14[ 0 ] = tmp2;\r
+        n_AR_Q10 = SKP_SMULWB( tmp2, AR_shp_Q13[ 0 ] );\r
+        /* Loop over allpass sections */\r
+        for( j = 2; j < shapingLPCOrder; j += 2 ) {\r
+            /* Output of allpass section */\r
+            tmp2 = SKP_SMLAWB( NSQ->sAR2_Q14[ j - 1 ], NSQ->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );\r
+            NSQ->sAR2_Q14[ j - 1 ] = tmp1;\r
+            n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );\r
+            /* Output of allpass section */\r
+            tmp1 = SKP_SMLAWB( NSQ->sAR2_Q14[ j + 0 ], NSQ->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );\r
+            NSQ->sAR2_Q14[ j + 0 ] = tmp2;\r
+            n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );\r
         }\r
+        NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;\r
+        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );\r
+\r
         n_AR_Q10 = SKP_RSHIFT( n_AR_Q10, 1 );   /* Q11 -> Q10 */\r
-        n_AR_Q10  = SKP_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );\r
+        n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );\r
 \r
-        n_LF_Q10   = SKP_LSHIFT( SKP_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 ); \r
-        n_LF_Q10   = SKP_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );\r
+        n_LF_Q10 = SKP_LSHIFT( SKP_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 ); \r
+        n_LF_Q10 = SKP_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );\r
 \r
-        SKP_assert( lag > 0 || sigtype == SIG_TYPE_UNVOICED);\r
+        SKP_assert( lag > 0 || sigtype == SIG_TYPE_UNVOICED );\r
 \r
         /* Long-term shaping */\r
         if( lag > 0 ) {\r
@@ -317,18 +332,17 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
 \r
         /* Input minus prediction plus noise feedback  */\r
         //r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP;\r
-        tmp   = SKP_SUB32( LTP_pred_Q14, n_LTP_Q14 );                       /* Add Q14 stuff */\r
-        tmp   = SKP_RSHIFT_ROUND( tmp, 4 );                                 /* round to Q10  */\r
-        tmp   = SKP_ADD32( tmp, LPC_pred_Q10 );                             /* add Q10 stuff */ \r
-        tmp   = SKP_SUB32( tmp, n_AR_Q10 );                                 /* subtract Q10 stuff */ \r
-        tmp   = SKP_SUB32( tmp, n_LF_Q10 );                                 /* subtract Q10 stuff */ \r
-        r_Q10 = SKP_SUB32( x_sc_Q10[ i ], tmp );\r
-\r
+        tmp1  = SKP_SUB32( LTP_pred_Q14, n_LTP_Q14 );                       /* Add Q14 stuff */\r
+        tmp1  = SKP_RSHIFT( tmp1, 4 );                                      /* convert to Q10  */\r
+        tmp1  = SKP_ADD32( tmp1, LPC_pred_Q10 );                            /* add Q10 stuff */ \r
+        tmp1  = SKP_SUB32( tmp1, n_AR_Q10 );                                /* subtract Q10 stuff */ \r
+        tmp1  = SKP_SUB32( tmp1, n_LF_Q10 );                                /* subtract Q10 stuff */ \r
+        r_Q10 = SKP_SUB32( x_sc_Q10[ i ], tmp1 );\r
 \r
         /* Flip sign depending on dither */\r
         r_Q10 = ( r_Q10 ^ dither ) - dither;\r
         r_Q10 = SKP_SUB32( r_Q10, offset_Q10 );\r
-        r_Q10 = SKP_LIMIT( r_Q10, -64 << 10, 64 << 10 );\r
+        r_Q10 = SKP_LIMIT_32( r_Q10, -64 << 10, 64 << 10 );\r
 \r
         /* Quantize */\r
         if( r_Q10 < thr1_Q10 ) {\r
@@ -344,7 +358,7 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
             q_Q0 = 0;\r
             q_Q10 = 0;\r
         }\r
-        q[ i ] = q_Q0;\r
+        q[ i ] = ( SKP_int8 )q_Q0; /* No saturation needed because max is 64 */\r
 \r
         /* Excitation */\r
         exc_Q10 = SKP_ADD32( q_Q10, offset_Q10 );\r
@@ -357,6 +371,11 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
         /* Scale XQ back to normal level before saving */\r
         xq[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( xq_Q10, Gain_Q16 ), 10 ) );\r
         \r
+#ifdef SAVE_ALL_INTERNAL_DATA\r
+        q_in_env[   i ] = ( (SKP_float)r_Q10       * (SKP_float)Gain_Q16 ) / ( 1024.0f * 65536.0f );\r
+        q_exc[      i ] = ( (SKP_float)exc_Q10     * (SKP_float)Gain_Q16 ) / ( 1024.0f * 65536.0f );\r
+        q_LPC_exc[  i ] = ( (SKP_float)LPC_exc_Q10 * (SKP_float)Gain_Q16 ) / ( 1024.0f * 65536.0f );\r
+#endif\r
         \r
         /* Update states */\r
         psLPC_Q14++;\r
@@ -365,7 +384,7 @@ SKP_INLINE void SKP_Silk_noise_shape_quantizer(
         NSQ->sLF_AR_shp_Q12 = SKP_LSHIFT( sLF_AR_shp_Q10, 2 );\r
 \r
         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx ] = SKP_SUB32( sLF_AR_shp_Q10, n_LF_Q10 );\r
-        sLTP_Q16[NSQ->sLTP_buf_idx] = SKP_LSHIFT( LPC_exc_Q10, 6 );\r
+        sLTP_Q16[ NSQ->sLTP_buf_idx ] = SKP_LSHIFT( LPC_exc_Q10, 6 );\r
         NSQ->sLTP_shp_buf_idx++;\r
         NSQ->sLTP_buf_idx++;\r
 \r
@@ -385,14 +404,14 @@ SKP_INLINE void SKP_Silk_nsq_scale_states(
     SKP_int32           sLTP_Q16[],         /* O LTP state matching scaled input    */\r
     SKP_int             subfr,              /* I subframe number                    */\r
     const SKP_int       LTP_scale_Q14,      /* I                                    */\r
-    const SKP_int32     Gains_Q16[ NB_SUBFR ], /* I                                 */\r
-    const SKP_int       pitchL[ NB_SUBFR ]  /* I                                    */\r
+    const SKP_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                                 */\r
+    const SKP_int       pitchL[ MAX_NB_SUBFR ]  /* I                                    */\r
 )\r
 {\r
     SKP_int   i, scale_length, lag;\r
     SKP_int32 inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;\r
 \r
-    inv_gain_Q16 = SKP_DIV32( SKP_int32_MAX, SKP_RSHIFT( Gains_Q16[ subfr ], 1) );\r
+    inv_gain_Q16 = SKP_INVERSE32_varQ( SKP_max( Gains_Q16[ subfr ], 1 ), 32 );\r
     inv_gain_Q16 = SKP_min( inv_gain_Q16, SKP_int16_MAX );\r
     lag          = pitchL[ subfr ];\r
 \r
@@ -409,8 +428,8 @@ SKP_INLINE void SKP_Silk_nsq_scale_states(
     }\r
 \r
     /* Prepare for Worst case. Next frame starts with max lag voiced */\r
-    scale_length = length * NB_SUBFR;                                           /* approx max lag */\r
-    scale_length = scale_length - SKP_SMULBB( NB_SUBFR - (subfr + 1), length ); /* subtract samples that will be too old in next frame */\r
+    scale_length = length * MAX_NB_SUBFR;                                           /* approx max lag */\r
+    scale_length = scale_length - SKP_SMULBB( MAX_NB_SUBFR - (subfr + 1), length ); /* subtract samples that will be too old in next frame */\r
     scale_length = SKP_max_int( scale_length, lag + LTP_ORDER );                /* make sure to scale whole pitch period if voiced */\r
 \r
     /* Adjust for changing gain */\r
@@ -433,6 +452,9 @@ SKP_INLINE void SKP_Silk_nsq_scale_states(
         for( i = 0; i < MAX_LPC_ORDER; i++ ) {\r
             NSQ->sLPC_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );\r
         }\r
+        for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {\r
+            NSQ->sAR2_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );\r
+        }\r
     }\r
 \r
     /* Scale input */\r
@@ -443,4 +465,4 @@ SKP_INLINE void SKP_Silk_nsq_scale_states(
     /* save inv_gain */\r
     SKP_assert( inv_gain_Q16 != 0 );\r
     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;\r
-}\r
+}
\ No newline at end of file