Fix stability test
[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 Diff_Q14;
47     opus_int32 Seed;
48     opus_int32 SeedInit;
49     opus_int32 RD_Q10;
50 } NSQ_del_dec_struct;
51
52 typedef struct {
53     opus_int32 Q_Q10;
54     opus_int32 RD_Q10;
55     opus_int32 xq_Q14;
56     opus_int32 LF_AR_Q14;
57     opus_int32 Diff_Q14;
58     opus_int32 sLTP_shp_Q14;
59     opus_int32 LPC_exc_Q14;
60 } NSQ_sample_struct;
61
62 typedef NSQ_sample_struct  NSQ_sample_pair[ 2 ];
63
64 #if defined(MIPSr1_ASM)
65 #include "mips/NSQ_del_dec_mipsr1.h"
66 #endif
67 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
68     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
69     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
70     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
71     const opus_int16    x16[],                      /* I    Input                               */
72     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
73     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
74     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
75     opus_int            subfr,                      /* I    Subframe number                     */
76     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
77     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
78     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
79     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
80     const opus_int      signal_type,                /* I    Signal type                         */
81     const opus_int      decisionDelay               /* I    Decision delay                      */
82 );
83
84 /******************************************/
85 /* Noise shape quantizer for one subframe */
86 /******************************************/
87 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
88     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
89     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
90     opus_int            signalType,             /* I    Signal type                         */
91     const opus_int32    x_Q10[],                /* I                                        */
92     opus_int8           pulses[],               /* O                                        */
93     opus_int16          xq[],                   /* O                                        */
94     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
95     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
96     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
97     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
98     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
99     opus_int            lag,                    /* I    Pitch lag                           */
100     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
101     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
102     opus_int32          LF_shp_Q14,             /* I                                        */
103     opus_int32          Gain_Q16,               /* I                                        */
104     opus_int            Lambda_Q10,             /* I                                        */
105     opus_int            offset_Q10,             /* I                                        */
106     opus_int            length,                 /* I    Input length                        */
107     opus_int            subfr,                  /* I    Subframe number                     */
108     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
109     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
110     opus_int            warping_Q16,            /* I                                        */
111     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
112     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
113     opus_int            decisionDelay,          /* I                                        */
114     int                 arch                    /* I                                        */
115 );
116
117 void silk_NSQ_del_dec_c(
118     const silk_encoder_state    *psEncC,                                    /* I/O  Encoder State                   */
119     silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                       */
120     SideInfoIndices             *psIndices,                                 /* I/O  Quantization Indices            */
121     const opus_int16            x16[],                                        /* I    Input                           */
122     opus_int8                   pulses[],                                   /* O    Quantized pulse signal          */
123     const opus_int16            PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefs     */
124     const opus_int16            LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefs      */
125     const opus_int16            AR_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I Noise shaping coefs              */
126     const opus_int              HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I    Long term shaping coefs         */
127     const opus_int              Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                   */
128     const opus_int32            LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I    Low frequency shaping coefs     */
129     const opus_int32            Gains_Q16[ MAX_NB_SUBFR ],                  /* I    Quantization step sizes         */
130     const opus_int              pitchL[ MAX_NB_SUBFR ],                     /* I    Pitch lags                      */
131     const opus_int              Lambda_Q10,                                 /* I    Rate/distortion tradeoff        */
132     const opus_int              LTP_scale_Q14                               /* I    LTP state scaling               */
133 )
134 {
135     opus_int            i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;
136     opus_int            last_smple_idx, smpl_buf_idx, decisionDelay;
137     const opus_int16    *A_Q12, *B_Q14, *AR_shp_Q13;
138     opus_int16          *pxq;
139     VARDECL( opus_int32, sLTP_Q15 );
140     VARDECL( opus_int16, sLTP );
141     opus_int32          HarmShapeFIRPacked_Q14;
142     opus_int            offset_Q10;
143     opus_int32          RDmin_Q10, Gain_Q10;
144     VARDECL( opus_int32, x_sc_Q10 );
145     VARDECL( opus_int32, delayedGain_Q10 );
146     VARDECL( NSQ_del_dec_struct, psDelDec );
147     NSQ_del_dec_struct  *psDD;
148     SAVE_STACK;
149
150     /* Set unvoiced lag to the previous one, overwrite later for voiced */
151     lag = NSQ->lagPrev;
152
153     silk_assert( NSQ->prev_gain_Q16 != 0 );
154
155     /* Initialize delayed decision states */
156     ALLOC( psDelDec, psEncC->nStatesDelayedDecision, NSQ_del_dec_struct );
157     silk_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );
158     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {
159         psDD                 = &psDelDec[ k ];
160         psDD->Seed           = ( k + psIndices->Seed ) & 3;
161         psDD->SeedInit       = psDD->Seed;
162         psDD->RD_Q10         = 0;
163         psDD->LF_AR_Q14      = NSQ->sLF_AR_shp_Q14;
164         psDD->Diff_Q14       = NSQ->sDiff_shp_Q14;
165         psDD->Shape_Q14[ 0 ] = NSQ->sLTP_shp_Q14[ psEncC->ltp_mem_length - 1 ];
166         silk_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
167         silk_memcpy( psDD->sAR2_Q14, NSQ->sAR2_Q14, sizeof( NSQ->sAR2_Q14 ) );
168     }
169
170     offset_Q10   = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
171     smpl_buf_idx = 0; /* index of oldest samples */
172
173     decisionDelay = silk_min_int( DECISION_DELAY, psEncC->subfr_length );
174
175     /* For voiced frames limit the decision delay to lower than the pitch lag */
176     if( psIndices->signalType == TYPE_VOICED ) {
177         for( k = 0; k < psEncC->nb_subfr; k++ ) {
178             decisionDelay = silk_min_int( decisionDelay, pitchL[ k ] - LTP_ORDER / 2 - 1 );
179         }
180     } else {
181         if( lag > 0 ) {
182             decisionDelay = silk_min_int( decisionDelay, lag - LTP_ORDER / 2 - 1 );
183         }
184     }
185
186     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
187         LSF_interpolation_flag = 0;
188     } else {
189         LSF_interpolation_flag = 1;
190     }
191
192     ALLOC( sLTP_Q15, psEncC->ltp_mem_length + psEncC->frame_length, opus_int32 );
193     ALLOC( sLTP, psEncC->ltp_mem_length + psEncC->frame_length, opus_int16 );
194     ALLOC( x_sc_Q10, psEncC->subfr_length, opus_int32 );
195     ALLOC( delayedGain_Q10, DECISION_DELAY, opus_int32 );
196     /* Set up pointers to start of sub frame */
197     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
198     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
199     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
200     subfr = 0;
201     for( k = 0; k < psEncC->nb_subfr; k++ ) {
202         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];
203         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];
204         AR_shp_Q13 = &AR_Q13[     k * MAX_SHAPE_LPC_ORDER ];
205
206         /* Noise shape parameters */
207         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
208         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
209         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( (opus_int32)silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
210
211         NSQ->rewhite_flag = 0;
212         if( psIndices->signalType == TYPE_VOICED ) {
213             /* Voiced */
214             lag = pitchL[ k ];
215
216             /* Re-whitening */
217             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
218                 if( k == 2 ) {
219                     /* RESET DELAYED DECISIONS */
220                     /* Find winner */
221                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
222                     Winner_ind = 0;
223                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {
224                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {
225                             RDmin_Q10 = psDelDec[ i ].RD_Q10;
226                             Winner_ind = i;
227                         }
228                     }
229                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {
230                         if( i != Winner_ind ) {
231                             psDelDec[ i ].RD_Q10 += ( silk_int32_MAX >> 4 );
232                             silk_assert( psDelDec[ i ].RD_Q10 >= 0 );
233                         }
234                     }
235
236                     /* Copy final part of signals from winner state to output and long-term filter states */
237                     psDD = &psDelDec[ Winner_ind ];
238                     last_smple_idx = smpl_buf_idx + decisionDelay;
239                     for( i = 0; i < decisionDelay; i++ ) {
240                         last_smple_idx = ( last_smple_idx - 1 ) % DECISION_DELAY;
241                         if( last_smple_idx < 0 ) last_smple_idx += DECISION_DELAY;
242                         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
243                         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
244                             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gains_Q16[ 1 ] ), 14 ) );
245                         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
246                     }
247
248                     subfr = 0;
249                 }
250
251                 /* Rewhiten with new A coefs */
252                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
253                 silk_assert( start_idx > 0 );
254
255                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
256                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder, psEncC->arch );
257
258                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
259                 NSQ->rewhite_flag = 1;
260             }
261         }
262
263         silk_nsq_del_dec_scale_states( psEncC, NSQ, psDelDec, x16, x_sc_Q10, sLTP, sLTP_Q15, k,
264             psEncC->nStatesDelayedDecision, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType, decisionDelay );
265
266         silk_noise_shape_quantizer_del_dec( NSQ, psDelDec, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15,
267             delayedGain_Q10, A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ],
268             Gains_Q16[ k ], Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder,
269             psEncC->predictLPCOrder, psEncC->warping_Q16, psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay, psEncC->arch );
270
271         x16    += psEncC->subfr_length;
272         pulses += psEncC->subfr_length;
273         pxq    += psEncC->subfr_length;
274     }
275
276     /* Find winner */
277     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
278     Winner_ind = 0;
279     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {
280         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {
281             RDmin_Q10 = psDelDec[ k ].RD_Q10;
282             Winner_ind = k;
283         }
284     }
285
286     /* Copy final part of signals from winner state to output and long-term filter states */
287     psDD = &psDelDec[ Winner_ind ];
288     psIndices->Seed = psDD->SeedInit;
289     last_smple_idx = smpl_buf_idx + decisionDelay;
290     Gain_Q10 = silk_RSHIFT32( Gains_Q16[ psEncC->nb_subfr - 1 ], 6 );
291     for( i = 0; i < decisionDelay; i++ ) {
292         last_smple_idx = ( last_smple_idx - 1 ) % DECISION_DELAY;
293         if( last_smple_idx < 0 ) last_smple_idx += DECISION_DELAY;
294
295         pulses[   i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
296         pxq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
297             silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], Gain_Q10 ), 8 ) );
298         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q14[ last_smple_idx ];
299     }
300     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
301     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
302
303     /* Update states */
304     NSQ->sLF_AR_shp_Q14 = psDD->LF_AR_Q14;
305     NSQ->sDiff_shp_Q14  = psDD->Diff_Q14;
306     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
307
308     /* Save quantized speech signal */
309     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
310     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
311     RESTORE_STACK;
312 }
313
314 /******************************************/
315 /* Noise shape quantizer for one subframe */
316 /******************************************/
317 #ifndef OVERRIDE_silk_noise_shape_quantizer_del_dec
318 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
319     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
320     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
321     opus_int            signalType,             /* I    Signal type                         */
322     const opus_int32    x_Q10[],                /* I                                        */
323     opus_int8           pulses[],               /* O                                        */
324     opus_int16          xq[],                   /* O                                        */
325     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
326     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
327     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
328     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
329     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
330     opus_int            lag,                    /* I    Pitch lag                           */
331     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
332     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
333     opus_int32          LF_shp_Q14,             /* I                                        */
334     opus_int32          Gain_Q16,               /* I                                        */
335     opus_int            Lambda_Q10,             /* I                                        */
336     opus_int            offset_Q10,             /* I                                        */
337     opus_int            length,                 /* I    Input length                        */
338     opus_int            subfr,                  /* I    Subframe number                     */
339     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
340     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
341     opus_int            warping_Q16,            /* I                                        */
342     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
343     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
344     opus_int            decisionDelay,          /* I                                        */
345     int                 arch                    /* I                                        */
346 )
347 {
348     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
349     opus_int32   Winner_rand_state;
350     opus_int32   LTP_pred_Q14, LPC_pred_Q14, n_AR_Q14, n_LTP_Q14;
351     opus_int32   n_LF_Q14, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
352     opus_int32   q1_Q0, q1_Q10, q2_Q10, exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
353     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
354     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
355 #ifdef silk_short_prediction_create_arch_coef
356     opus_int32   a_Q12_arch[MAX_LPC_ORDER];
357 #endif
358
359     VARDECL( NSQ_sample_pair, psSampleState );
360     NSQ_del_dec_struct *psDD;
361     NSQ_sample_struct  *psSS;
362     SAVE_STACK;
363
364     silk_assert( nStatesDelayedDecision > 0 );
365     ALLOC( psSampleState, nStatesDelayedDecision, NSQ_sample_pair );
366
367     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
368     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
369     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
370
371 #ifdef silk_short_prediction_create_arch_coef
372     silk_short_prediction_create_arch_coef(a_Q12_arch, a_Q12, predictLPCOrder);
373 #endif
374
375     for( i = 0; i < length; i++ ) {
376         /* Perform common calculations used in all states */
377
378         /* Long-term prediction */
379         if( signalType == TYPE_VOICED ) {
380             /* Unrolled loop */
381             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
382             LTP_pred_Q14 = 2;
383             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
384             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
385             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
386             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
387             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
388             LTP_pred_Q14 = silk_LSHIFT( LTP_pred_Q14, 1 );                          /* Q13 -> Q14 */
389             pred_lag_ptr++;
390         } else {
391             LTP_pred_Q14 = 0;
392         }
393
394         /* Long-term shaping */
395         if( lag > 0 ) {
396             /* Symmetric, packed FIR coefficients */
397             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
398             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
399             n_LTP_Q14 = silk_SUB_LSHIFT32( LTP_pred_Q14, n_LTP_Q14, 2 );            /* Q12 -> Q14 */
400             shp_lag_ptr++;
401         } else {
402             n_LTP_Q14 = 0;
403         }
404
405         for( k = 0; k < nStatesDelayedDecision; k++ ) {
406             /* Delayed decision state */
407             psDD = &psDelDec[ k ];
408
409             /* Sample state */
410             psSS = psSampleState[ k ];
411
412             /* Generate dither */
413             psDD->Seed = silk_RAND( psDD->Seed );
414
415             /* Pointer used in short term prediction and shaping */
416             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
417             /* Short-term prediction */
418             LPC_pred_Q14 = silk_noise_shape_quantizer_short_prediction(psLPC_Q14, a_Q12, a_Q12_arch, predictLPCOrder, arch);
419             LPC_pred_Q14 = silk_LSHIFT( LPC_pred_Q14, 4 );                              /* Q10 -> Q14 */
420
421             /* Noise shape feedback */
422             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
423             /* Output of lowpass section */
424             tmp2 = silk_SMLAWB( psDD->Diff_Q14, psDD->sAR2_Q14[ 0 ], warping_Q16 );
425             /* Output of allpass section */
426             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
427             psDD->sAR2_Q14[ 0 ] = tmp2;
428             n_AR_Q14 = silk_RSHIFT( shapingLPCOrder, 1 );
429             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ 0 ] );
430             /* Loop over allpass sections */
431             for( j = 2; j < shapingLPCOrder; j += 2 ) {
432                 /* Output of allpass section */
433                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
434                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
435                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ j - 1 ] );
436                 /* Output of allpass section */
437                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
438                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
439                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ j ] );
440             }
441             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
442             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
443
444             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 1 );                                      /* Q11 -> Q12 */
445             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, psDD->LF_AR_Q14, Tilt_Q14 );              /* Q12 */
446             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 2 );                                      /* Q12 -> Q14 */
447
448             n_LF_Q14 = silk_SMULWB( psDD->Shape_Q14[ *smpl_buf_idx ], LF_shp_Q14 );     /* Q12 */
449             n_LF_Q14 = silk_SMLAWT( n_LF_Q14, psDD->LF_AR_Q14, LF_shp_Q14 );            /* Q12 */
450             n_LF_Q14 = silk_LSHIFT( n_LF_Q14, 2 );                                      /* Q12 -> Q14 */
451
452             /* Input minus prediction plus noise feedback                       */
453             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
454             tmp1 = silk_ADD32( n_AR_Q14, n_LF_Q14 );                                    /* Q14 */
455             tmp2 = silk_ADD32( n_LTP_Q14, LPC_pred_Q14 );                               /* Q13 */
456             tmp1 = silk_SUB32( tmp2, tmp1 );                                            /* Q13 */
457             tmp1 = silk_RSHIFT_ROUND( tmp1, 4 );                                        /* Q10 */
458
459             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                                     /* residual error Q10 */
460
461             /* Flip sign depending on dither */
462             if ( psDD->Seed < 0 ) {
463                 r_Q10 = -r_Q10;
464             }
465             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
466
467             /* Find two quantization level candidates and measure their rate-distortion */
468             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
469             q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
470             if (Lambda_Q10 > 2048) {
471                 /* For aggressive RDO, the bias becomes more than one pulse. */
472                 int rdo_offset = Lambda_Q10/2 - 512;
473                 if (q1_Q10 > rdo_offset) {
474                     q1_Q0 = silk_RSHIFT( q1_Q10 - rdo_offset, 10 );
475                 } else if (q1_Q10 < -rdo_offset) {
476                     q1_Q0 = silk_RSHIFT( q1_Q10 + rdo_offset, 10 );
477                 } else if (q1_Q10 < 0) {
478                     q1_Q0 = -1;
479                 } else {
480                     q1_Q0 = 0;
481                 }
482             }
483             if( q1_Q0 > 0 ) {
484                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
485                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
486                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
487                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
488                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
489             } else if( q1_Q0 == 0 ) {
490                 q1_Q10  = offset_Q10;
491                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
492                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
493                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
494             } else if( q1_Q0 == -1 ) {
495                 q2_Q10  = offset_Q10;
496                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
497                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
498                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
499             } else {            /* q1_Q0 < -1 */
500                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
501                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
502                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
503                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
504                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
505             }
506             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
507             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
508             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
509             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
510
511             if( rd1_Q10 < rd2_Q10 ) {
512                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
513                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
514                 psSS[ 0 ].Q_Q10  = q1_Q10;
515                 psSS[ 1 ].Q_Q10  = q2_Q10;
516             } else {
517                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
518                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
519                 psSS[ 0 ].Q_Q10  = q2_Q10;
520                 psSS[ 1 ].Q_Q10  = q1_Q10;
521             }
522
523             /* Update states for best quantization */
524
525             /* Quantized excitation */
526             exc_Q14 = silk_LSHIFT32( psSS[ 0 ].Q_Q10, 4 );
527             if ( psDD->Seed < 0 ) {
528                 exc_Q14 = -exc_Q14;
529             }
530
531             /* Add predictions */
532             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
533             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
534
535             /* Update states */
536             psSS[ 0 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
537             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 0 ].Diff_Q14, n_AR_Q14 );
538             psSS[ 0 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
539             psSS[ 0 ].LF_AR_Q14    = sLF_AR_shp_Q14;
540             psSS[ 0 ].LPC_exc_Q14  = LPC_exc_Q14;
541             psSS[ 0 ].xq_Q14       = xq_Q14;
542
543             /* Update states for second best quantization */
544
545             /* Quantized excitation */
546             exc_Q14 = silk_LSHIFT32( psSS[ 1 ].Q_Q10, 4 );
547             if ( psDD->Seed < 0 ) {
548                 exc_Q14 = -exc_Q14;
549             }
550
551             /* Add predictions */
552             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
553             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
554
555             /* Update states */
556             psSS[ 1 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
557             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 1 ].Diff_Q14, n_AR_Q14 );
558             psSS[ 1 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
559             psSS[ 1 ].LF_AR_Q14    = sLF_AR_shp_Q14;
560             psSS[ 1 ].LPC_exc_Q14  = LPC_exc_Q14;
561             psSS[ 1 ].xq_Q14       = xq_Q14;
562         }
563
564         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) % DECISION_DELAY;
565         if( *smpl_buf_idx < 0 ) *smpl_buf_idx += DECISION_DELAY;
566         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) % DECISION_DELAY;
567
568         /* Find winner */
569         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
570         Winner_ind = 0;
571         for( k = 1; k < nStatesDelayedDecision; k++ ) {
572             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
573                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
574                 Winner_ind = k;
575             }
576         }
577
578         /* Increase RD values of expired states */
579         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
580         for( k = 0; k < nStatesDelayedDecision; k++ ) {
581             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
582                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, silk_int32_MAX >> 4 );
583                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, silk_int32_MAX >> 4 );
584                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
585             }
586         }
587
588         /* Find worst in first set and best in second set */
589         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
590         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
591         RDmax_ind = 0;
592         RDmin_ind = 0;
593         for( k = 1; k < nStatesDelayedDecision; k++ ) {
594             /* find worst in first set */
595             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
596                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
597                 RDmax_ind = k;
598             }
599             /* find best in second set */
600             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
601                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
602                 RDmin_ind = k;
603             }
604         }
605
606         /* Replace a state if best from second set outperforms worst in first set */
607         if( RDmin_Q10 < RDmax_Q10 ) {
608             silk_memcpy( ( (opus_int32 *)&psDelDec[ RDmax_ind ] ) + i,
609                          ( (opus_int32 *)&psDelDec[ RDmin_ind ] ) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
610             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
611         }
612
613         /* Write samples from winner to output and long-term filter states */
614         psDD = &psDelDec[ Winner_ind ];
615         if( subfr > 0 || i >= decisionDelay ) {
616             pulses[  i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
617             xq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
618                 silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], delayedGain_Q10[ last_smple_idx ] ), 8 ) );
619             NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q14[ last_smple_idx ];
620             sLTP_Q15[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q15[  last_smple_idx ];
621         }
622         NSQ->sLTP_shp_buf_idx++;
623         NSQ->sLTP_buf_idx++;
624
625         /* Update states */
626         for( k = 0; k < nStatesDelayedDecision; k++ ) {
627             psDD                                     = &psDelDec[ k ];
628             psSS                                     = &psSampleState[ k ][ 0 ];
629             psDD->LF_AR_Q14                          = psSS->LF_AR_Q14;
630             psDD->Diff_Q14                           = psSS->Diff_Q14;
631             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
632             psDD->Xq_Q14[    *smpl_buf_idx ]         = psSS->xq_Q14;
633             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
634             psDD->Pred_Q15[  *smpl_buf_idx ]         = silk_LSHIFT32( psSS->LPC_exc_Q14, 1 );
635             psDD->Shape_Q14[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q14;
636             psDD->Seed                               = silk_ADD32_ovflw( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
637             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
638             psDD->RD_Q10                             = psSS->RD_Q10;
639         }
640         delayedGain_Q10[     *smpl_buf_idx ]         = Gain_Q10;
641     }
642     /* Update LPC states */
643     for( k = 0; k < nStatesDelayedDecision; k++ ) {
644         psDD = &psDelDec[ k ];
645         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
646     }
647     RESTORE_STACK;
648 }
649 #endif /* OVERRIDE_silk_noise_shape_quantizer_del_dec */
650
651 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
652     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
653     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
654     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
655     const opus_int16    x16[],                      /* I    Input                               */
656     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
657     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
658     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
659     opus_int            subfr,                      /* I    Subframe number                     */
660     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
661     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
662     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
663     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
664     const opus_int      signal_type,                /* I    Signal type                         */
665     const opus_int      decisionDelay               /* I    Decision delay                      */
666 )
667 {
668     opus_int            i, k, lag;
669     opus_int32          gain_adj_Q16, inv_gain_Q31, inv_gain_Q26;
670     NSQ_del_dec_struct  *psDD;
671
672     lag          = pitchL[ subfr ];
673     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
674     silk_assert( inv_gain_Q31 != 0 );
675
676     /* Scale input */
677     inv_gain_Q26 = silk_RSHIFT_ROUND( inv_gain_Q31, 5 );
678     for( i = 0; i < psEncC->subfr_length; i++ ) {
679         x_sc_Q10[ i ] = silk_SMULWW( x16[ i ], inv_gain_Q26 );
680     }
681
682     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
683     if( NSQ->rewhite_flag ) {
684         if( subfr == 0 ) {
685             /* Do LTP downscaling */
686             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
687         }
688         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
689             silk_assert( i < MAX_FRAME_LENGTH );
690             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
691         }
692     }
693
694     /* Adjust for changing gain */
695     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
696         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
697
698         /* Scale long-term shaping state */
699         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
700             NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
701         }
702
703         /* Scale long-term prediction state */
704         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
705             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
706                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
707             }
708         }
709
710         for( k = 0; k < nStatesDelayedDecision; k++ ) {
711             psDD = &psDelDec[ k ];
712
713             /* Scale scalar states */
714             psDD->LF_AR_Q14 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q14 );
715             psDD->Diff_Q14 = silk_SMULWW( gain_adj_Q16, psDD->Diff_Q14 );
716
717             /* Scale short-term prediction and shaping states */
718             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
719                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
720             }
721             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
722                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
723             }
724             for( i = 0; i < DECISION_DELAY; i++ ) {
725                 psDD->Pred_Q15[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q15[  i ] );
726                 psDD->Shape_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q14[ i ] );
727             }
728         }
729
730         /* Save inverse gain */
731         NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
732     }
733 }