Cleaner way to take into account the prediction for stereo width
[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, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions 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 Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "main.h"
33
34 typedef struct {
35     opus_int32 sLPC_Q14[ MAX_FRAME_LENGTH / MAX_NB_SUBFR + NSQ_LPC_BUF_LENGTH ];
36     opus_int32 RandState[ DECISION_DELAY ];
37     opus_int32 Q_Q10[     DECISION_DELAY ];
38     opus_int32 Xq_Q10[    DECISION_DELAY ];
39     opus_int32 Pred_Q16[  DECISION_DELAY ];
40     opus_int32 Shape_Q10[ DECISION_DELAY ];
41     opus_int32 sAR2_Q14[ MAX_SHAPE_LPC_ORDER ];
42     opus_int32 LF_AR_Q12;
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_Q12;
53     opus_int32 sLTP_shp_Q10;
54     opus_int32 LPC_exc_Q16;
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_int16     x[],                        /* I    Input in Q0                         */
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_Q16[],                 /* 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_Q16[],             /* I/O  LTP filter state                    */
85     opus_int32           delayedGain_Q16[],      /* 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_int16                 x[],                                        /* I    Prefiltered input signal            */
111     opus_int8                        pulses[],                                   /* O    Quantized pulse signal              */
112     const opus_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Prediction coefs                    */
113     const opus_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    LT prediction coefs                 */
114     const opus_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I                                     */
115     const opus_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I                                        */
116     const opus_int                   Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                       */
117     const opus_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I                                        */
118     const opus_int32                 Gains_Q16[ MAX_NB_SUBFR ],                  /* I                                        */
119     const opus_int                   pitchL[ MAX_NB_SUBFR ],                     /* I                                        */
120     const opus_int                   Lambda_Q10,                                 /* I                                        */
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_Q16[ 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;
133     opus_int32   x_sc_Q10[ MAX_SUB_FRAME_LENGTH ];
134     opus_int32   delayedGain_Q16[  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_inv_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_Q12      = NSQ->sLF_AR_shp_Q12;
151         psDD->Shape_Q10[ 0 ] = NSQ->sLTP_shp_Q10[ 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     /* Setup 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_Q10[ last_smple_idx ], Gains_Q16[ 1 ] ), 10 ) );
226                         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ 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, x_sc_Q10, sLTP, sLTP_Q16, 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_Q16,
248             delayedGain_Q16, 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      += 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     for( i = 0; i < decisionDelay; i++ ) {
272         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
273         pulses[   i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
274         pxq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
275             silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], Gains_Q16[ psEncC->nb_subfr - 1 ] ), 10 ) );
276         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ last_smple_idx ];
277     }
278     silk_memcpy( NSQ->sLPC_Q14, &psDD->sLPC_Q14[ psEncC->subfr_length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
279     silk_memcpy( NSQ->sAR2_Q14, psDD->sAR2_Q14, sizeof( psDD->sAR2_Q14 ) );
280
281     /* Update states */
282     NSQ->sLF_AR_shp_Q12 = psDD->LF_AR_Q12;
283     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
284
285     /* Save quantized speech and noise shaping signals */
286     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
287     silk_memmove( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
288
289 #ifdef SAVE_ALL_INTERNAL_DATA
290     DEBUG_STORE_DATA( xq.dat,       &pxq[ -psEncC->frame_length ],       psEncC->frame_length * sizeof( opus_int16 ) );
291     DEBUG_STORE_DATA( q.dat,        &pulses[ -psEncC->frame_length ],    psEncC->frame_length * sizeof( opus_int8 ) );
292 #endif
293 }
294
295 /******************************************/
296 /* Noise shape quantizer for one subframe */
297 /******************************************/
298 static inline void silk_noise_shape_quantizer_del_dec(
299     silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */
300     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
301     opus_int             signalType,             /* I    Signal type                         */
302     const opus_int32     x_Q10[],                /* I                                        */
303     opus_int8            pulses[],               /* O                                        */
304     opus_int16           xq[],                   /* O                                        */
305     opus_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */
306     opus_int32           delayedGain_Q16[],      /* I/O  Gain delay buffer                   */
307     const opus_int16     a_Q12[],                /* I    Short term prediction coefs         */
308     const opus_int16     b_Q14[],                /* I    Long term prediction coefs          */
309     const opus_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */
310     opus_int             lag,                    /* I    Pitch lag                           */
311     opus_int32           HarmShapeFIRPacked_Q14, /* I                                        */
312     opus_int             Tilt_Q14,               /* I    Spectral tilt                       */
313     opus_int32           LF_shp_Q14,             /* I                                        */
314     opus_int32           Gain_Q16,               /* I                                        */
315     opus_int             Lambda_Q10,             /* I                                        */
316     opus_int             offset_Q10,             /* I                                        */
317     opus_int             length,                 /* I    Input length                        */
318     opus_int             subfr,                  /* I    Subframe number                     */
319     opus_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */
320     opus_int             predictLPCOrder,        /* I    Prediction filter order             */
321     opus_int             warping_Q16,            /* I                                        */
322     opus_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */
323     opus_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
324     opus_int             decisionDelay           /* I                                        */
325 )
326 {
327     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
328     opus_int32   Winner_rand_state;
329     opus_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14, LTP_Q10;
330     opus_int32   n_LF_Q10, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
331     opus_int32   q1_Q10, q2_Q10, dither, exc_Q10, LPC_exc_Q10, xq_Q10;
332     opus_int32   tmp1, tmp2, sLF_AR_shp_Q10;
333     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
334     NSQ_sample_struct  psSampleState[ MAX_DEL_DEC_STATES ][ 2 ];
335     NSQ_del_dec_struct *psDD;
336     NSQ_sample_struct  *psSS;
337
338     silk_assert( nStatesDelayedDecision > 0 );
339
340     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
341     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
342
343     for( i = 0; i < length; i++ ) {
344         /* Perform common calculations used in all states */
345
346         /* Long-term prediction */
347         if( signalType == TYPE_VOICED ) {
348             /* Unrolled loop */
349             LTP_pred_Q14 = silk_SMULWB(               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             pred_lag_ptr++;
355         } else {
356             LTP_pred_Q14 = 0;
357         }
358
359         /* Long-term shaping */
360         if( lag > 0 ) {
361             /* Symmetric, packed FIR coefficients */
362             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
363             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );
364             n_LTP_Q14 = silk_LSHIFT( n_LTP_Q14, 6 );
365             shp_lag_ptr++;
366
367             LTP_Q10 = silk_RSHIFT( silk_SUB32( LTP_pred_Q14, n_LTP_Q14 ), 4 );
368         } else {
369             LTP_Q10 = 0;
370         }
371
372         for( k = 0; k < nStatesDelayedDecision; k++ ) {
373             /* Delayed decision state */
374             psDD = &psDelDec[ k ];
375
376             /* Sample state */
377             psSS = psSampleState[ k ];
378
379             /* Generate dither */
380             psDD->Seed = silk_RAND( psDD->Seed );
381
382             /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */
383             dither = silk_RSHIFT( psDD->Seed, 31 );
384
385             /* Pointer used in short term prediction and shaping */
386             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
387             /* Short-term prediction */
388             silk_assert( predictLPCOrder >= 10 );            /* check that unrolling works */
389             silk_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */
390             silk_assert( ( (opus_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */
391             /* Partially unrolled */
392             LPC_pred_Q10 = silk_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );
393             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
394             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
395             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
396             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
397             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
398             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
399             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
400             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
401             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
402             for( j = 10; j < predictLPCOrder; j ++ ) {
403                 LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );
404             }
405
406             /* Noise shape feedback */
407             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
408             /* Output of lowpass section */
409             tmp2 = silk_SMLAWB( psLPC_Q14[ 0 ], psDD->sAR2_Q14[ 0 ], warping_Q16 );
410             /* Output of allpass section */
411             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
412             psDD->sAR2_Q14[ 0 ] = tmp2;
413             n_AR_Q10 = silk_SMULWB( tmp2, AR_shp_Q13[ 0 ] );
414             /* Loop over allpass sections */
415             for( j = 2; j < shapingLPCOrder; j += 2 ) {
416                 /* Output of allpass section */
417                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
418                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
419                 n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );
420                 /* Output of allpass section */
421                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
422                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
423                 n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );
424             }
425             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
426             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
427
428             n_AR_Q10 = silk_RSHIFT( n_AR_Q10, 1 );           /* Q11 -> Q10 */
429             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, psDD->LF_AR_Q12, Tilt_Q14 );
430
431             n_LF_Q10 = silk_LSHIFT( silk_SMULWB( psDD->Shape_Q10[ *smpl_buf_idx ], LF_shp_Q14 ), 2 );
432             n_LF_Q10 = silk_SMLAWT( n_LF_Q10, psDD->LF_AR_Q12, LF_shp_Q14 );
433
434             /* Input minus prediction plus noise feedback                       */
435             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
436             tmp1  = silk_ADD32( LTP_Q10, LPC_pred_Q10 );                         /* add Q10 stuff */
437             tmp1  = silk_SUB32( tmp1, n_AR_Q10 );                                /* subtract Q10 stuff */
438             tmp1  = silk_SUB32( tmp1, n_LF_Q10 );                                /* subtract Q10 stuff */
439             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                              /* residual error Q10 */
440
441             /* Flip sign depending on dither */
442             r_Q10 = r_Q10 ^ dither;
443             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
444
445             /* Find two quantization level candidates and measure their rate-distortion */
446             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
447             q1_Q10 = silk_RSHIFT( q1_Q10, 10 );
448             if( q1_Q10 > 0 ) {
449                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
450                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
451                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
452                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
453                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
454             } else if( q1_Q10 == 0 ) {
455                 q1_Q10  = offset_Q10;
456                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
457                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
458                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
459             } else if( q1_Q10 == -1 ) {
460                 q2_Q10  = offset_Q10;
461                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
462                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
463                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
464             } else {            /* Q1_Q10 < -1 */
465                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
466                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
467                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
468                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
469                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
470             }
471             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
472             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
473             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
474             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
475
476             if( rd1_Q10 < rd2_Q10 ) {
477                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
478                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
479                 psSS[ 0 ].Q_Q10  = q1_Q10;
480                 psSS[ 1 ].Q_Q10  = q2_Q10;
481             } else {
482                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
483                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
484                 psSS[ 0 ].Q_Q10  = q2_Q10;
485                 psSS[ 1 ].Q_Q10  = q1_Q10;
486             }
487
488             /* Update states for best quantization */
489
490             /* Quantized excitation */
491             exc_Q10 = psSS[ 0 ].Q_Q10 ^ dither;
492
493             /* Add predictions */
494             LPC_exc_Q10 = exc_Q10 + silk_RSHIFT_ROUND( LTP_pred_Q14, 4 );
495             xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
496
497             /* Update states */
498             sLF_AR_shp_Q10         = silk_SUB32(  xq_Q10, n_AR_Q10 );
499             psSS[ 0 ].sLTP_shp_Q10 = silk_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );
500             psSS[ 0 ].LF_AR_Q12    = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
501             psSS[ 0 ].xq_Q14       = silk_LSHIFT( xq_Q10,         4 );
502             psSS[ 0 ].LPC_exc_Q16  = silk_LSHIFT( LPC_exc_Q10,    6 );
503
504             /* Update states for second best quantization */
505
506             /* Quantized excitation */
507             exc_Q10 = psSS[ 1 ].Q_Q10 ^ dither;
508
509             /* Add predictions */
510             LPC_exc_Q10 = exc_Q10 + silk_RSHIFT_ROUND( LTP_pred_Q14, 4 );
511             xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
512
513             /* Update states */
514             sLF_AR_shp_Q10         = silk_SUB32(  xq_Q10, n_AR_Q10 );
515             psSS[ 1 ].sLTP_shp_Q10 = silk_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );
516             psSS[ 1 ].LF_AR_Q12    = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
517             psSS[ 1 ].xq_Q14       = silk_LSHIFT( xq_Q10,         4 );
518             psSS[ 1 ].LPC_exc_Q16  = silk_LSHIFT( LPC_exc_Q10,    6 );
519         }
520
521         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) & DECISION_DELAY_MASK;                   /* Index to newest samples              */
522         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) & DECISION_DELAY_MASK;       /* Index to decisionDelay old samples   */
523
524         /* Find winner */
525         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
526         Winner_ind = 0;
527         for( k = 1; k < nStatesDelayedDecision; k++ ) {
528             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
529                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
530                 Winner_ind = k;
531             }
532         }
533
534         /* Increase RD values of expired states */
535         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
536         for( k = 0; k < nStatesDelayedDecision; k++ ) {
537             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
538                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, ( silk_int32_MAX >> 4 ) );
539                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, ( silk_int32_MAX >> 4 ) );
540                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
541             }
542         }
543
544         /* Find worst in first set and best in second set */
545         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
546         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
547         RDmax_ind = 0;
548         RDmin_ind = 0;
549         for( k = 1; k < nStatesDelayedDecision; k++ ) {
550             /* find worst in first set */
551             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
552                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
553                 RDmax_ind = k;
554             }
555             /* find best in second set */
556             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
557                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
558                 RDmin_ind = k;
559             }
560         }
561
562         /* Replace a state if best from second set outperforms worst in first set */
563         if( RDmin_Q10 < RDmax_Q10 ) {
564             silk_memcpy( ((opus_int32 *)&psDelDec[ RDmax_ind ]) + i,
565                         ((opus_int32 *)&psDelDec[ RDmin_ind ]) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
566             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
567         }
568
569         /* Write samples from winner to output and long-term filter states */
570         psDD = &psDelDec[ Winner_ind ];
571         if( subfr > 0 || i >= decisionDelay ) {
572             pulses[  i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
573             xq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
574                 silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], delayedGain_Q16[ last_smple_idx ] ), 10 ) );
575             NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q10[ last_smple_idx ];
576             sLTP_Q16[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q16[  last_smple_idx ];
577         }
578         NSQ->sLTP_shp_buf_idx++;
579         NSQ->sLTP_buf_idx++;
580
581         /* Update states */
582         for( k = 0; k < nStatesDelayedDecision; k++ ) {
583             psDD                                     = &psDelDec[ k ];
584             psSS                                     = &psSampleState[ k ][ 0 ];
585             psDD->LF_AR_Q12                          = psSS->LF_AR_Q12;
586             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
587             psDD->Xq_Q10[    *smpl_buf_idx ]         = silk_RSHIFT( psSS->xq_Q14, 4 );
588             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
589             psDD->Pred_Q16[  *smpl_buf_idx ]         = psSS->LPC_exc_Q16;
590             psDD->Shape_Q10[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q10;
591             psDD->Seed                               = silk_ADD32( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
592             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
593             psDD->RD_Q10                             = psSS->RD_Q10;
594         }
595         delayedGain_Q16[     *smpl_buf_idx ]         = Gain_Q16;
596     }
597     /* Update LPC states */
598     for( k = 0; k < nStatesDelayedDecision; k++ ) {
599         psDD = &psDelDec[ k ];
600         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
601     }
602 }
603
604 static inline void silk_nsq_del_dec_scale_states(
605     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
606     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
607     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
608     const opus_int16     x[],                        /* I    Input in Q0                         */
609     opus_int32           x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
610     const opus_int16     sLTP[],                     /* I    Re-whitened LTP state in Q0         */
611     opus_int32           sLTP_Q16[],                 /* O    LTP state matching scaled input     */
612     opus_int             subfr,                      /* I    Subframe number                     */
613     opus_int             nStatesDelayedDecision,     /* I    Number of del dec states            */
614     const opus_int       LTP_scale_Q14,              /* I    LTP state scaling                   */
615     const opus_int32     Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
616     const opus_int       pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
617     const opus_int       signal_type,                /* I    Signal type                         */
618     const opus_int       decisionDelay               /* I    Decision delay                      */
619 )
620 {
621     opus_int            i, k, lag;
622     opus_int32          inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;
623     NSQ_del_dec_struct *psDD;
624
625     inv_gain_Q16 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 32 );
626     inv_gain_Q16 = silk_min( inv_gain_Q16, silk_int16_MAX );
627     lag          = pitchL[ subfr ];
628
629     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
630     if( NSQ->rewhite_flag ) {
631         inv_gain_Q32 = silk_LSHIFT( inv_gain_Q16, 16 );
632         if( subfr == 0 ) {
633             /* Do LTP downscaling */
634             inv_gain_Q32 = silk_LSHIFT( silk_SMULWB( inv_gain_Q32, LTP_scale_Q14 ), 2 );
635         }
636         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
637             silk_assert( i < MAX_FRAME_LENGTH );
638             sLTP_Q16[ i ] = silk_SMULWB( inv_gain_Q32, sLTP[ i ] );
639         }
640     }
641
642     /* Adjust for changing gain */
643     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {
644         gain_adj_Q16 = silk_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );
645
646         /* Scale long-term shaping state */
647         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
648             NSQ->sLTP_shp_Q10[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );
649         }
650
651         /* Scale long-term prediction state */
652         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
653             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx - decisionDelay; i++ ) {
654                 sLTP_Q16[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q16[ i ] );
655             }
656         }
657
658         for( k = 0; k < nStatesDelayedDecision; k++ ) {
659             psDD = &psDelDec[ k ];
660
661             /* Scale scalar states */
662             psDD->LF_AR_Q12 = silk_SMULWW( gain_adj_Q16, psDD->LF_AR_Q12 );
663
664             /* Scale short-term prediction and shaping states */
665             for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
666                 psDD->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sLPC_Q14[ i ] );
667             }
668             for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
669                 psDD->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, psDD->sAR2_Q14[ i ] );
670             }
671             for( i = 0; i < DECISION_DELAY; i++ ) {
672                 psDD->Pred_Q16[  i ] = silk_SMULWW( gain_adj_Q16, psDD->Pred_Q16[  i ] );
673                 psDD->Shape_Q10[ i ] = silk_SMULWW( gain_adj_Q16, psDD->Shape_Q10[ i ] );
674             }
675         }
676     }
677
678     /* Scale input */
679     for( i = 0; i < psEncC->subfr_length; i++ ) {
680         x_sc_Q10[ i ] = silk_RSHIFT( silk_SMULBB( x[ i ], ( opus_int16 )inv_gain_Q16 ), 6 );
681     }
682
683     /* save inv_gain */
684     silk_assert( inv_gain_Q16 != 0 );
685     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;
686 }