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