Apply Neon short prediction optimization to silk_noise_shape_quantizer_del_dec.
[opus.git] / silk / NSQ_del_dec.c
1 /***********************************************************************
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3 Redistribution and use in source and binary forms, with or without
4 modification, are permitted provided that the following conditions
5 are met:
6 - Redistributions of source code must retain the above copyright notice,
7 this list of conditions and the following disclaimer.
8 - Redistributions in binary form must reproduce the above copyright
9 notice, this list of conditions and the following disclaimer in the
10 documentation and/or other materials provided with the distribution.
11 - Neither the name of Internet Society, IETF or IETF Trust, nor the
12 names of specific contributors, may be used to endorse or promote
13 products derived from this software without specific prior written
14 permission.
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "main.h"
33 #include "stack_alloc.h"
34 #include "NSQ.h"
35
36
37 typedef struct {
38     opus_int32 sLPC_Q14[ MAX_SUB_FRAME_LENGTH + NSQ_LPC_BUF_LENGTH ];
39     opus_int32 RandState[ DECISION_DELAY ];
40     opus_int32 Q_Q10[     DECISION_DELAY ];
41     opus_int32 Xq_Q14[    DECISION_DELAY ];
42     opus_int32 Pred_Q15[  DECISION_DELAY ];
43     opus_int32 Shape_Q14[ DECISION_DELAY ];
44     opus_int32 sAR2_Q14[ MAX_SHAPE_LPC_ORDER ];
45     opus_int32 LF_AR_Q14;
46     opus_int32 Seed;
47     opus_int32 SeedInit;
48     opus_int32 RD_Q10;
49 } NSQ_del_dec_struct;
50
51 typedef struct {
52     opus_int32 Q_Q10;
53     opus_int32 RD_Q10;
54     opus_int32 xq_Q14;
55     opus_int32 LF_AR_Q14;
56     opus_int32 sLTP_shp_Q14;
57     opus_int32 LPC_exc_Q14;
58 } NSQ_sample_struct;
59
60 typedef NSQ_sample_struct  NSQ_sample_pair[ 2 ];
61
62 #if defined(MIPSr1_ASM)
63 #include "mips/NSQ_del_dec_mipsr1.h"
64 #endif
65 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
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(
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     int                 arch                    /* I                                        */
113 );
114
115 void silk_NSQ_del_dec_c(
116     const silk_encoder_state    *psEncC,                                    /* I/O  Encoder State                   */
117     silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                       */
118     SideInfoIndices             *psIndices,                                 /* I/O  Quantization Indices            */
119     const opus_int32            x_Q3[],                                     /* I    Prefiltered input signal        */
120     opus_int8                   pulses[],                                   /* O    Quantized pulse signal          */
121     const opus_int16            PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefs     */
122     const opus_int16            LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefs      */
123     const opus_int16            AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I Noise shaping coefs             */
124     const opus_int              HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I    Long term shaping coefs         */
125     const opus_int              Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                   */
126     const opus_int32            LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I    Low frequency shaping coefs     */
127     const opus_int32            Gains_Q16[ MAX_NB_SUBFR ],                  /* I    Quantization step sizes         */
128     const opus_int              pitchL[ MAX_NB_SUBFR ],                     /* I    Pitch lags                      */
129     const opus_int              Lambda_Q10,                                 /* I    Rate/distortion tradeoff        */
130     const opus_int              LTP_scale_Q14                               /* I    LTP state scaling               */
131 )
132 {
133     opus_int            i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;
134     opus_int            last_smple_idx, smpl_buf_idx, decisionDelay;
135     const opus_int16    *A_Q12, *B_Q14, *AR_shp_Q13;
136     opus_int16          *pxq;
137     VARDECL( opus_int32, sLTP_Q15 );
138     VARDECL( opus_int16, sLTP );
139     opus_int32          HarmShapeFIRPacked_Q14;
140     opus_int            offset_Q10;
141     opus_int32          RDmin_Q10, Gain_Q10;
142     VARDECL( opus_int32, x_sc_Q10 );
143     VARDECL( opus_int32, delayedGain_Q10 );
144     VARDECL( NSQ_del_dec_struct, psDelDec );
145     NSQ_del_dec_struct  *psDD;
146     SAVE_STACK;
147
148     /* Set unvoiced lag to the previous one, overwrite later for voiced */
149     lag = NSQ->lagPrev;
150
151     silk_assert( NSQ->prev_gain_Q16 != 0 );
152
153     /* Initialize delayed decision states */
154     ALLOC( psDelDec, psEncC->nStatesDelayedDecision, NSQ_del_dec_struct );
155     silk_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );
156     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {
157         psDD                 = &psDelDec[ k ];
158         psDD->Seed           = ( k + psIndices->Seed ) & 3;
159         psDD->SeedInit       = psDD->Seed;
160         psDD->RD_Q10         = 0;
161         psDD->LF_AR_Q14      = NSQ->sLF_AR_shp_Q14;
162         psDD->Shape_Q14[ 0 ] = NSQ->sLTP_shp_Q14[ psEncC->ltp_mem_length - 1 ];
163         silk_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
164         silk_memcpy( psDD->sAR2_Q14, NSQ->sAR2_Q14, sizeof( NSQ->sAR2_Q14 ) );
165     }
166
167     offset_Q10   = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
168     smpl_buf_idx = 0; /* index of oldest samples */
169
170     decisionDelay = silk_min_int( DECISION_DELAY, psEncC->subfr_length );
171
172     /* For voiced frames limit the decision delay to lower than the pitch lag */
173     if( psIndices->signalType == TYPE_VOICED ) {
174         for( k = 0; k < psEncC->nb_subfr; k++ ) {
175             decisionDelay = silk_min_int( decisionDelay, pitchL[ k ] - LTP_ORDER / 2 - 1 );
176         }
177     } else {
178         if( lag > 0 ) {
179             decisionDelay = silk_min_int( decisionDelay, lag - LTP_ORDER / 2 - 1 );
180         }
181     }
182
183     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
184         LSF_interpolation_flag = 0;
185     } else {
186         LSF_interpolation_flag = 1;
187     }
188
189     ALLOC( sLTP_Q15,
190            psEncC->ltp_mem_length + psEncC->frame_length, opus_int32 );
191     ALLOC( sLTP, psEncC->ltp_mem_length + psEncC->frame_length, opus_int16 );
192     ALLOC( x_sc_Q10, psEncC->subfr_length, opus_int32 );
193     ALLOC( delayedGain_Q10, DECISION_DELAY, opus_int32 );
194     /* Set up pointers to start of sub frame */
195     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
196     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
197     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
198     subfr = 0;
199     for( k = 0; k < psEncC->nb_subfr; k++ ) {
200         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];
201         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];
202         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];
203
204         /* Noise shape parameters */
205         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
206         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
207         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( (opus_int32)silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
208
209         NSQ->rewhite_flag = 0;
210         if( psIndices->signalType == TYPE_VOICED ) {
211             /* Voiced */
212             lag = pitchL[ k ];
213
214             /* Re-whitening */
215             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
216                 if( k == 2 ) {
217                     /* RESET DELAYED DECISIONS */
218                     /* Find winner */
219                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
220                     Winner_ind = 0;
221                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {
222                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {
223                             RDmin_Q10 = psDelDec[ i ].RD_Q10;
224                             Winner_ind = i;
225                         }
226                     }
227                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {
228                         if( i != Winner_ind ) {
229                             psDelDec[ i ].RD_Q10 += ( silk_int32_MAX >> 4 );
230                             silk_assert( psDelDec[ i ].RD_Q10 >= 0 );
231                         }
232                     }
233
234                     /* Copy final part of signals from winner state to output and long-term filter states */
235                     psDD = &psDelDec[ Winner_ind ];
236                     last_smple_idx = smpl_buf_idx + decisionDelay;
237                     for( i = 0; i < decisionDelay; i++ ) {
238                         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
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( 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( 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, psEncC->arch );
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_MASK;
290         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
291         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
292             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gain_Q10 ), 8 ) );
293         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
294     }
295     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
296     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
297
298     /* Update states */
299     NSQ->sLF_AR_shp_Q14 = psDD->LF_AR_Q14;
300     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
301
302     /* Save quantized speech signal */
303     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[psEncC->ltp_mem_length], psEncC->frame_length * sizeof( opus_int16 ) ) */
304     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
305     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
306     RESTORE_STACK;
307 }
308
309 /******************************************/
310 /* Noise shape quantizer for one subframe */
311 /******************************************/
312 #ifndef OVERRIDE_silk_noise_shape_quantizer_del_dec
313 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
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     int                 arch                    /* I                                        */
341 )
342 {
343     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
344     opus_int32   Winner_rand_state;
345     opus_int32   LTP_pred_Q14, LPC_pred_Q14, n_AR_Q14, n_LTP_Q14;
346     opus_int32   n_LF_Q14, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
347     opus_int32   q1_Q0, q1_Q10, q2_Q10, exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
348     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
349     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
350 #ifdef silk_short_prediction_create_arch_coef
351     opus_int32   a_Q12_arch[MAX_LPC_ORDER];
352 #endif
353
354     VARDECL( NSQ_sample_pair, psSampleState );
355     NSQ_del_dec_struct *psDD;
356     NSQ_sample_struct  *psSS;
357     SAVE_STACK;
358
359     silk_assert( nStatesDelayedDecision > 0 );
360     ALLOC( psSampleState, nStatesDelayedDecision, NSQ_sample_pair );
361
362     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
363     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
364     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
365
366 #ifdef silk_short_prediction_create_arch_coef
367     silk_short_prediction_create_arch_coef(a_Q12_arch, a_Q12, predictLPCOrder);
368 #endif
369
370     for( i = 0; i < length; i++ ) {
371         /* Perform common calculations used in all states */
372
373         /* Long-term prediction */
374         if( signalType == TYPE_VOICED ) {
375             /* Unrolled loop */
376             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
377             LTP_pred_Q14 = 2;
378             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
379             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
380             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
381             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
382             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
383             LTP_pred_Q14 = silk_LSHIFT( LTP_pred_Q14, 1 );                          /* Q13 -> Q14 */
384             pred_lag_ptr++;
385         } else {
386             LTP_pred_Q14 = 0;
387         }
388
389         /* Long-term shaping */
390         if( lag > 0 ) {
391             /* Symmetric, packed FIR coefficients */
392             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
393             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
394             n_LTP_Q14 = silk_SUB_LSHIFT32( LTP_pred_Q14, n_LTP_Q14, 2 );            /* Q12 -> Q14 */
395             shp_lag_ptr++;
396         } else {
397             n_LTP_Q14 = 0;
398         }
399
400         for( k = 0; k < nStatesDelayedDecision; k++ ) {
401             /* Delayed decision state */
402             psDD = &psDelDec[ k ];
403
404             /* Sample state */
405             psSS = psSampleState[ k ];
406
407             /* Generate dither */
408             psDD->Seed = silk_RAND( psDD->Seed );
409
410             /* Pointer used in short term prediction and shaping */
411             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
412             /* Short-term prediction */
413             LPC_pred_Q14 = silk_noise_shape_quantizer_short_prediction(psLPC_Q14, a_Q12, a_Q12_arch, predictLPCOrder, arch);
414             LPC_pred_Q14 = silk_LSHIFT( LPC_pred_Q14, 4 );                              /* Q10 -> Q14 */
415
416             /* Noise shape feedback */
417             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
418             /* Output of lowpass section */
419             tmp2 = silk_SMLAWB( psLPC_Q14[ 0 ], psDD->sAR2_Q14[ 0 ], warping_Q16 );
420             /* Output of allpass section */
421             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
422             psDD->sAR2_Q14[ 0 ] = tmp2;
423             n_AR_Q14 = silk_RSHIFT( shapingLPCOrder, 1 );
424             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ 0 ] );
425             /* Loop over allpass sections */
426             for( j = 2; j < shapingLPCOrder; j += 2 ) {
427                 /* Output of allpass section */
428                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
429                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
430                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ j - 1 ] );
431                 /* Output of allpass section */
432                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
433                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
434                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ j ] );
435             }
436             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
437             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
438
439             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 1 );                                      /* Q11 -> Q12 */
440             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, psDD->LF_AR_Q14, Tilt_Q14 );              /* Q12 */
441             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 2 );                                      /* Q12 -> Q14 */
442
443             n_LF_Q14 = silk_SMULWB( psDD->Shape_Q14[ *smpl_buf_idx ], LF_shp_Q14 );     /* Q12 */
444             n_LF_Q14 = silk_SMLAWT( n_LF_Q14, psDD->LF_AR_Q14, LF_shp_Q14 );            /* Q12 */
445             n_LF_Q14 = silk_LSHIFT( n_LF_Q14, 2 );                                      /* Q12 -> Q14 */
446
447             /* Input minus prediction plus noise feedback                       */
448             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
449             tmp1 = silk_ADD32( n_AR_Q14, n_LF_Q14 );                                    /* Q14 */
450             tmp2 = silk_ADD32( n_LTP_Q14, LPC_pred_Q14 );                               /* Q13 */
451             tmp1 = silk_SUB32( tmp2, tmp1 );                                            /* Q13 */
452             tmp1 = silk_RSHIFT_ROUND( tmp1, 4 );                                        /* Q10 */
453
454             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                                     /* residual error Q10 */
455
456             /* Flip sign depending on dither */
457             if ( psDD->Seed < 0 ) {
458                 r_Q10 = -r_Q10;
459             }
460             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
461
462             /* Find two quantization level candidates and measure their rate-distortion */
463             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
464             q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
465             if( q1_Q0 > 0 ) {
466                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
467                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
468                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
469                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
470                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
471             } else if( q1_Q0 == 0 ) {
472                 q1_Q10  = offset_Q10;
473                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
474                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
475                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
476             } else if( q1_Q0 == -1 ) {
477                 q2_Q10  = offset_Q10;
478                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
479                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
480                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
481             } else {            /* q1_Q0 < -1 */
482                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
483                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
484                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
485                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
486                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
487             }
488             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
489             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
490             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
491             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
492
493             if( rd1_Q10 < rd2_Q10 ) {
494                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
495                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
496                 psSS[ 0 ].Q_Q10  = q1_Q10;
497                 psSS[ 1 ].Q_Q10  = q2_Q10;
498             } else {
499                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
500                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
501                 psSS[ 0 ].Q_Q10  = q2_Q10;
502                 psSS[ 1 ].Q_Q10  = q1_Q10;
503             }
504
505             /* Update states for best quantization */
506
507             /* Quantized excitation */
508             exc_Q14 = silk_LSHIFT32( psSS[ 0 ].Q_Q10, 4 );
509             if ( psDD->Seed < 0 ) {
510                 exc_Q14 = -exc_Q14;
511             }
512
513             /* Add predictions */
514             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
515             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
516
517             /* Update states */
518             sLF_AR_shp_Q14         = silk_SUB32( xq_Q14, n_AR_Q14 );
519             psSS[ 0 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
520             psSS[ 0 ].LF_AR_Q14    = sLF_AR_shp_Q14;
521             psSS[ 0 ].LPC_exc_Q14  = LPC_exc_Q14;
522             psSS[ 0 ].xq_Q14       = xq_Q14;
523
524             /* Update states for second best quantization */
525
526             /* Quantized excitation */
527             exc_Q14 = silk_LSHIFT32( psSS[ 1 ].Q_Q10, 4 );
528             if ( psDD->Seed < 0 ) {
529                 exc_Q14 = -exc_Q14;
530             }
531
532
533             /* Add predictions */
534             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
535             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
536
537             /* Update states */
538             sLF_AR_shp_Q14         = silk_SUB32( xq_Q14, n_AR_Q14 );
539             psSS[ 1 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
540             psSS[ 1 ].LF_AR_Q14    = sLF_AR_shp_Q14;
541             psSS[ 1 ].LPC_exc_Q14  = LPC_exc_Q14;
542             psSS[ 1 ].xq_Q14       = xq_Q14;
543         }
544
545         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) & DECISION_DELAY_MASK;                   /* Index to newest samples              */
546         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) & DECISION_DELAY_MASK;       /* Index to decisionDelay old samples   */
547
548         /* Find winner */
549         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
550         Winner_ind = 0;
551         for( k = 1; k < nStatesDelayedDecision; k++ ) {
552             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
553                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
554                 Winner_ind = k;
555             }
556         }
557
558         /* Increase RD values of expired states */
559         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
560         for( k = 0; k < nStatesDelayedDecision; k++ ) {
561             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
562                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, silk_int32_MAX >> 4 );
563                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, silk_int32_MAX >> 4 );
564                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
565             }
566         }
567
568         /* Find worst in first set and best in second set */
569         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
570         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
571         RDmax_ind = 0;
572         RDmin_ind = 0;
573         for( k = 1; k < nStatesDelayedDecision; k++ ) {
574             /* find worst in first set */
575             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
576                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
577                 RDmax_ind = k;
578             }
579             /* find best in second set */
580             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
581                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
582                 RDmin_ind = k;
583             }
584         }
585
586         /* Replace a state if best from second set outperforms worst in first set */
587         if( RDmin_Q10 < RDmax_Q10 ) {
588             silk_memcpy( ( (opus_int32 *)&psDelDec[ RDmax_ind ] ) + i,
589                          ( (opus_int32 *)&psDelDec[ RDmin_ind ] ) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
590             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
591         }
592
593         /* Write samples from winner to output and long-term filter states */
594         psDD = &psDelDec[ Winner_ind ];
595         if( subfr > 0 || i >= decisionDelay ) {
596             pulses[  i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
597             xq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
598                 silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], delayedGain_Q10[ last_smple_idx ] ), 8 ) );
599             NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q14[ last_smple_idx ];
600             sLTP_Q15[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q15[  last_smple_idx ];
601         }
602         NSQ->sLTP_shp_buf_idx++;
603         NSQ->sLTP_buf_idx++;
604
605         /* Update states */
606         for( k = 0; k < nStatesDelayedDecision; k++ ) {
607             psDD                                     = &psDelDec[ k ];
608             psSS                                     = &psSampleState[ k ][ 0 ];
609             psDD->LF_AR_Q14                          = psSS->LF_AR_Q14;
610             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
611             psDD->Xq_Q14[    *smpl_buf_idx ]         = psSS->xq_Q14;
612             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
613             psDD->Pred_Q15[  *smpl_buf_idx ]         = silk_LSHIFT32( psSS->LPC_exc_Q14, 1 );
614             psDD->Shape_Q14[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q14;
615             psDD->Seed                               = silk_ADD32_ovflw( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
616             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
617             psDD->RD_Q10                             = psSS->RD_Q10;
618         }
619         delayedGain_Q10[     *smpl_buf_idx ]         = Gain_Q10;
620     }
621     /* Update LPC states */
622     for( k = 0; k < nStatesDelayedDecision; k++ ) {
623         psDD = &psDelDec[ k ];
624         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
625     }
626     RESTORE_STACK;
627 }
628 #endif /* OVERRIDE_silk_noise_shape_quantizer_del_dec */
629
630 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
631     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
632     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
633     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
634     const opus_int32    x_Q3[],                     /* I    Input in Q3                         */
635     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
636     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
637     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
638     opus_int            subfr,                      /* I    Subframe number                     */
639     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
640     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
641     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
642     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
643     const opus_int      signal_type,                /* I    Signal type                         */
644     const opus_int      decisionDelay               /* I    Decision delay                      */
645 )
646 {
647     opus_int            i, k, lag;
648     opus_int32          gain_adj_Q16, inv_gain_Q31, inv_gain_Q23;
649     NSQ_del_dec_struct  *psDD;
650
651     lag          = pitchL[ subfr ];
652     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
653     silk_assert( inv_gain_Q31 != 0 );
654
655     /* Calculate gain adjustment factor */
656     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
657         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
658     } else {
659         gain_adj_Q16 = (opus_int32)1 << 16;
660     }
661
662     /* Scale input */
663     inv_gain_Q23 = silk_RSHIFT_ROUND( inv_gain_Q31, 8 );
664     for( i = 0; i < psEncC->subfr_length; i++ ) {
665         x_sc_Q10[ i ] = silk_SMULWW( x_Q3[ i ], inv_gain_Q23 );
666     }
667
668     /* Save inverse gain */
669     NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
670
671     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
672     if( NSQ->rewhite_flag ) {
673         if( subfr == 0 ) {
674             /* Do LTP downscaling */
675             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
676         }
677         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
678             silk_assert( i < MAX_FRAME_LENGTH );
679             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
680         }
681     }
682
683     /* Adjust for changing gain */
684     if( gain_adj_Q16 != (opus_int32)1 << 16 ) {
685         /* Scale long-term shaping state */
686         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
687             NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
688         }
689
690         /* Scale long-term prediction state */
691         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
692             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
693                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
694             }
695         }
696
697         for( k = 0; k < nStatesDelayedDecision; k++ ) {
698             psDD = &psDelDec[ k ];
699
700             /* Scale scalar states */
701             psDD->LF_AR_Q14 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q14 );
702
703             /* Scale short-term prediction and shaping states */
704             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
705                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
706             }
707             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
708                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
709             }
710             for( i = 0; i < DECISION_DELAY; i++ ) {
711                 psDD->Pred_Q15[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q15[  i ] );
712                 psDD->Shape_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q14[ i ] );
713             }
714         }
715     }
716 }