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