Eliminate some unreachable cases from the cwrs code and fixup the
[opus.git] / silk / silk_NSQ.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 #include "silk_main.h"
29
30 SKP_INLINE void silk_nsq_scale_states(
31     const silk_encoder_state *psEncC,       /* I    Encoder State                   */
32     silk_nsq_state      *NSQ,               /* I/O  NSQ state                       */
33     const opus_int16     x[],                /* I    input in Q0                     */
34     opus_int32           x_sc_Q10[],         /* O    input scaled with 1/Gain        */
35     const opus_int16     sLTP[],             /* I    re-whitened LTP state in Q0     */
36     opus_int32           sLTP_Q16[],         /* O    LTP state matching scaled input */
37     opus_int             subfr,              /* I    subframe number                 */
38     const opus_int       LTP_scale_Q14,      /* I                                    */
39     const opus_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                             */
40     const opus_int       pitchL[ MAX_NB_SUBFR ]  /* I                                */
41 );
42
43 SKP_INLINE void silk_noise_shape_quantizer(
44     silk_nsq_state      *NSQ,               /* I/O  NSQ state                       */
45     opus_int             signalType,         /* I    Signal type                     */
46     const opus_int32     x_sc_Q10[],         /* I                                    */
47     opus_int8            pulses[],           /* O                                    */
48     opus_int16           xq[],               /* O                                    */
49     opus_int32           sLTP_Q16[],         /* I/O  LTP state                       */
50     const opus_int16     a_Q12[],            /* I    Short term prediction coefs     */
51     const opus_int16     b_Q14[],            /* I    Long term prediction coefs      */
52     const opus_int16     AR_shp_Q13[],       /* I    Noise shaping AR coefs          */
53     opus_int             lag,                /* I    Pitch lag                       */
54     opus_int32           HarmShapeFIRPacked_Q14, /* I                                */
55     opus_int             Tilt_Q14,           /* I    Spectral tilt                   */
56     opus_int32           LF_shp_Q14,         /* I                                    */
57     opus_int32           Gain_Q16,           /* I                                    */
58     opus_int             Lambda_Q10,         /* I                                    */
59     opus_int             offset_Q10,         /* I                                    */
60     opus_int             length,             /* I    Input length                    */
61     opus_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */
62     opus_int             predictLPCOrder     /* I    Prediction filter order         */
63 );
64
65 void silk_NSQ(
66     const silk_encoder_state        *psEncC,                                    /* I/O  Encoder State                       */
67     silk_nsq_state                  *NSQ,                                       /* I/O  NSQ state                           */
68     SideInfoIndices                 *psIndices,                                 /* I/O  Quantization Indices                */
69     const opus_int16                 x[],                                        /* I    prefiltered input signal            */
70     opus_int8                        pulses[],                                   /* O    quantized qulse signal              */
71     const opus_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefficients  */
72     const opus_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefficients   */
73     const opus_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I                                     */
74     const opus_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I                                        */
75     const opus_int                   Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                       */
76     const opus_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I                                        */
77     const opus_int32                 Gains_Q16[ MAX_NB_SUBFR ],                  /* I                                        */
78     const opus_int                   pitchL[ MAX_NB_SUBFR ],                     /* I                                        */
79     const opus_int                   Lambda_Q10,                                 /* I                                        */
80     const opus_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */
81 )
82 {
83     opus_int     k, lag, start_idx, LSF_interpolation_flag;
84     const opus_int16 *A_Q12, *B_Q14, *AR_shp_Q13;
85     opus_int16   *pxq;
86     opus_int32   sLTP_Q16[ 2 * MAX_FRAME_LENGTH ];
87     opus_int16   sLTP[     2 * MAX_FRAME_LENGTH ];
88     opus_int32   HarmShapeFIRPacked_Q14;
89     opus_int     offset_Q10;
90     opus_int32   x_sc_Q10[ MAX_FRAME_LENGTH / MAX_NB_SUBFR ];
91
92     NSQ->rand_seed = psIndices->Seed;
93
94     /* Set unvoiced lag to the previous one, overwrite later for voiced */
95     lag = NSQ->lagPrev;
96
97     SKP_assert( NSQ->prev_inv_gain_Q16 != 0 );
98
99     offset_Q10 = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
100
101     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
102         LSF_interpolation_flag = 0;
103     } else {
104         LSF_interpolation_flag = 1;
105     }
106
107     /* Setup pointers to start of sub frame */
108     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
109     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
110     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
111     for( k = 0; k < psEncC->nb_subfr; k++ ) {
112         A_Q12      = &PredCoef_Q12[ (( k >> 1 ) | ( 1 - LSF_interpolation_flag )) * MAX_LPC_ORDER ];
113         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER ];
114         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];
115
116         /* Noise shape parameters */
117         SKP_assert( HarmShapeGain_Q14[ k ] >= 0 );
118         HarmShapeFIRPacked_Q14  =                          SKP_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
119         HarmShapeFIRPacked_Q14 |= SKP_LSHIFT( ( opus_int32 )SKP_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
120
121         NSQ->rewhite_flag = 0;
122         if( psIndices->signalType == TYPE_VOICED ) {
123             /* Voiced */
124             lag = pitchL[ k ];
125
126             /* Re-whitening */
127             if( ( k & ( 3 - SKP_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
128                 /* Rewhiten with new A coefs */
129                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
130                 SKP_assert( start_idx > 0 );
131
132                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
133                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder );
134
135                 NSQ->rewhite_flag = 1;
136                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
137             }
138         }
139
140         silk_nsq_scale_states( psEncC, NSQ, x, x_sc_Q10, sLTP, sLTP_Q16, k, LTP_scale_Q14, Gains_Q16, pitchL );
141
142         silk_noise_shape_quantizer( NSQ, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q16, A_Q12, B_Q14,
143             AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], Lambda_Q10,
144             offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder );
145
146         x      += psEncC->subfr_length;
147         pulses += psEncC->subfr_length;
148         pxq    += psEncC->subfr_length;
149     }
150
151     /* Update lagPrev for next frame */
152     NSQ->lagPrev = pitchL[ psEncC->nb_subfr - 1 ];
153
154     /* Save quantized speech and noise shaping signals */
155     SKP_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
156     SKP_memmove( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
157
158 #ifdef SAVE_ALL_INTERNAL_DATA
159     DEBUG_STORE_DATA( xq.dat,       &pxq[ -psEncC->frame_length ],       psEncC->frame_length * sizeof( opus_int16 ) );
160     DEBUG_STORE_DATA( q.dat,        &pulses[ -psEncC->frame_length ],    psEncC->frame_length * sizeof( opus_int8 ) );
161     DEBUG_STORE_DATA( sLTP_Q16.dat, &sLTP_Q16[ psEncC->ltp_mem_length ], psEncC->frame_length * sizeof( opus_int32 ) );
162 #endif
163 }
164
165 /***********************************/
166 /* silk_noise_shape_quantizer  */
167 /***********************************/
168 SKP_INLINE void silk_noise_shape_quantizer(
169     silk_nsq_state  *NSQ,               /* I/O  NSQ state                       */
170     opus_int             signalType,         /* I    Signal type                     */
171     const opus_int32     x_sc_Q10[],         /* I                                    */
172     opus_int8            pulses[],           /* O                                    */
173     opus_int16           xq[],               /* O                                    */
174     opus_int32           sLTP_Q16[],         /* I/O  LTP state                       */
175     const opus_int16     a_Q12[],            /* I    Short term prediction coefs     */
176     const opus_int16     b_Q14[],            /* I    Long term prediction coefs      */
177     const opus_int16     AR_shp_Q13[],       /* I    Noise shaping AR coefs          */
178     opus_int             lag,                /* I    Pitch lag                       */
179     opus_int32           HarmShapeFIRPacked_Q14, /* I                                */
180     opus_int             Tilt_Q14,           /* I    Spectral tilt                   */
181     opus_int32           LF_shp_Q14,         /* I                                    */
182     opus_int32           Gain_Q16,           /* I                                    */
183     opus_int             Lambda_Q10,         /* I                                    */
184     opus_int             offset_Q10,         /* I                                    */
185     opus_int             length,             /* I    Input length                    */
186     opus_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */
187     opus_int             predictLPCOrder     /* I    Prediction filter order         */
188 )
189 {
190     opus_int     i, j;
191     opus_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14;
192     opus_int32   n_LF_Q10, r_Q10, rr_Q10, q1_Q10, q2_Q10, rd1_Q10, rd2_Q10;
193     opus_int32   dither, exc_Q10, LPC_exc_Q10, xq_Q10;
194     opus_int32   tmp1, tmp2, sLF_AR_shp_Q10;
195     opus_int32   *psLPC_Q14, *shp_lag_ptr, *pred_lag_ptr;
196
197     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
198     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
199
200     /* Setup short term AR state */
201     psLPC_Q14 = &NSQ->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 ];
202
203     for( i = 0; i < length; i++ ) {
204         /* Generate dither */
205         NSQ->rand_seed = SKP_RAND( NSQ->rand_seed );
206
207         /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */
208         dither = SKP_RSHIFT( NSQ->rand_seed, 31 );
209
210         /* Short-term prediction */
211         SKP_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */
212         SKP_assert( ( (opus_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */
213         SKP_assert( predictLPCOrder >= 10 );            /* check that unrolling works */
214
215         /* Partially unrolled */
216         LPC_pred_Q10 = SKP_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );
217         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
218         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
219         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
220         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
221         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
222         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
223         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
224         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
225         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
226         for( j = 10; j < predictLPCOrder; j ++ ) {
227             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );
228         }
229
230         /* Long-term prediction */
231         if( signalType == TYPE_VOICED ) {
232             /* Unrolled loop */
233             LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );
234             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
235             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
236             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
237             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
238             pred_lag_ptr++;
239         } else {
240             LTP_pred_Q14 = 0;
241         }
242
243         /* Noise shape feedback */
244         SKP_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
245         tmp2 = psLPC_Q14[ 0 ];
246         tmp1 = NSQ->sAR2_Q14[ 0 ];
247         NSQ->sAR2_Q14[ 0 ] = tmp2;
248         n_AR_Q10 = SKP_SMULWB( tmp2, AR_shp_Q13[ 0 ] );
249         for( j = 2; j < shapingLPCOrder; j += 2 ) {
250             tmp2 = NSQ->sAR2_Q14[ j - 1 ];
251             NSQ->sAR2_Q14[ j - 1 ] = tmp1;
252             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );
253             tmp1 = NSQ->sAR2_Q14[ j + 0 ];
254             NSQ->sAR2_Q14[ j + 0 ] = tmp2;
255             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );
256         }
257         NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
258         n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
259
260         n_AR_Q10 = SKP_RSHIFT( n_AR_Q10, 1 );   /* Q11 -> Q10 */
261         n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );
262
263         n_LF_Q10 = SKP_LSHIFT( SKP_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 );
264         n_LF_Q10 = SKP_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );
265
266         SKP_assert( lag > 0 || signalType != TYPE_VOICED );
267
268         /* Long-term shaping */
269         if( lag > 0 ) {
270             /* Symmetric, packed FIR coefficients */
271             n_LTP_Q14 = SKP_SMULWB( SKP_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
272             n_LTP_Q14 = SKP_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );
273             n_LTP_Q14 = SKP_LSHIFT( n_LTP_Q14, 6 );
274             shp_lag_ptr++;
275
276             tmp1 = SKP_SUB32( LTP_pred_Q14, n_LTP_Q14 );                        /* Add Q14 stuff */
277             tmp1 = SKP_RSHIFT( tmp1, 4 );                                       /* convert to Q10  */
278             tmp1 = SKP_ADD32( tmp1, LPC_pred_Q10 );                             /* add Q10 stuff */
279             tmp1 = SKP_SUB32( tmp1, n_AR_Q10 );                                 /* subtract Q10 stuff */
280         } else {
281             tmp1 = SKP_SUB32( LPC_pred_Q10, n_AR_Q10 );                         /* subtract Q10 stuff */
282         }
283
284         /* Input minus prediction plus noise feedback  */
285         //r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP;
286         tmp1  = SKP_SUB32( tmp1, n_LF_Q10 );                                    /* subtract Q10 stuff */
287         r_Q10 = SKP_SUB32( x_sc_Q10[ i ], tmp1 );
288
289         /* Flip sign depending on dither */
290         r_Q10 = r_Q10 ^ dither;
291         r_Q10 = SKP_LIMIT_32( r_Q10, -31 << 10, 30 << 10 );
292
293         /* Find two quantization level candidates and measure their rate-distortion */
294         q1_Q10 = SKP_SUB32( r_Q10, offset_Q10 );
295         q1_Q10 = SKP_RSHIFT( q1_Q10, 10 );
296         if( q1_Q10 > 0 ) {
297             q1_Q10  = SKP_SUB32( SKP_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
298             q1_Q10  = SKP_ADD32( q1_Q10, offset_Q10 );
299             q2_Q10  = SKP_ADD32( q1_Q10, 1024 );
300             rd1_Q10 = SKP_SMULBB( q1_Q10, Lambda_Q10 );
301             rd2_Q10 = SKP_SMULBB( q2_Q10, Lambda_Q10 );
302         } else if( q1_Q10 == 0 ) {
303             q1_Q10  = offset_Q10;
304             q2_Q10  = SKP_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
305             rd1_Q10 = SKP_SMULBB( q1_Q10, Lambda_Q10 );
306             rd2_Q10 = SKP_SMULBB( q2_Q10, Lambda_Q10 );
307         } else if( q1_Q10 == -1 ) {
308             q2_Q10  = offset_Q10;
309             q1_Q10  = SKP_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
310             rd1_Q10 = SKP_SMULBB( -q1_Q10, Lambda_Q10 );
311             rd2_Q10 = SKP_SMULBB(  q2_Q10, Lambda_Q10 );
312         } else {            /* Q1_Q10 < -1 */
313             q1_Q10  = SKP_ADD32( SKP_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
314             q1_Q10  = SKP_ADD32( q1_Q10, offset_Q10 );
315             q2_Q10  = SKP_ADD32( q1_Q10, 1024 );
316             rd1_Q10 = SKP_SMULBB( -q1_Q10, Lambda_Q10 );
317             rd2_Q10 = SKP_SMULBB( -q2_Q10, Lambda_Q10 );
318         }
319         rr_Q10  = SKP_SUB32( r_Q10, q1_Q10 );
320         rd1_Q10 = SKP_RSHIFT( SKP_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
321         rr_Q10  = SKP_SUB32( r_Q10, q2_Q10 );
322         rd2_Q10 = SKP_RSHIFT( SKP_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
323
324         if( rd2_Q10 < rd1_Q10 ) {
325             q1_Q10 = q2_Q10;
326         }
327
328         pulses[ i ] = ( opus_int8 )SKP_RSHIFT_ROUND( q1_Q10, 10 );
329
330         /* Excitation */
331         exc_Q10 = q1_Q10 ^ dither;
332
333         /* Add predictions */
334         LPC_exc_Q10 = SKP_ADD32( exc_Q10, SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );
335         xq_Q10      = SKP_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
336
337         /* Scale XQ back to normal level before saving */
338         xq[ i ] = ( opus_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( xq_Q10, Gain_Q16 ), 10 ) );
339
340         /* Update states */
341         psLPC_Q14++;
342         *psLPC_Q14 = SKP_LSHIFT( xq_Q10, 4 );
343         sLF_AR_shp_Q10 = SKP_SUB32( xq_Q10, n_AR_Q10 );
344         NSQ->sLF_AR_shp_Q12 = SKP_LSHIFT( sLF_AR_shp_Q10, 2 );
345
346         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx ] = SKP_SUB32( sLF_AR_shp_Q10, n_LF_Q10 );
347         sLTP_Q16[ NSQ->sLTP_buf_idx ] = SKP_LSHIFT( LPC_exc_Q10, 6 );
348         NSQ->sLTP_shp_buf_idx++;
349         NSQ->sLTP_buf_idx++;
350
351         /* Make dither dependent on quantized signal */
352         NSQ->rand_seed += pulses[ i ];
353     }
354
355     /* Update LPC synth buffer */
356     SKP_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
357 }
358
359 SKP_INLINE void silk_nsq_scale_states(
360     const silk_encoder_state *psEncC,       /* I    Encoder State                   */
361     silk_nsq_state      *NSQ,               /* I/O  NSQ state                       */
362     const opus_int16     x[],                /* I    input in Q0                     */
363     opus_int32           x_sc_Q10[],         /* O    input scaled with 1/Gain        */
364     const opus_int16     sLTP[],             /* I    re-whitened LTP state in Q0     */
365     opus_int32           sLTP_Q16[],         /* O    LTP state matching scaled input */
366     opus_int             subfr,              /* I    subframe number                 */
367     const opus_int       LTP_scale_Q14,      /* I                                    */
368     const opus_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                             */
369     const opus_int       pitchL[ MAX_NB_SUBFR ]  /* I                                */
370 )
371 {
372     opus_int   i, lag;
373     opus_int32 inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;
374
375     inv_gain_Q16 = silk_INVERSE32_varQ( SKP_max( Gains_Q16[ subfr ], 1 ), 32 );
376     inv_gain_Q16 = SKP_min( inv_gain_Q16, SKP_int16_MAX );
377     lag          = pitchL[ subfr ];
378
379     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
380     if( NSQ->rewhite_flag ) {
381         inv_gain_Q32 = SKP_LSHIFT( inv_gain_Q16, 16 );
382         if( subfr == 0 ) {
383             /* Do LTP downscaling */
384             inv_gain_Q32 = SKP_LSHIFT( SKP_SMULWB( inv_gain_Q32, LTP_scale_Q14 ), 2 );
385         }
386         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
387             SKP_assert( i < MAX_FRAME_LENGTH );
388             sLTP_Q16[ i ] = SKP_SMULWB( inv_gain_Q32, sLTP[ i ] );
389         }
390     }
391
392     /* Adjust for changing gain */
393     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {
394         gain_adj_Q16 = silk_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );
395
396         /* Scale long-term shaping state */
397         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
398             NSQ->sLTP_shp_Q10[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );
399         }
400
401         /* Scale long-term prediction state */
402         if( NSQ->rewhite_flag == 0 ) {
403             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
404                 sLTP_Q16[ i ] = SKP_SMULWW( gain_adj_Q16, sLTP_Q16[ i ] );
405             }
406         }
407
408         NSQ->sLF_AR_shp_Q12 = SKP_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q12 );
409
410         /* Scale short-term prediction and shaping states */
411         for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
412             NSQ->sLPC_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );
413         }
414         for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
415             NSQ->sAR2_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );
416         }
417     }
418
419     /* Scale input */
420     for( i = 0; i < psEncC->subfr_length; i++ ) {
421         x_sc_Q10[ i ] = SKP_RSHIFT( SKP_SMULBB( x[ i ], ( opus_int16 )inv_gain_Q16 ), 6 );
422     }
423
424     /* save inv_gain */
425     SKP_assert( inv_gain_Q16 != 0 );
426     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;
427 }