760042622c41947122820c1b00b40edd1b1a6fcb
[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_int32    x_Q10[],                /* 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_Q15[],             /* 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_Q15[],             /* 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_int32            x_Q10[],                                    /* 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_Q15[ 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_SUB_FRAME_LENGTH ];
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     /* Set up 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_Q10, x_sc_Q10, sLTP, sLTP_Q15, k, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType );
146
147         silk_noise_shape_quantizer( NSQ, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15, 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_Q10  += 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_Q15[],             /* 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_Q13, 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_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
198
199     /* Set up 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 == 10 || predictLPCOrder == 16 );
211         LPC_pred_Q10 = silk_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );
212         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
213         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
214         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
215         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
216         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
217         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
218         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
219         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
220         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
221         if( predictLPCOrder == 16 ) {
222             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -10 ], a_Q12[ 10 ] );
223             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -11 ], a_Q12[ 11 ] );
224             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -12 ], a_Q12[ 12 ] );
225             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -13 ], a_Q12[ 13 ] );
226             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -14 ], a_Q12[ 14 ] );
227             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -15 ], a_Q12[ 15 ] );
228         }
229
230         /* Long-term prediction */
231         if( signalType == TYPE_VOICED ) {
232             /* Unrolled loop */
233             LTP_pred_Q13 = silk_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );
234             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
235             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
236             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
237             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
238             pred_lag_ptr++;
239         } else {
240             LTP_pred_Q13 = 0;
241         }
242
243         /* Noise shape feedback */
244         silk_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 = silk_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 = silk_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 = silk_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );
256         }
257         NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
258         n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
259
260         n_AR_Q10 = silk_RSHIFT( n_AR_Q10, 1 );   /* Q11 -> Q10 */
261         n_AR_Q10 = silk_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );
262
263         n_LF_Q10 = silk_LSHIFT( silk_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 );
264         n_LF_Q10 = silk_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );
265
266         silk_assert( lag > 0 || signalType != TYPE_VOICED );
267
268         /* Long-term shaping */
269         if( lag > 0 ) {
270             /* Symmetric, packed FIR coefficients */
271             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
272             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
273             n_LTP_Q14 = silk_LSHIFT( n_LTP_Q14, 6 );
274             shp_lag_ptr++;
275
276             tmp1 = silk_SUB32( LTP_pred_Q13 << 1, n_LTP_Q14 );                        /* Add Q14 stuff */
277             tmp1 = silk_RSHIFT( tmp1, 4 );                                       /* convert to Q10  */
278             tmp1 = silk_ADD32( tmp1, LPC_pred_Q10 );                             /* add Q10 stuff */
279             tmp1 = silk_SUB32( tmp1, n_AR_Q10 );                                 /* subtract Q10 stuff */
280         } else {
281             tmp1 = silk_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  = silk_SUB32( tmp1, n_LF_Q10 );                                    /* subtract Q10 stuff */
287         r_Q10 = silk_SUB32( x_sc_Q10[ i ], tmp1 );
288
289         /* Flip sign depending on dither */
290         r_Q10 = r_Q10 ^ dither;
291         r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
292
293         /* Find two quantization level candidates and measure their rate-distortion */
294         q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
295         q1_Q10 = silk_RSHIFT( q1_Q10, 10 );
296         if( q1_Q10 > 0 ) {
297             q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
298             q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
299             q2_Q10  = silk_ADD32( q1_Q10, 1024 );
300             rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
301             rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
302         } else if( q1_Q10 == 0 ) {
303             q1_Q10  = offset_Q10;
304             q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
305             rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
306             rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
307         } else if( q1_Q10 == -1 ) {
308             q2_Q10  = offset_Q10;
309             q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
310             rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
311             rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
312         } else {            /* Q1_Q10 < -1 */
313             q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
314             q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
315             q2_Q10  = silk_ADD32( q1_Q10, 1024 );
316             rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
317             rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
318         }
319         rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
320         rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
321         rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
322         rd2_Q10 = silk_RSHIFT( silk_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)silk_RSHIFT_ROUND( q1_Q10, 10 );
329
330         /* Excitation */
331         exc_Q10 = q1_Q10 ^ dither;
332
333         /* Add predictions */
334         LPC_exc_Q10 = silk_ADD32( exc_Q10, silk_RSHIFT_ROUND( LTP_pred_Q13, 3 ) );
335         xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
336
337         /* Scale XQ back to normal level before saving */
338         xq[ i ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( xq_Q10, Gain_Q16 ), 10 ) );
339
340                 /* DEBUG_STORE_DATA( enc.pcm, &xq[i], sizeof( opus_int16 ) ) */
341         /* Update states */
342         psLPC_Q14++;
343         *psLPC_Q14 = silk_LSHIFT( xq_Q10, 4 );
344         sLF_AR_shp_Q10 = silk_SUB32( xq_Q10, n_AR_Q10 );
345         NSQ->sLF_AR_shp_Q12 = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
346
347         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx ] = silk_SUB32( sLF_AR_shp_Q10, n_LF_Q10 );
348         sLTP_Q15[ NSQ->sLTP_buf_idx ] = silk_LSHIFT( LPC_exc_Q10, 5 );
349         NSQ->sLTP_shp_buf_idx++;
350         NSQ->sLTP_buf_idx++;
351
352         /* Make dither dependent on quantized signal */
353         NSQ->rand_seed = silk_ADD32_ovflw(NSQ->rand_seed, pulses[ i ]);
354     }
355
356     /* Update LPC synth buffer */
357     silk_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
358 }
359
360 static inline void silk_nsq_scale_states(
361     const silk_encoder_state *psEncC,           /* I    Encoder State                   */
362     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                       */
363     const opus_int32    x_Q10[],                /* I    input in Q0                     */
364     opus_int32          x_sc_Q10[],             /* O    input scaled with 1/Gain        */
365     const opus_int16    sLTP[],                 /* I    re-whitened LTP state in Q0     */
366     opus_int32          sLTP_Q15[],             /* O    LTP state matching scaled input */
367     opus_int            subfr,                  /* I    subframe number                 */
368     const opus_int      LTP_scale_Q14,          /* I                                    */
369     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ], /* I                                 */
370     const opus_int      pitchL[ MAX_NB_SUBFR ], /* I    Pitch lag                       */
371     const opus_int      signal_type             /* I    Signal type                     */
372 )
373 {
374     opus_int   i, lag;
375     opus_int32 inv_gain_Q16, gain_adj_Q16, inv_gain_Q31;
376
377     inv_gain_Q16 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 32 );
378     lag          = pitchL[ subfr ];
379
380     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
381     if( NSQ->rewhite_flag ) {
382         inv_gain_Q31 = silk_LSHIFT( inv_gain_Q16, 15 );
383         if( subfr == 0 ) {
384             /* Do LTP downscaling */
385             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
386         }
387         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
388             silk_assert( i < MAX_FRAME_LENGTH );
389             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
390         }
391     }
392
393     /* Adjust for changing gain */
394     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {
395         gain_adj_Q16 = silk_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );
396
397         /* Scale long-term shaping state */
398         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
399             NSQ->sLTP_shp_Q10[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );
400         }
401
402         /* Scale long-term prediction state */
403         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
404             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
405                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
406             }
407         }
408
409         NSQ->sLF_AR_shp_Q12 = silk_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q12 );
410
411         /* Scale short-term prediction and shaping states */
412         for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
413             NSQ->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );
414         }
415         for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
416             NSQ->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );
417         }
418     }
419
420     /* Scale input */
421     for( i = 0; i < psEncC->subfr_length; i++ ) {
422         x_sc_Q10[ i ] = silk_SMULWW( x_Q10[ i ], inv_gain_Q16 );
423     }
424
425     /* Save inv_gain */
426     silk_assert( inv_gain_Q16 != 0 );
427     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;
428 }