increase NSQ decision delay from 32 to 40
[opus.git] / silk / x86 / NSQ_del_dec_sse.c
1 /* Copyright (c) 2014, Cisco Systems, INC
2    Written by XiangMingZhu WeiZhou MinPeng YanWang
3
4    Redistribution and use in source and binary forms, with or without
5    modification, are permitted provided that the following conditions
6    are met:
7
8    - Redistributions of source code must retain the above copyright
9    notice, this list of conditions and the following disclaimer.
10
11    - Redistributions in binary form must reproduce the above copyright
12    notice, this list of conditions and the following disclaimer in the
13    documentation and/or other materials provided with the distribution.
14
15    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
16    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
17    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
18    A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
19    OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
20    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
21    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
22    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
23    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
24    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include <xmmintrin.h>
33 #include <emmintrin.h>
34 #include <smmintrin.h>
35 #include "main.h"
36 #include "celt/x86/x86cpu.h"
37
38 #include "stack_alloc.h"
39
40 typedef struct {
41     opus_int32 sLPC_Q14[ MAX_SUB_FRAME_LENGTH + NSQ_LPC_BUF_LENGTH ];
42     opus_int32 RandState[ DECISION_DELAY ];
43     opus_int32 Q_Q10[     DECISION_DELAY ];
44     opus_int32 Xq_Q14[    DECISION_DELAY ];
45     opus_int32 Pred_Q15[  DECISION_DELAY ];
46     opus_int32 Shape_Q14[ DECISION_DELAY ];
47     opus_int32 sAR2_Q14[ MAX_SHAPE_LPC_ORDER ];
48     opus_int32 LF_AR_Q14;
49     opus_int32 Seed;
50     opus_int32 SeedInit;
51     opus_int32 RD_Q10;
52 } NSQ_del_dec_struct;
53
54 typedef struct {
55     opus_int32 Q_Q10;
56     opus_int32 RD_Q10;
57     opus_int32 xq_Q14;
58     opus_int32 LF_AR_Q14;
59     opus_int32 sLTP_shp_Q14;
60     opus_int32 LPC_exc_Q14;
61 } NSQ_sample_struct;
62
63 typedef NSQ_sample_struct  NSQ_sample_pair[ 2 ];
64
65 static OPUS_INLINE void silk_nsq_del_dec_scale_states_sse4_1(
66     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
67     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
68     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
69     const opus_int32    x_Q3[],                     /* I    Input in Q3                         */
70     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
71     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
72     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
73     opus_int            subfr,                      /* I    Subframe number                     */
74     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
75     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
76     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
77     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
78     const opus_int      signal_type,                /* I    Signal type                         */
79     const opus_int      decisionDelay               /* I    Decision delay                      */
80 );
81
82 /******************************************/
83 /* Noise shape quantizer for one subframe */
84 /******************************************/
85 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec_sse4_1(
86     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
87     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
88     opus_int            signalType,             /* I    Signal type                         */
89     const opus_int32    x_Q10[],                /* I                                        */
90     opus_int8           pulses[],               /* O                                        */
91     opus_int16          xq[],                   /* O                                        */
92     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
93     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
94     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
95     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
96     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
97     opus_int            lag,                    /* I    Pitch lag                           */
98     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
99     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
100     opus_int32          LF_shp_Q14,             /* I                                        */
101     opus_int32          Gain_Q16,               /* I                                        */
102     opus_int            Lambda_Q10,             /* I                                        */
103     opus_int            offset_Q10,             /* I                                        */
104     opus_int            length,                 /* I    Input length                        */
105     opus_int            subfr,                  /* I    Subframe number                     */
106     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
107     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
108     opus_int            warping_Q16,            /* I                                        */
109     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
110     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
111     opus_int            decisionDelay           /* I                                        */
112 );
113
114 void silk_NSQ_del_dec_sse4_1(
115     const silk_encoder_state    *psEncC,                                    /* I/O  Encoder State                   */
116     silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                       */
117     SideInfoIndices             *psIndices,                                 /* I/O  Quantization Indices            */
118     const opus_int32            x_Q3[],                                     /* I    Prefiltered input signal        */
119     opus_int8                   pulses[],                                   /* O    Quantized pulse signal          */
120     const opus_int16            PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefs     */
121     const opus_int16            LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefs      */
122     const opus_int16            AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I Noise shaping coefs             */
123     const opus_int              HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I    Long term shaping coefs         */
124     const opus_int              Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                   */
125     const opus_int32            LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I    Low frequency shaping coefs     */
126     const opus_int32            Gains_Q16[ MAX_NB_SUBFR ],                  /* I    Quantization step sizes         */
127     const opus_int              pitchL[ MAX_NB_SUBFR ],                     /* I    Pitch lags                      */
128     const opus_int              Lambda_Q10,                                 /* I    Rate/distortion tradeoff        */
129     const opus_int              LTP_scale_Q14                               /* I    LTP state scaling               */
130 )
131 {
132     opus_int            i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;
133     opus_int            last_smple_idx, smpl_buf_idx, decisionDelay;
134     const opus_int16    *A_Q12, *B_Q14, *AR_shp_Q13;
135     opus_int16          *pxq;
136     VARDECL( opus_int32, sLTP_Q15 );
137     VARDECL( opus_int16, sLTP );
138     opus_int32          HarmShapeFIRPacked_Q14;
139     opus_int            offset_Q10;
140     opus_int32          RDmin_Q10, Gain_Q10;
141     VARDECL( opus_int32, x_sc_Q10 );
142     VARDECL( opus_int32, delayedGain_Q10 );
143     VARDECL( NSQ_del_dec_struct, psDelDec );
144     NSQ_del_dec_struct  *psDD;
145     SAVE_STACK;
146
147     /* Set unvoiced lag to the previous one, overwrite later for voiced */
148     lag = NSQ->lagPrev;
149
150     silk_assert( NSQ->prev_gain_Q16 != 0 );
151
152     /* Initialize delayed decision states */
153     ALLOC( psDelDec, psEncC->nStatesDelayedDecision, NSQ_del_dec_struct );
154     silk_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );
155     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {
156         psDD                 = &psDelDec[ k ];
157         psDD->Seed           = ( k + psIndices->Seed ) & 3;
158         psDD->SeedInit       = psDD->Seed;
159         psDD->RD_Q10         = 0;
160         psDD->LF_AR_Q14      = NSQ->sLF_AR_shp_Q14;
161         psDD->Shape_Q14[ 0 ] = NSQ->sLTP_shp_Q14[ psEncC->ltp_mem_length - 1 ];
162         silk_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
163         silk_memcpy( psDD->sAR2_Q14, NSQ->sAR2_Q14, sizeof( NSQ->sAR2_Q14 ) );
164     }
165
166     offset_Q10   = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
167     smpl_buf_idx = 0; /* index of oldest samples */
168
169     decisionDelay = silk_min_int( DECISION_DELAY, psEncC->subfr_length );
170
171     /* For voiced frames limit the decision delay to lower than the pitch lag */
172     if( psIndices->signalType == TYPE_VOICED ) {
173         for( k = 0; k < psEncC->nb_subfr; k++ ) {
174             decisionDelay = silk_min_int( decisionDelay, pitchL[ k ] - LTP_ORDER / 2 - 1 );
175         }
176     } else {
177         if( lag > 0 ) {
178             decisionDelay = silk_min_int( decisionDelay, lag - LTP_ORDER / 2 - 1 );
179         }
180     }
181
182     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
183         LSF_interpolation_flag = 0;
184     } else {
185         LSF_interpolation_flag = 1;
186     }
187
188     ALLOC( sLTP_Q15,
189            psEncC->ltp_mem_length + psEncC->frame_length, opus_int32 );
190     ALLOC( sLTP, psEncC->ltp_mem_length + psEncC->frame_length, opus_int16 );
191     ALLOC( x_sc_Q10, psEncC->subfr_length, opus_int32 );
192     ALLOC( delayedGain_Q10, DECISION_DELAY, opus_int32 );
193     /* Set up pointers to start of sub frame */
194     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
195     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
196     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
197     subfr = 0;
198     for( k = 0; k < psEncC->nb_subfr; k++ ) {
199         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];
200         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];
201         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];
202
203         /* Noise shape parameters */
204         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
205         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
206         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( (opus_int32)silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
207
208         NSQ->rewhite_flag = 0;
209         if( psIndices->signalType == TYPE_VOICED ) {
210             /* Voiced */
211             lag = pitchL[ k ];
212
213             /* Re-whitening */
214             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
215                 if( k == 2 ) {
216                     /* RESET DELAYED DECISIONS */
217                     /* Find winner */
218                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
219                     Winner_ind = 0;
220                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {
221                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {
222                             RDmin_Q10 = psDelDec[ i ].RD_Q10;
223                             Winner_ind = i;
224                         }
225                     }
226                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {
227                         if( i != Winner_ind ) {
228                             psDelDec[ i ].RD_Q10 += ( silk_int32_MAX >> 4 );
229                             silk_assert( psDelDec[ i ].RD_Q10 >= 0 );
230                         }
231                     }
232
233                     /* Copy final part of signals from winner state to output and long-term filter states */
234                     psDD = &psDelDec[ Winner_ind ];
235                     last_smple_idx = smpl_buf_idx + decisionDelay;
236                     for( i = 0; i < decisionDelay; i++ ) {
237                         last_smple_idx = ( last_smple_idx - 1 ) % DECISION_DELAY;
238                         if( last_smple_idx < 0 ) last_smple_idx += DECISION_DELAY;
239                         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
240                         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
241                             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gains_Q16[ 1 ] ), 14 ) );
242                         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
243                     }
244
245                     subfr = 0;
246                 }
247
248                 /* Rewhiten with new A coefs */
249                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
250                 silk_assert( start_idx > 0 );
251
252                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
253                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder, psEncC->arch );
254
255                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
256                 NSQ->rewhite_flag = 1;
257             }
258         }
259
260         silk_nsq_del_dec_scale_states_sse4_1( psEncC, NSQ, psDelDec, x_Q3, x_sc_Q10, sLTP, sLTP_Q15, k,
261             psEncC->nStatesDelayedDecision, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType, decisionDelay );
262
263         silk_noise_shape_quantizer_del_dec_sse4_1( NSQ, psDelDec, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15,
264             delayedGain_Q10, A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ],
265             Gains_Q16[ k ], Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder,
266             psEncC->predictLPCOrder, psEncC->warping_Q16, psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay );
267
268         x_Q3   += psEncC->subfr_length;
269         pulses += psEncC->subfr_length;
270         pxq    += psEncC->subfr_length;
271     }
272
273     /* Find winner */
274     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
275     Winner_ind = 0;
276     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {
277         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {
278             RDmin_Q10 = psDelDec[ k ].RD_Q10;
279             Winner_ind = k;
280         }
281     }
282
283     /* Copy final part of signals from winner state to output and long-term filter states */
284     psDD = &psDelDec[ Winner_ind ];
285     psIndices->Seed = psDD->SeedInit;
286     last_smple_idx = smpl_buf_idx + decisionDelay;
287     Gain_Q10 = silk_RSHIFT32( Gains_Q16[ psEncC->nb_subfr - 1 ], 6 );
288     for( i = 0; i < decisionDelay; i++ ) {
289         last_smple_idx = ( last_smple_idx - 1 ) % DECISION_DELAY;
290         if( last_smple_idx < 0 ) last_smple_idx += DECISION_DELAY;
291         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
292         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
293             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gain_Q10 ), 8 ) );
294         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
295     }
296     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
297     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
298
299     /* Update states */
300     NSQ->sLF_AR_shp_Q14 = psDD->LF_AR_Q14;
301     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
302
303     /* Save quantized speech signal */
304     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[psEncC->ltp_mem_length], psEncC->frame_length * sizeof( opus_int16 ) ) */
305     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
306     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
307     RESTORE_STACK;
308 }
309
310 /******************************************/
311 /* Noise shape quantizer for one subframe */
312 /******************************************/
313 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec_sse4_1(
314     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
315     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
316     opus_int            signalType,             /* I    Signal type                         */
317     const opus_int32    x_Q10[],                /* I                                        */
318     opus_int8           pulses[],               /* O                                        */
319     opus_int16          xq[],                   /* O                                        */
320     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
321     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
322     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
323     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
324     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
325     opus_int            lag,                    /* I    Pitch lag                           */
326     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
327     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
328     opus_int32          LF_shp_Q14,             /* I                                        */
329     opus_int32          Gain_Q16,               /* I                                        */
330     opus_int            Lambda_Q10,             /* I                                        */
331     opus_int            offset_Q10,             /* I                                        */
332     opus_int            length,                 /* I    Input length                        */
333     opus_int            subfr,                  /* I    Subframe number                     */
334     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
335     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
336     opus_int            warping_Q16,            /* I                                        */
337     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
338     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
339     opus_int            decisionDelay           /* I                                        */
340 )
341 {
342     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
343     opus_int32   Winner_rand_state;
344     opus_int32   LTP_pred_Q14, LPC_pred_Q14, n_AR_Q14, n_LTP_Q14;
345     opus_int32   n_LF_Q14, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
346     opus_int32   q1_Q0, q1_Q10, q2_Q10, exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
347     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
348     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
349     VARDECL( NSQ_sample_pair, psSampleState );
350     NSQ_del_dec_struct *psDD;
351     NSQ_sample_struct  *psSS;
352
353     __m128i a_Q12_0123, a_Q12_4567, a_Q12_89AB, a_Q12_CDEF;
354     __m128i b_Q12_0123, b_sr_Q12_0123;
355     SAVE_STACK;
356
357     silk_assert( nStatesDelayedDecision > 0 );
358     ALLOC( psSampleState, nStatesDelayedDecision, NSQ_sample_pair );
359
360     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
361     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
362     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
363
364     a_Q12_0123 = OP_CVTEPI16_EPI32_M64( a_Q12 );
365     a_Q12_4567 = OP_CVTEPI16_EPI32_M64( a_Q12 + 4 );
366
367     if( opus_likely( predictLPCOrder == 16 ) ) {
368         a_Q12_89AB = OP_CVTEPI16_EPI32_M64( a_Q12 + 8 );
369         a_Q12_CDEF = OP_CVTEPI16_EPI32_M64( a_Q12 + 12 );
370     }
371
372     if( signalType == TYPE_VOICED ){
373         b_Q12_0123 = OP_CVTEPI16_EPI32_M64( b_Q14 );
374         b_sr_Q12_0123 = _mm_shuffle_epi32( b_Q12_0123, _MM_SHUFFLE( 0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
375     }
376     for( i = 0; i < length; i++ ) {
377         /* Perform common calculations used in all states */
378
379         /* Long-term prediction */
380         if( signalType == TYPE_VOICED ) {
381             /* Unrolled loop */
382             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
383             LTP_pred_Q14 = 2;
384             {
385                 __m128i tmpa, tmpb, pred_lag_ptr_tmp;
386                 pred_lag_ptr_tmp    = _mm_loadu_si128( (__m128i *)(&pred_lag_ptr[ -3 ] ) );
387                 pred_lag_ptr_tmp    = _mm_shuffle_epi32( pred_lag_ptr_tmp, 0x1B );
388                 tmpa                = _mm_mul_epi32( pred_lag_ptr_tmp, b_Q12_0123 );
389                 tmpa                = _mm_srli_si128( tmpa, 2 );
390
391                 pred_lag_ptr_tmp = _mm_shuffle_epi32( pred_lag_ptr_tmp, _MM_SHUFFLE( 0, 3, 2, 1 ) );/* equal shift right 4 bytes */
392                 pred_lag_ptr_tmp    = _mm_mul_epi32( pred_lag_ptr_tmp, b_sr_Q12_0123 );
393                 pred_lag_ptr_tmp    = _mm_srli_si128( pred_lag_ptr_tmp, 2 );
394                 pred_lag_ptr_tmp    = _mm_add_epi32( pred_lag_ptr_tmp, tmpa );
395
396                 tmpb = _mm_shuffle_epi32( pred_lag_ptr_tmp, _MM_SHUFFLE( 0, 0, 3, 2 ) );/* equal shift right 8 bytes */
397                 pred_lag_ptr_tmp    = _mm_add_epi32( pred_lag_ptr_tmp, tmpb );
398                 LTP_pred_Q14        += _mm_cvtsi128_si32( pred_lag_ptr_tmp );
399
400                 LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
401                 LTP_pred_Q14 = silk_LSHIFT( LTP_pred_Q14, 1 );                          /* Q13 -> Q14 */
402                 pred_lag_ptr++;
403             }
404         } else {
405             LTP_pred_Q14 = 0;
406         }
407
408         /* Long-term shaping */
409         if( lag > 0 ) {
410             /* Symmetric, packed FIR coefficients */
411             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
412             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
413             n_LTP_Q14 = silk_SUB_LSHIFT32( LTP_pred_Q14, n_LTP_Q14, 2 );            /* Q12 -> Q14 */
414             shp_lag_ptr++;
415         } else {
416             n_LTP_Q14 = 0;
417         }
418         {
419             __m128i tmpa, tmpb, psLPC_Q14_tmp, a_Q12_tmp;
420
421             for( k = 0; k < nStatesDelayedDecision; k++ ) {
422                 /* Delayed decision state */
423                 psDD = &psDelDec[ k ];
424
425                 /* Sample state */
426                 psSS = psSampleState[ k ];
427
428                 /* Generate dither */
429                 psDD->Seed = silk_RAND( psDD->Seed );
430
431                 /* Pointer used in short term prediction and shaping */
432                 psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
433                 /* Short-term prediction */
434                 silk_assert( predictLPCOrder == 10 || predictLPCOrder == 16 );
435                 /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
436                 LPC_pred_Q14 = silk_RSHIFT( predictLPCOrder, 1 );
437
438                 tmpb = _mm_setzero_si128();
439
440                 /* step 1 */
441                 psLPC_Q14_tmp   = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[ -3 ] ) ); /* -3, -2 , -1, 0 */
442                 psLPC_Q14_tmp   = _mm_shuffle_epi32( psLPC_Q14_tmp, 0x1B );      /* 0, -1, -2, -3 */
443                 tmpa            = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_0123 );    /* 0, -1, -2, -3 * 0123 -> 0*0, 2*-2 */
444
445                 tmpa            = _mm_srli_epi64( tmpa, 16 );
446                 tmpb            = _mm_add_epi32( tmpb, tmpa );
447
448                 psLPC_Q14_tmp = _mm_shuffle_epi32( psLPC_Q14_tmp, _MM_SHUFFLE( 0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
449                 a_Q12_tmp = _mm_shuffle_epi32( a_Q12_0123, _MM_SHUFFLE(0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
450                 psLPC_Q14_tmp   = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_tmp ); /* 1*-1, 3*-3 */
451                 psLPC_Q14_tmp   = _mm_srli_epi64( psLPC_Q14_tmp, 16 );
452                 tmpb            = _mm_add_epi32( tmpb, psLPC_Q14_tmp );
453
454                 /* step 2 */
455                 psLPC_Q14_tmp   = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[ -7 ] ) );
456                 psLPC_Q14_tmp   = _mm_shuffle_epi32( psLPC_Q14_tmp, 0x1B );
457                 tmpa            = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_4567 );
458                 tmpa            = _mm_srli_epi64( tmpa, 16 );
459                 tmpb            = _mm_add_epi32( tmpb, tmpa );
460
461                 psLPC_Q14_tmp = _mm_shuffle_epi32( psLPC_Q14_tmp, _MM_SHUFFLE( 0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
462                 a_Q12_tmp = _mm_shuffle_epi32( a_Q12_4567, _MM_SHUFFLE(0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
463                 psLPC_Q14_tmp   = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_tmp );
464                 psLPC_Q14_tmp   = _mm_srli_epi64( psLPC_Q14_tmp, 16 );
465                 tmpb            = _mm_add_epi32( tmpb, psLPC_Q14_tmp );
466
467                 if ( opus_likely( predictLPCOrder == 16 ) )
468                 {
469                     /* step 3 */
470                     psLPC_Q14_tmp   = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[ -11 ] ) );
471                     psLPC_Q14_tmp   = _mm_shuffle_epi32( psLPC_Q14_tmp, 0x1B );
472                     tmpa            = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_89AB );
473                     tmpa            = _mm_srli_epi64( tmpa, 16 );
474                     tmpb            = _mm_add_epi32( tmpb, tmpa );
475
476                     psLPC_Q14_tmp = _mm_shuffle_epi32( psLPC_Q14_tmp, _MM_SHUFFLE( 0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
477                     a_Q12_tmp = _mm_shuffle_epi32( a_Q12_89AB, _MM_SHUFFLE(0, 3, 2, 1 ) );/* equal shift right 4 bytes */
478                     psLPC_Q14_tmp   = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_tmp );
479                     psLPC_Q14_tmp   = _mm_srli_epi64( psLPC_Q14_tmp, 16 );
480                     tmpb            = _mm_add_epi32( tmpb, psLPC_Q14_tmp );
481
482                     /* setp 4 */
483                     psLPC_Q14_tmp   = _mm_loadu_si128( (__m128i *)(&psLPC_Q14[ -15 ] ) );
484                     psLPC_Q14_tmp   = _mm_shuffle_epi32( psLPC_Q14_tmp, 0x1B );
485                     tmpa            = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_CDEF );
486                     tmpa            = _mm_srli_epi64( tmpa, 16 );
487                     tmpb            = _mm_add_epi32( tmpb, tmpa );
488
489                     psLPC_Q14_tmp = _mm_shuffle_epi32( psLPC_Q14_tmp, _MM_SHUFFLE( 0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
490                     a_Q12_tmp = _mm_shuffle_epi32( a_Q12_CDEF, _MM_SHUFFLE(0, 3, 2, 1 ) ); /* equal shift right 4 bytes */
491                     psLPC_Q14_tmp   = _mm_mul_epi32( psLPC_Q14_tmp, a_Q12_tmp );
492                     psLPC_Q14_tmp   = _mm_srli_epi64( psLPC_Q14_tmp, 16 );
493                     tmpb            = _mm_add_epi32( tmpb, psLPC_Q14_tmp );
494
495                     /* add at last */
496                     /* equal shift right 8 bytes*/
497                     tmpa            = _mm_shuffle_epi32( tmpb, _MM_SHUFFLE( 0, 0, 3, 2 ) );
498                     tmpb            = _mm_add_epi32( tmpb, tmpa );
499                     LPC_pred_Q14    += _mm_cvtsi128_si32( tmpb );
500                 }
501                 else
502                 {
503                     /* add at last */
504                     tmpa            = _mm_shuffle_epi32( tmpb, _MM_SHUFFLE( 0, 0, 3, 2 ) ); /* equal shift right 8 bytes*/
505                     tmpb            = _mm_add_epi32( tmpb, tmpa );
506                     LPC_pred_Q14    += _mm_cvtsi128_si32( tmpb );
507
508                     LPC_pred_Q14 = silk_SMLAWB( LPC_pred_Q14, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
509                     LPC_pred_Q14 = silk_SMLAWB( LPC_pred_Q14, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
510                 }
511
512                 LPC_pred_Q14 = silk_LSHIFT( LPC_pred_Q14, 4 ); /* Q10 -> Q14 */
513
514                 /* Noise shape feedback */
515                 silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
516                 /* Output of lowpass section */
517                 tmp2 = silk_SMLAWB( psLPC_Q14[ 0 ], psDD->sAR2_Q14[ 0 ], warping_Q16 );
518                 /* Output of allpass section */
519                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
520                 psDD->sAR2_Q14[ 0 ] = tmp2;
521                 n_AR_Q14 = silk_RSHIFT( shapingLPCOrder, 1 );
522                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ 0 ] );
523                 /* Loop over allpass sections */
524                 for( j = 2; j < shapingLPCOrder; j += 2 ) {
525                     /* Output of allpass section */
526                     tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
527                     psDD->sAR2_Q14[ j - 1 ] = tmp1;
528                     n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ j - 1 ] );
529                     /* Output of allpass section */
530                     tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
531                     psDD->sAR2_Q14[ j + 0 ] = tmp2;
532                     n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ j ] );
533                 }
534                 psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
535                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
536
537                 n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 1 );                                      /* Q11 -> Q12 */
538                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, psDD->LF_AR_Q14, Tilt_Q14 );              /* Q12 */
539                 n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 2 );                                      /* Q12 -> Q14 */
540
541                 n_LF_Q14 = silk_SMULWB( psDD->Shape_Q14[ *smpl_buf_idx ], LF_shp_Q14 );     /* Q12 */
542                 n_LF_Q14 = silk_SMLAWT( n_LF_Q14, psDD->LF_AR_Q14, LF_shp_Q14 );            /* Q12 */
543                 n_LF_Q14 = silk_LSHIFT( n_LF_Q14, 2 );                                      /* Q12 -> Q14 */
544
545                 /* Input minus prediction plus noise feedback                       */
546                 /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
547                 tmp1 = silk_ADD32( n_AR_Q14, n_LF_Q14 );                                    /* Q14 */
548                 tmp2 = silk_ADD32( n_LTP_Q14, LPC_pred_Q14 );                               /* Q13 */
549                 tmp1 = silk_SUB32( tmp2, tmp1 );                                            /* Q13 */
550                 tmp1 = silk_RSHIFT_ROUND( tmp1, 4 );                                        /* Q10 */
551
552                 r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                                     /* residual error Q10 */
553
554                 /* Flip sign depending on dither */
555                 if ( psDD->Seed < 0 ) {
556                     r_Q10 = -r_Q10;
557                 }
558                 r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
559
560                 /* Find two quantization level candidates and measure their rate-distortion */
561                 q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
562                 q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
563                 if( q1_Q0 > 0 ) {
564                     q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
565                     q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
566                     q2_Q10  = silk_ADD32( q1_Q10, 1024 );
567                     rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
568                     rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
569                 } else if( q1_Q0 == 0 ) {
570                     q1_Q10  = offset_Q10;
571                     q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
572                     rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
573                     rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
574                 } else if( q1_Q0 == -1 ) {
575                     q2_Q10  = offset_Q10;
576                     q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
577                     rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
578                     rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
579                 } else {            /* q1_Q0 < -1 */
580                     q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
581                     q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
582                     q2_Q10  = silk_ADD32( q1_Q10, 1024 );
583                     rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
584                     rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
585                 }
586                 rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
587                 rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
588                 rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
589                 rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
590
591                 if( rd1_Q10 < rd2_Q10 ) {
592                     psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
593                     psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
594                     psSS[ 0 ].Q_Q10  = q1_Q10;
595                     psSS[ 1 ].Q_Q10  = q2_Q10;
596                 } else {
597                     psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
598                     psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
599                     psSS[ 0 ].Q_Q10  = q2_Q10;
600                     psSS[ 1 ].Q_Q10  = q1_Q10;
601                 }
602
603                 /* Update states for best quantization */
604
605                 /* Quantized excitation */
606                 exc_Q14 = silk_LSHIFT32( psSS[ 0 ].Q_Q10, 4 );
607                 if ( psDD->Seed < 0 ) {
608                     exc_Q14 = -exc_Q14;
609                 }
610
611                 /* Add predictions */
612                 LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
613                 xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
614
615                 /* Update states */
616                 sLF_AR_shp_Q14         = silk_SUB32( xq_Q14, n_AR_Q14 );
617                 psSS[ 0 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
618                 psSS[ 0 ].LF_AR_Q14    = sLF_AR_shp_Q14;
619                 psSS[ 0 ].LPC_exc_Q14  = LPC_exc_Q14;
620                 psSS[ 0 ].xq_Q14       = xq_Q14;
621
622                 /* Update states for second best quantization */
623
624                 /* Quantized excitation */
625                 exc_Q14 = silk_LSHIFT32( psSS[ 1 ].Q_Q10, 4 );
626                 if ( psDD->Seed < 0 ) {
627                     exc_Q14 = -exc_Q14;
628                 }
629
630
631                 /* Add predictions */
632                 LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
633                 xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
634
635                 /* Update states */
636                 sLF_AR_shp_Q14         = silk_SUB32( xq_Q14, n_AR_Q14 );
637                 psSS[ 1 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
638                 psSS[ 1 ].LF_AR_Q14    = sLF_AR_shp_Q14;
639                 psSS[ 1 ].LPC_exc_Q14  = LPC_exc_Q14;
640                 psSS[ 1 ].xq_Q14       = xq_Q14;
641             }
642         }
643         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) % DECISION_DELAY;
644         if( *smpl_buf_idx < 0 ) *smpl_buf_idx += DECISION_DELAY;
645         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) % DECISION_DELAY;
646
647         /* Find winner */
648         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
649         Winner_ind = 0;
650         for( k = 1; k < nStatesDelayedDecision; k++ ) {
651             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
652                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
653                 Winner_ind = k;
654             }
655         }
656
657         /* Increase RD values of expired states */
658         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
659         for( k = 0; k < nStatesDelayedDecision; k++ ) {
660             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
661                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, silk_int32_MAX >> 4 );
662                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, silk_int32_MAX >> 4 );
663                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
664             }
665         }
666
667         /* Find worst in first set and best in second set */
668         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
669         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
670         RDmax_ind = 0;
671         RDmin_ind = 0;
672         for( k = 1; k < nStatesDelayedDecision; k++ ) {
673             /* find worst in first set */
674             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
675                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
676                 RDmax_ind = k;
677             }
678             /* find best in second set */
679             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
680                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
681                 RDmin_ind = k;
682             }
683         }
684
685         /* Replace a state if best from second set outperforms worst in first set */
686         if( RDmin_Q10 < RDmax_Q10 ) {
687             silk_memcpy( ( (opus_int32 *)&psDelDec[ RDmax_ind ] ) + i,
688                          ( (opus_int32 *)&psDelDec[ RDmin_ind ] ) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
689             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
690         }
691
692         /* Write samples from winner to output and long-term filter states */
693         psDD = &psDelDec[ Winner_ind ];
694         if( subfr > 0 || i >= decisionDelay ) {
695             pulses[  i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
696             xq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
697                 silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], delayedGain_Q10[ last_smple_idx ] ), 8 ) );
698             NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q14[ last_smple_idx ];
699             sLTP_Q15[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q15[  last_smple_idx ];
700         }
701         NSQ->sLTP_shp_buf_idx++;
702         NSQ->sLTP_buf_idx++;
703
704         /* Update states */
705         for( k = 0; k < nStatesDelayedDecision; k++ ) {
706             psDD                                     = &psDelDec[ k ];
707             psSS                                     = &psSampleState[ k ][ 0 ];
708             psDD->LF_AR_Q14                          = psSS->LF_AR_Q14;
709             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
710             psDD->Xq_Q14[    *smpl_buf_idx ]         = psSS->xq_Q14;
711             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
712             psDD->Pred_Q15[  *smpl_buf_idx ]         = silk_LSHIFT32( psSS->LPC_exc_Q14, 1 );
713             psDD->Shape_Q14[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q14;
714             psDD->Seed                               = silk_ADD32_ovflw( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
715             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
716             psDD->RD_Q10                             = psSS->RD_Q10;
717         }
718         delayedGain_Q10[     *smpl_buf_idx ]         = Gain_Q10;
719     }
720     /* Update LPC states */
721     for( k = 0; k < nStatesDelayedDecision; k++ ) {
722         psDD = &psDelDec[ k ];
723         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
724     }
725     RESTORE_STACK;
726 }
727
728 static OPUS_INLINE void silk_nsq_del_dec_scale_states_sse4_1(
729     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
730     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
731     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
732     const opus_int32    x_Q3[],                     /* I    Input in Q3                         */
733     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
734     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
735     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
736     opus_int            subfr,                      /* I    Subframe number                     */
737     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
738     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
739     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
740     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
741     const opus_int      signal_type,                /* I    Signal type                         */
742     const opus_int      decisionDelay               /* I    Decision delay                      */
743 )
744 {
745     opus_int            i, k, lag;
746     opus_int32          gain_adj_Q16, inv_gain_Q31, inv_gain_Q23;
747     NSQ_del_dec_struct  *psDD;
748     __m128i xmm_inv_gain_Q23, xmm_x_Q3_x2x0, xmm_x_Q3_x3x1;
749
750     lag          = pitchL[ subfr ];
751     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
752
753     silk_assert( inv_gain_Q31 != 0 );
754
755     /* Calculate gain adjustment factor */
756     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
757         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
758     } else {
759         gain_adj_Q16 = (opus_int32)1 << 16;
760     }
761
762     /* Scale input */
763     inv_gain_Q23 = silk_RSHIFT_ROUND( inv_gain_Q31, 8 );
764
765     /* prepare inv_gain_Q23 in packed 4 32-bits */
766     xmm_inv_gain_Q23 = _mm_set1_epi32(inv_gain_Q23);
767
768     for( i = 0; i < psEncC->subfr_length - 3; i += 4 ) {
769         xmm_x_Q3_x2x0 = _mm_loadu_si128( (__m128i *)(&(x_Q3[ i ] ) ) );
770         /* equal shift right 4 bytes*/
771         xmm_x_Q3_x3x1 = _mm_shuffle_epi32( xmm_x_Q3_x2x0, _MM_SHUFFLE( 0, 3, 2, 1 ) );
772
773         xmm_x_Q3_x2x0 = _mm_mul_epi32( xmm_x_Q3_x2x0, xmm_inv_gain_Q23 );
774         xmm_x_Q3_x3x1 = _mm_mul_epi32( xmm_x_Q3_x3x1, xmm_inv_gain_Q23 );
775
776         xmm_x_Q3_x2x0 = _mm_srli_epi64( xmm_x_Q3_x2x0, 16 );
777         xmm_x_Q3_x3x1 = _mm_slli_epi64( xmm_x_Q3_x3x1, 16 );
778
779         xmm_x_Q3_x2x0 = _mm_blend_epi16( xmm_x_Q3_x2x0, xmm_x_Q3_x3x1, 0xCC );
780
781         _mm_storeu_si128( (__m128i *)(&(x_sc_Q10[ i ])), xmm_x_Q3_x2x0 );
782     }
783
784     for( ; i < psEncC->subfr_length; i++ ) {
785         x_sc_Q10[ i ] = silk_SMULWW( x_Q3[ i ], inv_gain_Q23 );
786     }
787
788     /* Save inverse gain */
789     NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
790
791     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
792     if( NSQ->rewhite_flag ) {
793         if( subfr == 0 ) {
794             /* Do LTP downscaling */
795             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
796         }
797         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
798             silk_assert( i < MAX_FRAME_LENGTH );
799             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
800         }
801     }
802
803     /* Adjust for changing gain */
804     if( gain_adj_Q16 != (opus_int32)1 << 16 ) {
805         /* Scale long-term shaping state */
806         {
807             __m128i xmm_gain_adj_Q16, xmm_sLTP_shp_Q14_x2x0, xmm_sLTP_shp_Q14_x3x1;
808
809             /* prepare gain_adj_Q16 in packed 4 32-bits */
810             xmm_gain_adj_Q16 = _mm_set1_epi32( gain_adj_Q16 );
811
812             for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx - 3; i += 4 )
813             {
814                 xmm_sLTP_shp_Q14_x2x0 = _mm_loadu_si128( (__m128i *)(&(NSQ->sLTP_shp_Q14[ i ] ) ) );
815                 /* equal shift right 4 bytes*/
816                 xmm_sLTP_shp_Q14_x3x1 = _mm_shuffle_epi32( xmm_sLTP_shp_Q14_x2x0, _MM_SHUFFLE( 0, 3, 2, 1 ) );
817
818                 xmm_sLTP_shp_Q14_x2x0 = _mm_mul_epi32( xmm_sLTP_shp_Q14_x2x0, xmm_gain_adj_Q16 );
819                 xmm_sLTP_shp_Q14_x3x1 = _mm_mul_epi32( xmm_sLTP_shp_Q14_x3x1, xmm_gain_adj_Q16 );
820
821                 xmm_sLTP_shp_Q14_x2x0 = _mm_srli_epi64( xmm_sLTP_shp_Q14_x2x0, 16 );
822                 xmm_sLTP_shp_Q14_x3x1 = _mm_slli_epi64( xmm_sLTP_shp_Q14_x3x1, 16 );
823
824                 xmm_sLTP_shp_Q14_x2x0 = _mm_blend_epi16( xmm_sLTP_shp_Q14_x2x0, xmm_sLTP_shp_Q14_x3x1, 0xCC );
825
826                 _mm_storeu_si128( (__m128i *)(&(NSQ->sLTP_shp_Q14[ i ] ) ), xmm_sLTP_shp_Q14_x2x0 );
827             }
828
829             for( ; i < NSQ->sLTP_shp_buf_idx; i++ ) {
830                 NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
831             }
832
833             /* Scale long-term prediction state */
834             if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
835                 for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
836                     sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
837                 }
838             }
839
840             for( k = 0; k < nStatesDelayedDecision; k++ ) {
841                 psDD = &psDelDec[ k ];
842
843                 /* Scale scalar states */
844                 psDD->LF_AR_Q14 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q14 );
845
846                 /* Scale short-term prediction and shaping states */
847                 for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
848                     psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
849                 }
850                 for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
851                     psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
852                 }
853                 for( i = 0; i < DECISION_DELAY; i++ ) {
854                     psDD->Pred_Q15[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q15[  i ] );
855                     psDD->Shape_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q14[ i ] );
856                 }
857             }
858         }
859     }
860 }