Making the left shift macros use unsigned to avoid undefined behaviour
[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     opus_int             smpl_buf_idx,               /* I    Index to newest samples in buffers  */
68     const opus_int       LTP_scale_Q14,              /* I    LTP state scaling                   */
69     const opus_int32     Gains_Q16[ MAX_NB_SUBFR ],  /* I                                        */
70     const opus_int       pitchL[ MAX_NB_SUBFR ],     /* I    Pitch lag                           */
71     const opus_int       signal_type,                /* I    Signal type                         */
72     const opus_int       decisionDelay               /* I    Decision delay                      */
73 );
74
75 /******************************************/
76 /* Noise shape quantizer for one subframe */
77 /******************************************/
78 static inline void silk_noise_shape_quantizer_del_dec(
79     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                           */
80     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
81     opus_int             signalType,             /* I    Signal type                         */
82     const opus_int32     x_Q10[],                /* I                                        */
83     opus_int8            pulses[],               /* O                                        */
84     opus_int16           xq[],                   /* O                                        */
85     opus_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */
86     opus_int32           delayedGain_Q16[],      /* I/O  Gain delay buffer                   */
87     const opus_int16     a_Q12[],                /* I    Short term prediction coefs         */
88     const opus_int16     b_Q14[],                /* I    Long term prediction coefs          */
89     const opus_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */
90     opus_int             lag,                    /* I    Pitch lag                           */
91     opus_int32           HarmShapeFIRPacked_Q14, /* I                                        */
92     opus_int             Tilt_Q14,               /* I    Spectral tilt                       */
93     opus_int32           LF_shp_Q14,             /* I                                        */
94     opus_int32           Gain_Q16,               /* I                                        */
95     opus_int             Lambda_Q10,             /* I                                        */
96     opus_int             offset_Q10,             /* I                                        */
97     opus_int             length,                 /* I    Input length                        */
98     opus_int             subfr,                  /* I    Subframe number                     */
99     opus_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */
100     opus_int             predictLPCOrder,        /* I    Prediction filter order             */
101     opus_int             warping_Q16,            /* I                                        */
102     opus_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */
103     opus_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
104     opus_int             decisionDelay           /* I                                        */
105 );
106
107 void silk_NSQ_del_dec(
108     const silk_encoder_state        *psEncC,                                    /* I/O  Encoder State                       */
109     silk_nsq_state                  *NSQ,                                       /* I/O  NSQ state                           */
110     SideInfoIndices                 *psIndices,                                 /* I/O  Quantization Indices                */
111     const opus_int16                 x[],                                        /* I    Prefiltered input signal            */
112     opus_int8                        pulses[],                                   /* O    Quantized pulse signal              */
113     const opus_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Prediction coefs                    */
114     const opus_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    LT prediction coefs                 */
115     const opus_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I                                     */
116     const opus_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I                                        */
117     const opus_int                   Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                       */
118     const opus_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I                                        */
119     const opus_int32                 Gains_Q16[ MAX_NB_SUBFR ],                  /* I                                        */
120     const opus_int                   pitchL[ MAX_NB_SUBFR ],                     /* I                                        */
121     const opus_int                   Lambda_Q10,                                 /* I                                        */
122     const opus_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */
123 )
124 {
125     opus_int     i, k, lag, start_idx, LSF_interpolation_flag, Winner_ind, subfr;
126     opus_int     last_smple_idx, smpl_buf_idx, decisionDelay;
127     const opus_int16 *A_Q12, *B_Q14, *AR_shp_Q13;
128     opus_int16   *pxq;
129     opus_int32   sLTP_Q16[ 2 * MAX_FRAME_LENGTH ];
130     opus_int16   sLTP[     2 * MAX_FRAME_LENGTH ];
131     opus_int32   HarmShapeFIRPacked_Q14;
132     opus_int     offset_Q10;
133     opus_int32   RDmin_Q10;
134     opus_int32   x_sc_Q10[ MAX_SUB_FRAME_LENGTH ];
135     opus_int32   delayedGain_Q16[  DECISION_DELAY ];
136     NSQ_del_dec_struct psDelDec[ MAX_DEL_DEC_STATES ];
137     NSQ_del_dec_struct *psDD;
138
139     /* Set unvoiced lag to the previous one, overwrite later for voiced */
140     lag = NSQ->lagPrev;
141
142     silk_assert( NSQ->prev_inv_gain_Q16 != 0 );
143
144     /* Initialize delayed decision states */
145     silk_memset( psDelDec, 0, psEncC->nStatesDelayedDecision * sizeof( NSQ_del_dec_struct ) );
146     for( k = 0; k < psEncC->nStatesDelayedDecision; k++ ) {
147         psDD                 = &psDelDec[ k ];
148         psDD->Seed           = ( k + psIndices->Seed ) & 3;
149         psDD->SeedInit       = psDD->Seed;
150         psDD->RD_Q10         = 0;
151         psDD->LF_AR_Q12      = NSQ->sLF_AR_shp_Q12;
152         psDD->Shape_Q10[ 0 ] = NSQ->sLTP_shp_Q10[ psEncC->ltp_mem_length - 1 ];
153         silk_memcpy( psDD->sLPC_Q14, NSQ->sLPC_Q14, NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
154         silk_memcpy( psDD->sAR2_Q14, NSQ->sAR2_Q14, sizeof( NSQ->sAR2_Q14 ) );
155     }
156
157     offset_Q10   = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
158     smpl_buf_idx = 0; /* index of oldest samples */
159
160     decisionDelay = silk_min_int( DECISION_DELAY, psEncC->subfr_length );
161
162     /* For voiced frames limit the decision delay to lower than the pitch lag */
163     if( psIndices->signalType == TYPE_VOICED ) {
164         for( k = 0; k < psEncC->nb_subfr; k++ ) {
165             decisionDelay = silk_min_int( decisionDelay, pitchL[ k ] - LTP_ORDER / 2 - 1 );
166         }
167     } else {
168         if( lag > 0 ) {
169             decisionDelay = silk_min_int( decisionDelay, lag - LTP_ORDER / 2 - 1 );
170         }
171     }
172
173     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
174         LSF_interpolation_flag = 0;
175     } else {
176         LSF_interpolation_flag = 1;
177     }
178
179     /* Setup pointers to start of sub frame */
180     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
181     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
182     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
183     subfr = 0;
184     for( k = 0; k < psEncC->nb_subfr; k++ ) {
185         A_Q12      = &PredCoef_Q12[ ( ( k >> 1 ) | ( 1 - LSF_interpolation_flag ) ) * MAX_LPC_ORDER ];
186         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER           ];
187         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];
188
189         /* Noise shape parameters */
190         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
191         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
192         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( ( opus_int32 )silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
193
194         NSQ->rewhite_flag = 0;
195         if( psIndices->signalType == TYPE_VOICED ) {
196             /* Voiced */
197             lag = pitchL[ k ];
198
199             /* Re-whitening */
200             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
201                 if( k == 2 ) {
202                     /* RESET DELAYED DECISIONS */
203                     /* Find winner */
204                     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
205                     Winner_ind = 0;
206                     for( i = 1; i < psEncC->nStatesDelayedDecision; i++ ) {
207                         if( psDelDec[ i ].RD_Q10 < RDmin_Q10 ) {
208                             RDmin_Q10 = psDelDec[ i ].RD_Q10;
209                             Winner_ind = i;
210                         }
211                     }
212                     for( i = 0; i < psEncC->nStatesDelayedDecision; i++ ) {
213                         if( i != Winner_ind ) {
214                             psDelDec[ i ].RD_Q10 += ( silk_int32_MAX >> 4 );
215                             silk_assert( psDelDec[ i ].RD_Q10 >= 0 );
216                         }
217                     }
218
219                     /* Copy final part of signals from winner state to output and long-term filter states */
220                     psDD = &psDelDec[ Winner_ind ];
221                     last_smple_idx = smpl_buf_idx + decisionDelay;
222                     for( i = 0; i < decisionDelay; i++ ) {
223                         last_smple_idx = ( last_smple_idx - 1 ) & DECISION_DELAY_MASK;
224                         pulses[   i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
225                         pxq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
226                             silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], Gains_Q16[ 1 ] ), 10 ) );
227                         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ last_smple_idx ];
228                     }
229
230                     subfr = 0;
231                 }
232
233                 /* Rewhiten with new A coefs */
234                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
235                 silk_assert( start_idx > 0 );
236
237                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
238                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder );
239
240                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
241                 NSQ->rewhite_flag = 1;
242             }
243         }
244
245         silk_nsq_del_dec_scale_states( psEncC, NSQ, psDelDec, x, x_sc_Q10, sLTP, sLTP_Q16, k,
246             psEncC->nStatesDelayedDecision, smpl_buf_idx, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType, decisionDelay );
247
248         silk_noise_shape_quantizer_del_dec( NSQ, psDelDec, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q16,
249             delayedGain_Q16, A_Q12, B_Q14, AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ],
250             Gains_Q16[ k ], Lambda_Q10, offset_Q10, psEncC->subfr_length, subfr++, psEncC->shapingLPCOrder,
251             psEncC->predictLPCOrder, psEncC->warping_Q16, psEncC->nStatesDelayedDecision, &smpl_buf_idx, decisionDelay );
252
253         x      += psEncC->subfr_length;
254         pulses += psEncC->subfr_length;
255         pxq    += psEncC->subfr_length;
256     }
257
258     /* Find winner */
259     RDmin_Q10 = psDelDec[ 0 ].RD_Q10;
260     Winner_ind = 0;
261     for( k = 1; k < psEncC->nStatesDelayedDecision; k++ ) {
262         if( psDelDec[ k ].RD_Q10 < RDmin_Q10 ) {
263             RDmin_Q10 = psDelDec[ k ].RD_Q10;
264             Winner_ind = k;
265         }
266     }
267
268     /* Copy final part of signals from winner state to output and long-term filter states */
269     psDD = &psDelDec[ Winner_ind ];
270     psIndices->Seed = psDD->SeedInit;
271     last_smple_idx = smpl_buf_idx + decisionDelay;
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_Q10[ last_smple_idx ], Gains_Q16[ psEncC->nb_subfr - 1 ] ), 10 ) );
277         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay + i ] = psDD->Shape_Q10[ 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_Q12 = psDD->LF_AR_Q12;
284     NSQ->lagPrev        = pitchL[ psEncC->nb_subfr - 1 ];
285
286     /* Save quantized speech and noise shaping signals */
287     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
288     silk_memmove( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
289
290 #ifdef SAVE_ALL_INTERNAL_DATA
291     DEBUG_STORE_DATA( xq.dat,       &pxq[ -psEncC->frame_length ],       psEncC->frame_length * sizeof( opus_int16 ) );
292     DEBUG_STORE_DATA( q.dat,        &pulses[ -psEncC->frame_length ],    psEncC->frame_length * sizeof( opus_int8 ) );
293 #endif
294 }
295
296 /******************************************/
297 /* Noise shape quantizer for one subframe */
298 /******************************************/
299 static inline void silk_noise_shape_quantizer_del_dec(
300     silk_nsq_state  *NSQ,                   /* I/O  NSQ state                           */
301     NSQ_del_dec_struct  psDelDec[],             /* I/O  Delayed decision states             */
302     opus_int             signalType,             /* I    Signal type                         */
303     const opus_int32     x_Q10[],                /* I                                        */
304     opus_int8            pulses[],               /* O                                        */
305     opus_int16           xq[],                   /* O                                        */
306     opus_int32           sLTP_Q16[],             /* I/O  LTP filter state                    */
307     opus_int32           delayedGain_Q16[],      /* I/O  Gain delay buffer                   */
308     const opus_int16     a_Q12[],                /* I    Short term prediction coefs         */
309     const opus_int16     b_Q14[],                /* I    Long term prediction coefs          */
310     const opus_int16     AR_shp_Q13[],           /* I    Noise shaping coefs                 */
311     opus_int             lag,                    /* I    Pitch lag                           */
312     opus_int32           HarmShapeFIRPacked_Q14, /* I                                        */
313     opus_int             Tilt_Q14,               /* I    Spectral tilt                       */
314     opus_int32           LF_shp_Q14,             /* I                                        */
315     opus_int32           Gain_Q16,               /* I                                        */
316     opus_int             Lambda_Q10,             /* I                                        */
317     opus_int             offset_Q10,             /* I                                        */
318     opus_int             length,                 /* I    Input length                        */
319     opus_int             subfr,                  /* I    Subframe number                     */
320     opus_int             shapingLPCOrder,        /* I    Shaping LPC filter order            */
321     opus_int             predictLPCOrder,        /* I    Prediction filter order             */
322     opus_int             warping_Q16,            /* I                                        */
323     opus_int             nStatesDelayedDecision, /* I    Number of states in decision tree   */
324     opus_int             *smpl_buf_idx,          /* I    Index to newest samples in buffers  */
325     opus_int             decisionDelay           /* I                                        */
326 )
327 {
328     opus_int     i, j, k, Winner_ind, RDmin_ind, RDmax_ind, last_smple_idx;
329     opus_int32   Winner_rand_state;
330     opus_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14, LTP_Q10;
331     opus_int32   n_LF_Q10, r_Q10, rr_Q10, rd1_Q10, rd2_Q10, RDmin_Q10, RDmax_Q10;
332     opus_int32   q1_Q10, q2_Q10, dither, exc_Q10, LPC_exc_Q10, xq_Q10;
333     opus_int32   tmp1, tmp2, sLF_AR_shp_Q10;
334     opus_int32   *pred_lag_ptr, *shp_lag_ptr, *psLPC_Q14;
335     NSQ_sample_struct  psSampleState[ MAX_DEL_DEC_STATES ][ 2 ];
336     NSQ_del_dec_struct *psDD;
337     NSQ_sample_struct  *psSS;
338
339     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
340     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
341
342     for( i = 0; i < length; i++ ) {
343         /* Perform common calculations used in all states */
344
345         /* Long-term prediction */
346         if( signalType == TYPE_VOICED ) {
347             /* Unrolled loop */
348             LTP_pred_Q14 = silk_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );
349             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
350             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
351             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
352             LTP_pred_Q14 = silk_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
353             pred_lag_ptr++;
354         } else {
355             LTP_pred_Q14 = 0;
356         }
357
358         /* Long-term shaping */
359         if( lag > 0 ) {
360             /* Symmetric, packed FIR coefficients */
361             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
362             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );
363             n_LTP_Q14 = silk_LSHIFT( n_LTP_Q14, 6 );
364             shp_lag_ptr++;
365
366             LTP_Q10 = silk_RSHIFT( silk_SUB32( LTP_pred_Q14, n_LTP_Q14 ), 4 );
367         } else {
368             LTP_Q10 = 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             /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */
382             dither = silk_RSHIFT( psDD->Seed, 31 );
383
384             /* Pointer used in short term prediction and shaping */
385             psLPC_Q14 = &psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 + i ];
386             /* Short-term prediction */
387             silk_assert( predictLPCOrder >= 10 );            /* check that unrolling works */
388             silk_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */
389             silk_assert( ( (opus_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */
390             /* Partially unrolled */
391             LPC_pred_Q10 = silk_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );
392             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
393             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
394             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
395             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
396             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
397             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
398             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
399             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
400             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
401             for( j = 10; j < predictLPCOrder; j ++ ) {
402                 LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );
403             }
404
405             /* Noise shape feedback */
406             silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
407             /* Output of lowpass section */
408             tmp2 = silk_SMLAWB( psLPC_Q14[ 0 ], psDD->sAR2_Q14[ 0 ], warping_Q16 );
409             /* Output of allpass section */
410             tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ 0 ], psDD->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );
411             psDD->sAR2_Q14[ 0 ] = tmp2;
412             n_AR_Q10 = silk_SMULWB( tmp2, AR_shp_Q13[ 0 ] );
413             /* Loop over allpass sections */
414             for( j = 2; j < shapingLPCOrder; j += 2 ) {
415                 /* Output of allpass section */
416                 tmp2 = silk_SMLAWB( psDD->sAR2_Q14[ j - 1 ], psDD->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );
417                 psDD->sAR2_Q14[ j - 1 ] = tmp1;
418                 n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );
419                 /* Output of allpass section */
420                 tmp1 = silk_SMLAWB( psDD->sAR2_Q14[ j + 0 ], psDD->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );
421                 psDD->sAR2_Q14[ j + 0 ] = tmp2;
422                 n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );
423             }
424             psDD->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
425             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
426
427             n_AR_Q10 = silk_RSHIFT( n_AR_Q10, 1 );           /* Q11 -> Q10 */
428             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, psDD->LF_AR_Q12, Tilt_Q14 );
429
430             n_LF_Q10 = silk_LSHIFT( silk_SMULWB( psDD->Shape_Q10[ *smpl_buf_idx ], LF_shp_Q14 ), 2 );
431             n_LF_Q10 = silk_SMLAWT( n_LF_Q10, psDD->LF_AR_Q12, LF_shp_Q14 );
432
433             /* Input minus prediction plus noise feedback                       */
434             /* r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP  */
435             tmp1  = silk_ADD32( LTP_Q10, LPC_pred_Q10 );                         /* add Q10 stuff */
436             tmp1  = silk_SUB32( tmp1, n_AR_Q10 );                                /* subtract Q10 stuff */
437             tmp1  = silk_SUB32( tmp1, n_LF_Q10 );                                /* subtract Q10 stuff */
438             r_Q10 = silk_SUB32( x_Q10[ i ], tmp1 );                              /* residual error Q10 */
439
440             /* Flip sign depending on dither */
441             r_Q10 = r_Q10 ^ dither;
442             r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
443
444             /* Find two quantization level candidates and measure their rate-distortion */
445             q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
446             q1_Q10 = silk_RSHIFT( q1_Q10, 10 );
447             if( q1_Q10 > 0 ) {
448                 q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
449                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
450                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
451                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
452                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
453             } else if( q1_Q10 == 0 ) {
454                 q1_Q10  = offset_Q10;
455                 q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
456                 rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
457                 rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
458             } else if( q1_Q10 == -1 ) {
459                 q2_Q10  = offset_Q10;
460                 q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
461                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
462                 rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
463             } else {            /* Q1_Q10 < -1 */
464                 q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
465                 q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
466                 q2_Q10  = silk_ADD32( q1_Q10, 1024 );
467                 rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
468                 rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
469             }
470             rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
471             rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
472             rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
473             rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
474
475             if( rd1_Q10 < rd2_Q10 ) {
476                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
477                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
478                 psSS[ 0 ].Q_Q10  = q1_Q10;
479                 psSS[ 1 ].Q_Q10  = q2_Q10;
480             } else {
481                 psSS[ 0 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd2_Q10 );
482                 psSS[ 1 ].RD_Q10 = silk_ADD32( psDD->RD_Q10, rd1_Q10 );
483                 psSS[ 0 ].Q_Q10  = q2_Q10;
484                 psSS[ 1 ].Q_Q10  = q1_Q10;
485             }
486
487             /* Update states for best quantization */
488
489             /* Quantized excitation */
490             exc_Q10 = psSS[ 0 ].Q_Q10 ^ dither;
491
492             /* Add predictions */
493             LPC_exc_Q10 = exc_Q10 + silk_RSHIFT_ROUND( LTP_pred_Q14, 4 );
494             xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
495
496             /* Update states */
497             sLF_AR_shp_Q10         = silk_SUB32(  xq_Q10, n_AR_Q10 );
498             psSS[ 0 ].sLTP_shp_Q10 = silk_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );
499             psSS[ 0 ].LF_AR_Q12    = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
500             psSS[ 0 ].xq_Q14       = silk_LSHIFT( xq_Q10,         4 );
501             psSS[ 0 ].LPC_exc_Q16  = silk_LSHIFT( LPC_exc_Q10,    6 );
502
503             /* Update states for second best quantization */
504
505             /* Quantized excitation */
506             exc_Q10 = psSS[ 1 ].Q_Q10 ^ dither;
507
508             /* Add predictions */
509             LPC_exc_Q10 = exc_Q10 + silk_RSHIFT_ROUND( LTP_pred_Q14, 4 );
510             xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
511
512             /* Update states */
513             sLF_AR_shp_Q10         = silk_SUB32(  xq_Q10, n_AR_Q10 );
514             psSS[ 1 ].sLTP_shp_Q10 = silk_SUB32(  sLF_AR_shp_Q10, n_LF_Q10 );
515             psSS[ 1 ].LF_AR_Q12    = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
516             psSS[ 1 ].xq_Q14       = silk_LSHIFT( xq_Q10,         4 );
517             psSS[ 1 ].LPC_exc_Q16  = silk_LSHIFT( LPC_exc_Q10,    6 );
518         }
519
520         *smpl_buf_idx  = ( *smpl_buf_idx - 1 ) & DECISION_DELAY_MASK;                   /* Index to newest samples              */
521         last_smple_idx = ( *smpl_buf_idx + decisionDelay ) & DECISION_DELAY_MASK;       /* Index to decisionDelay old samples   */
522
523         /* Find winner */
524         RDmin_Q10 = psSampleState[ 0 ][ 0 ].RD_Q10;
525         Winner_ind = 0;
526         for( k = 1; k < nStatesDelayedDecision; k++ ) {
527             if( psSampleState[ k ][ 0 ].RD_Q10 < RDmin_Q10 ) {
528                 RDmin_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
529                 Winner_ind = k;
530             }
531         }
532
533         /* Increase RD values of expired states */
534         Winner_rand_state = psDelDec[ Winner_ind ].RandState[ last_smple_idx ];
535         for( k = 0; k < nStatesDelayedDecision; k++ ) {
536             if( psDelDec[ k ].RandState[ last_smple_idx ] != Winner_rand_state ) {
537                 psSampleState[ k ][ 0 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 0 ].RD_Q10, ( silk_int32_MAX >> 4 ) );
538                 psSampleState[ k ][ 1 ].RD_Q10 = silk_ADD32( psSampleState[ k ][ 1 ].RD_Q10, ( silk_int32_MAX >> 4 ) );
539                 silk_assert( psSampleState[ k ][ 0 ].RD_Q10 >= 0 );
540             }
541         }
542
543         /* Find worst in first set and best in second set */
544         RDmax_Q10  = psSampleState[ 0 ][ 0 ].RD_Q10;
545         RDmin_Q10  = psSampleState[ 0 ][ 1 ].RD_Q10;
546         RDmax_ind = 0;
547         RDmin_ind = 0;
548         for( k = 1; k < nStatesDelayedDecision; k++ ) {
549             /* find worst in first set */
550             if( psSampleState[ k ][ 0 ].RD_Q10 > RDmax_Q10 ) {
551                 RDmax_Q10  = psSampleState[ k ][ 0 ].RD_Q10;
552                 RDmax_ind = k;
553             }
554             /* find best in second set */
555             if( psSampleState[ k ][ 1 ].RD_Q10 < RDmin_Q10 ) {
556                 RDmin_Q10  = psSampleState[ k ][ 1 ].RD_Q10;
557                 RDmin_ind = k;
558             }
559         }
560
561         /* Replace a state if best from second set outperforms worst in first set */
562         if( RDmin_Q10 < RDmax_Q10 ) {
563             silk_memcpy( ((opus_int32 *)&psDelDec[ RDmax_ind ]) + i,
564                         ((opus_int32 *)&psDelDec[ RDmin_ind ]) + i, sizeof( NSQ_del_dec_struct ) - i * sizeof( opus_int32) );
565             silk_memcpy( &psSampleState[ RDmax_ind ][ 0 ], &psSampleState[ RDmin_ind ][ 1 ], sizeof( NSQ_sample_struct ) );
566         }
567
568         /* Write samples from winner to output and long-term filter states */
569         psDD = &psDelDec[ Winner_ind ];
570         if( subfr > 0 || i >= decisionDelay ) {
571             pulses[  i - decisionDelay ] = ( opus_int8 )silk_RSHIFT_ROUND( psDD->Q_Q10[ last_smple_idx ], 10 );
572             xq[ i - decisionDelay ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND(
573                 silk_SMULWW( psDD->Xq_Q10[ last_smple_idx ], delayedGain_Q16[ last_smple_idx ] ), 10 ) );
574             NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - decisionDelay ] = psDD->Shape_Q10[ last_smple_idx ];
575             sLTP_Q16[          NSQ->sLTP_buf_idx     - decisionDelay ] = psDD->Pred_Q16[  last_smple_idx ];
576         }
577         NSQ->sLTP_shp_buf_idx++;
578         NSQ->sLTP_buf_idx++;
579
580         /* Update states */
581         for( k = 0; k < nStatesDelayedDecision; k++ ) {
582             psDD                                     = &psDelDec[ k ];
583             psSS                                     = &psSampleState[ k ][ 0 ];
584             psDD->LF_AR_Q12                          = psSS->LF_AR_Q12;
585             psDD->sLPC_Q14[ NSQ_LPC_BUF_LENGTH + i ] = psSS->xq_Q14;
586             psDD->Xq_Q10[    *smpl_buf_idx ]         = silk_RSHIFT( psSS->xq_Q14, 4 );
587             psDD->Q_Q10[     *smpl_buf_idx ]         = psSS->Q_Q10;
588             psDD->Pred_Q16[  *smpl_buf_idx ]         = psSS->LPC_exc_Q16;
589             psDD->Shape_Q10[ *smpl_buf_idx ]         = psSS->sLTP_shp_Q10;
590             psDD->Seed                               = silk_ADD32( psDD->Seed, silk_RSHIFT_ROUND( psSS->Q_Q10, 10 ) );
591             psDD->RandState[ *smpl_buf_idx ]         = psDD->Seed;
592             psDD->RD_Q10                             = psSS->RD_Q10;
593         }
594         delayedGain_Q16[     *smpl_buf_idx ]         = Gain_Q16;
595     }
596     /* Update LPC states */
597     for( k = 0; k < nStatesDelayedDecision; k++ ) {
598         psDD = &psDelDec[ k ];
599         silk_memcpy( psDD->sLPC_Q14, &psDD->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
600     }
601 }
602
603 static inline void silk_nsq_del_dec_scale_states(
604     const silk_encoder_state *psEncC,               /* I    Encoder State                       */
605     silk_nsq_state      *NSQ,                       /* I/O  NSQ state                           */
606     NSQ_del_dec_struct  psDelDec[],                 /* I/O  Delayed decision states             */
607     const opus_int16     x[],                        /* I    Input in Q0                         */
608     opus_int32           x_sc_Q10[],                 /* O    Input scaled with 1/Gain in Q10     */
609     const opus_int16     sLTP[],                     /* I    Re-whitened LTP state in Q0         */
610     opus_int32           sLTP_Q16[],                 /* O    LTP state matching scaled input     */
611     opus_int             subfr,                      /* I    Subframe number                     */
612     opus_int             nStatesDelayedDecision,     /* I    Number of del dec states            */
613     opus_int             smpl_buf_idx,               /* I    Index to newest samples in buffers  */
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 }