Speeding up PVQ using unlikely() and moving first position out of the loop
[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     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[psEncC->ltp_mem_length], psEncC->frame_length * sizeof( opus_int16 ) ) */
310     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
311     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
312     RESTORE_STACK;
313 }
314
315 /******************************************/
316 /* Noise shape quantizer for one subframe */
317 /******************************************/
318 #ifndef OVERRIDE_silk_noise_shape_quantizer_del_dec
319 static OPUS_INLINE void silk_noise_shape_quantizer_del_dec(
320     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
321     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
322     opus_int            signalType,             /* I    Signal type                         */
323     const opus_int32    x_Q10[],                /* I                                        */
324     opus_int8           pulses[],               /* O                                        */
325     opus_int16          xq[],                   /* O                                        */
326     opus_int32          sLTP_Q15[],             /* I/O  LTP filter state                    */
327     opus_int32          delayedGain_Q10[],      /* I/O  Gain delay buffer                   */
328     const opus_int16    a_Q12[],                /* I    Short term prediction coefs         */
329     const opus_int16    b_Q14[],                /* I    Long term prediction coefs          */
330     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping coefs                 */
331     opus_int            lag,                    /* I    Pitch lag                           */
332     opus_int32          HarmShapeFIRPacked_Q14, /* I                                        */
333     opus_int            Tilt_Q14,               /* I    Spectral tilt                       */
334     opus_int32          LF_shp_Q14,             /* I                                        */
335     opus_int32          Gain_Q16,               /* I                                        */
336     opus_int            Lambda_Q10,             /* I                                        */
337     opus_int            offset_Q10,             /* I                                        */
338     opus_int            length,                 /* I    Input length                        */
339     opus_int            subfr,                  /* I    Subframe number                     */
340     opus_int            shapingLPCOrder,        /* I    Shaping LPC filter order            */
341     opus_int            predictLPCOrder,        /* I    Prediction filter order             */
342     opus_int            warping_Q16,            /* I                                        */
343     opus_int            nStatesDelayedDecision, /* I    Number of states in decision tree   */
344     opus_int            *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
345     opus_int            decisionDelay,          /* I                                        */
346     int                 arch                    /* I                                        */
347 )
348 {
349     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
350     opus_int32   Winner_rand_state;
351     opus_int32   LTP_pred_Q14, LPC_pred_Q14, n_AR_Q14, n_LTP_Q14;
352     opus_int32   n_LF_Q14, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
353     opus_int32   q1_Q0, q1_Q10, q2_Q10, exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
354     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
355     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
356 #ifdef silk_short_prediction_create_arch_coef
357     opus_int32   a_Q12_arch[MAX_LPC_ORDER];
358 #endif
359
360     VARDECL( NSQ_sample_pair, psSampleState );
361     NSQ_del_dec_struct *psDD;
362     NSQ_sample_struct  *psSS;
363     SAVE_STACK;
364
365     silk_assert( nStatesDelayedDecision > 0 );
366     ALLOC( psSampleState, nStatesDelayedDecision, NSQ_sample_pair );
367
368     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
369     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
370     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
371
372 #ifdef silk_short_prediction_create_arch_coef
373     silk_short_prediction_create_arch_coef(a_Q12_arch, a_Q12, predictLPCOrder);
374 #endif
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             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
385             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
386             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
387             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
388             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
389             LTP_pred_Q14 = silk_LSHIFT( LTP_pred_Q14, 1 );                          /* Q13 -> Q14 */
390             pred_lag_ptr++;
391         } else {
392             LTP_pred_Q14 = 0;
393         }
394
395         /* Long-term shaping */
396         if( lag > 0 ) {
397             /* Symmetric, packed FIR coefficients */
398             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
399             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
400             n_LTP_Q14 = silk_SUB_LSHIFT32( LTP_pred_Q14, n_LTP_Q14, 2 );            /* Q12 -> Q14 */
401             shp_lag_ptr++;
402         } else {
403             n_LTP_Q14 = 0;
404         }
405
406         for( k = 0; k < nStatesDelayedDecision; k++ ) {
407             /* Delayed decision state */
408             psDD = &psDelDec[ k ];
409
410             /* Sample state */
411             psSS = psSampleState[ k ];
412
413             /* Generate dither */
414             psDD->Seed = silk_RAND( psDD->Seed );
415
416             /* Pointer used in short term prediction and shaping */
417             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
418             /* Short-term prediction */
419             LPC_pred_Q14 = silk_noise_shape_quantizer_short_prediction(psLPC_Q14, a_Q12, a_Q12_arch, predictLPCOrder, arch);
420             LPC_pred_Q14 = silk_LSHIFT( LPC_pred_Q14, 4 );                              /* Q10 -> Q14 */
421
422             /* Noise shape feedback */
423             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
424             /* Output of lowpass section */
425             tmp2 = silk_SMLAWB( psDD->Diff_Q14, psDD->sAR2_Q14[ 0 ], warping_Q16 );
426             /* Output of allpass section */
427             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
428             psDD->sAR2_Q14[ 0 ] = tmp2;
429             n_AR_Q14 = silk_RSHIFT( shapingLPCOrder, 1 );
430             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ 0 ] );
431             /* Loop over allpass sections */
432             for( j = 2; j < shapingLPCOrder; j += 2 ) {
433                 /* Output of allpass section */
434                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
435                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
436                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ j - 1 ] );
437                 /* Output of allpass section */
438                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
439                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
440                 n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp2, AR_shp_Q13[ j ] );
441             }
442             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
443             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
444
445             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 1 );                                      /* Q11 -> Q12 */
446             n_AR_Q14 = silk_SMLAWB( n_AR_Q14, psDD->LF_AR_Q14, Tilt_Q14 );              /* Q12 */
447             n_AR_Q14 = silk_LSHIFT( n_AR_Q14, 2 );                                      /* Q12 -> Q14 */
448
449             n_LF_Q14 = silk_SMULWB( psDD->Shape_Q14[ *smpl_buf_idx ], LF_shp_Q14 );     /* Q12 */
450             n_LF_Q14 = silk_SMLAWT( n_LF_Q14, psDD->LF_AR_Q14, LF_shp_Q14 );            /* Q12 */
451             n_LF_Q14 = silk_LSHIFT( n_LF_Q14, 2 );                                      /* Q12 -> Q14 */
452
453             /* Input minus prediction plus noise feedback                       */
454             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
455             tmp1 = silk_ADD32( n_AR_Q14, n_LF_Q14 );                                    /* Q14 */
456             tmp2 = silk_ADD32( n_LTP_Q14, LPC_pred_Q14 );                               /* Q13 */
457             tmp1 = silk_SUB32( tmp2, tmp1 );                                            /* Q13 */
458             tmp1 = silk_RSHIFT_ROUND( tmp1, 4 );                                        /* Q10 */
459
460             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                                     /* residual error Q10 */
461
462             /* Flip sign depending on dither */
463             if ( psDD->Seed < 0 ) {
464                 r_Q10 = -r_Q10;
465             }
466             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
467
468             /* Find two quantization level candidates and measure their rate-distortion */
469             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
470             q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
471             if (Lambda_Q10 > 2048) {
472                 /* For aggressive RDO, the bias becomes more than one pulse. */
473                 int rdo_offset = Lambda_Q10/2 - 512;
474                 if (q1_Q10 > rdo_offset) {
475                     q1_Q0 = silk_RSHIFT( q1_Q10 - rdo_offset, 10 );
476                 } else if (q1_Q10 < -rdo_offset) {
477                     q1_Q0 = silk_RSHIFT( q1_Q10 + rdo_offset, 10 );
478                 } else if (q1_Q10 < 0) {
479                     q1_Q0 = -1;
480                 } else {
481                     q1_Q0 = 0;
482                 }
483             }
484             if( q1_Q0 > 0 ) {
485                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
486                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
487                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
488                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
489                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
490             } else if( q1_Q0 == 0 ) {
491                 q1_Q10  = offset_Q10;
492                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
493                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
494                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
495             } else if( q1_Q0 == -1 ) {
496                 q2_Q10  = offset_Q10;
497                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
498                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
499                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
500             } else {            /* q1_Q0 < -1 */
501                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
502                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
503                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
504                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
505                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
506             }
507             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
508             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
509             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
510             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
511
512             if( rd1_Q10 < rd2_Q10 ) {
513                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
514                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
515                 psSS[ 0 ].Q_Q10  = q1_Q10;
516                 psSS[ 1 ].Q_Q10  = q2_Q10;
517             } else {
518                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
519                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
520                 psSS[ 0 ].Q_Q10  = q2_Q10;
521                 psSS[ 1 ].Q_Q10  = q1_Q10;
522             }
523
524             /* Update states for best quantization */
525
526             /* Quantized excitation */
527             exc_Q14 = silk_LSHIFT32( psSS[ 0 ].Q_Q10, 4 );
528             if ( psDD->Seed < 0 ) {
529                 exc_Q14 = -exc_Q14;
530             }
531
532             /* Add predictions */
533             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
534             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
535
536             /* Update states */
537             psSS[ 0 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
538             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 0 ].Diff_Q14, n_AR_Q14 );
539             psSS[ 0 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
540             psSS[ 0 ].LF_AR_Q14    = sLF_AR_shp_Q14;
541             psSS[ 0 ].LPC_exc_Q14  = LPC_exc_Q14;
542             psSS[ 0 ].xq_Q14       = xq_Q14;
543
544             /* Update states for second best quantization */
545
546             /* Quantized excitation */
547             exc_Q14 = silk_LSHIFT32( psSS[ 1 ].Q_Q10, 4 );
548             if ( psDD->Seed < 0 ) {
549                 exc_Q14 = -exc_Q14;
550             }
551
552             /* Add predictions */
553             LPC_exc_Q14 = silk_ADD32( exc_Q14, LTP_pred_Q14 );
554             xq_Q14      = silk_ADD32( LPC_exc_Q14, LPC_pred_Q14 );
555
556             /* Update states */
557             psSS[ 1 ].Diff_Q14     = silk_SUB_LSHIFT32( xq_Q14, x_Q10[ i ], 4 );
558             sLF_AR_shp_Q14         = silk_SUB32( psSS[ 1 ].Diff_Q14, n_AR_Q14 );
559             psSS[ 1 ].sLTP_shp_Q14 = silk_SUB32( sLF_AR_shp_Q14, n_LF_Q14 );
560             psSS[ 1 ].LF_AR_Q14    = sLF_AR_shp_Q14;
561             psSS[ 1 ].LPC_exc_Q14  = LPC_exc_Q14;
562             psSS[ 1 ].xq_Q14       = xq_Q14;
563         }
564
565         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) % DECISION_DELAY;
566         if( *smpl_buf_idx < 0 ) *smpl_buf_idx += DECISION_DELAY;
567         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) % DECISION_DELAY;
568
569         /* Find winner */
570         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
571         Winner_ind = 0;
572         for( k = 1; k < nStatesDelayedDecision; k++ ) {
573             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
574                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
575                 Winner_ind = k;
576             }
577         }
578
579         /* Increase RD values of expired states */
580         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
581         for( k = 0; k < nStatesDelayedDecision; k++ ) {
582             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
583                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, silk_int32_MAX >> 4 );
584                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, silk_int32_MAX >> 4 );
585                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
586             }
587         }
588
589         /* Find worst in first set and best in second set */
590         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
591         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
592         RDmax_ind = 0;
593         RDmin_ind = 0;
594         for( k = 1; k < nStatesDelayedDecision; k++ ) {
595             /* find worst in first set */
596             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
597                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
598                 RDmax_ind = k;
599             }
600             /* find best in second set */
601             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
602                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
603                 RDmin_ind = k;
604             }
605         }
606
607         /* Replace a state if best from second set outperforms worst in first set */
608         if( RDmin_Q10 < RDmax_Q10 ) {
609             silk_memcpy( ( (opus_int32 *)&psDelDec[ RDmax_ind ] ) + i,
610                          ( (opus_int32 *)&psDelDec[ RDmin_ind ] ) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
611             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
612         }
613
614         /* Write samples from winner to output and long-term filter states */
615         psDD = &psDelDec[ Winner_ind ];
616         if( subfr > 0 || i >= decisionDelay ) {
617             pulses[  i - decisionDelay ] = (opus_int8)silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
618             xq[ i - decisionDelay ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND(
619                 silk_SMULWW( psDD->Xq_Q14[ last_smple_idx ], delayedGain_Q10[ last_smple_idx ] ), 8 ) );
620             NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q14[ last_smple_idx ];
621             sLTP_Q15[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q15[  last_smple_idx ];
622         }
623         NSQ->sLTP_shp_buf_idx++;
624         NSQ->sLTP_buf_idx++;
625
626         /* Update states */
627         for( k = 0; k < nStatesDelayedDecision; k++ ) {
628             psDD                                     = &psDelDec[ k ];
629             psSS                                     = &psSampleState[ k ][ 0 ];
630             psDD->LF_AR_Q14                          = psSS->LF_AR_Q14;
631             psDD->Diff_Q14                           = psSS->Diff_Q14;
632             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
633             psDD->Xq_Q14[    *smpl_buf_idx ]         = psSS->xq_Q14;
634             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
635             psDD->Pred_Q15[  *smpl_buf_idx ]         = silk_LSHIFT32( psSS->LPC_exc_Q14, 1 );
636             psDD->Shape_Q14[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q14;
637             psDD->Seed                               = silk_ADD32_ovflw( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
638             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
639             psDD->RD_Q10                             = psSS->RD_Q10;
640         }
641         delayedGain_Q10[     *smpl_buf_idx ]         = Gain_Q10;
642     }
643     /* Update LPC states */
644     for( k = 0; k < nStatesDelayedDecision; k++ ) {
645         psDD = &psDelDec[ k ];
646         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
647     }
648     RESTORE_STACK;
649 }
650 #endif /* OVERRIDE_silk_noise_shape_quantizer_del_dec */
651
652 static OPUS_INLINE void silk_nsq_del_dec_scale_states(
653     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
654     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
655     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
656     const opus_int16    x16[],                      /* I    Input                               */
657     opus_int32          x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
658     const opus_int16    sLTP[],                     /* I    Re-whitened LTP state in Q0         */
659     opus_int32          sLTP_Q15[],                 /* O    LTP state matching scaled input     */
660     opus_int            subfr,                      /* I    Subframe number                     */
661     opus_int            nStatesDelayedDecision,     /* I    Number of del dec states            */
662     const opus_int      LTP_scale_Q14,              /* I    LTP state scaling                   */
663     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
664     const opus_int      pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
665     const opus_int      signal_type,                /* I    Signal type                         */
666     const opus_int      decisionDelay               /* I    Decision delay                      */
667 )
668 {
669     opus_int            i, k, lag;
670     opus_int32          gain_adj_Q16, inv_gain_Q31, inv_gain_Q26;
671     NSQ_del_dec_struct  *psDD;
672
673     lag          = pitchL[ subfr ];
674     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
675     silk_assert( inv_gain_Q31 != 0 );
676
677     /* Scale input */
678     inv_gain_Q26 = silk_RSHIFT_ROUND( inv_gain_Q31, 5 );
679     for( i = 0; i < psEncC->subfr_length; i++ ) {
680         x_sc_Q10[ i ] = silk_SMULWW( x16[ i ], inv_gain_Q26 );
681     }
682
683     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
684     if( NSQ->rewhite_flag ) {
685         if( subfr == 0 ) {
686             /* Do LTP downscaling */
687             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
688         }
689         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
690             silk_assert( i < MAX_FRAME_LENGTH );
691             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
692         }
693     }
694
695     /* Adjust for changing gain */
696     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
697         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
698
699         /* Scale long-term shaping state */
700         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
701             NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
702         }
703
704         /* Scale long-term prediction state */
705         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
706             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
707                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
708             }
709         }
710
711         for( k = 0; k < nStatesDelayedDecision; k++ ) {
712             psDD = &psDelDec[ k ];
713
714             /* Scale scalar states */
715             psDD->LF_AR_Q14 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q14 );
716             psDD->Diff_Q14 = silk_SMULWW( gain_adj_Q16, psDD->Diff_Q14 );
717
718             /* Scale short-term prediction and shaping states */
719             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
720                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
721             }
722             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
723                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
724             }
725             for( i = 0; i < DECISION_DELAY; i++ ) {
726                 psDD->Pred_Q15[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q15[  i ] );
727                 psDD->Shape_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q14[ i ] );
728             }
729         }
730
731         /* Save inverse gain */
732         NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
733     }
734 }