Improves the accuracy such that it matches a float decoder much better
[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_Q3[],                 /* I    input in Q3                     */
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_Q3[],                                     /* 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_Q31 != 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_Q3, 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_Q3   += 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     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[ psEncC->ltp_mem_length ], psEncC->frame_length * sizeof( opus_int16 ) ) */
161     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
162     silk_memmove( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
163 }
164
165 /***********************************/
166 /* silk_noise_shape_quantizer  */
167 /***********************************/
168 static 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_Q15[],             /* 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_Q13, 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_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
199
200     /* Set up 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 = silk_RAND( NSQ->rand_seed );
206
207         /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */
208         dither = silk_RSHIFT( NSQ->rand_seed, 31 );
209
210         /* Short-term prediction */
211         silk_assert( predictLPCOrder == 10 || predictLPCOrder == 16 );
212         /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
213         LPC_pred_Q10 = silk_RSHIFT( predictLPCOrder, 1 );
214         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[  0 ], a_Q12[ 0 ] );
215         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
216         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
217         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
218         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
219         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
220         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
221         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
222         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
223         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
224         if( predictLPCOrder == 16 ) {
225             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -10 ], a_Q12[ 10 ] );
226             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -11 ], a_Q12[ 11 ] );
227             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -12 ], a_Q12[ 12 ] );
228             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -13 ], a_Q12[ 13 ] );
229             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -14 ], a_Q12[ 14 ] );
230             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -15 ], a_Q12[ 15 ] );
231         }
232
233         /* Long-term prediction */
234         if( signalType == TYPE_VOICED ) {
235             /* Unrolled loop */
236             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
237             LTP_pred_Q13 = 2;
238             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
239             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
240             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
241             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
242             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
243             pred_lag_ptr++;
244         } else {
245             LTP_pred_Q13 = 0;
246         }
247
248         /* Noise shape feedback */
249         silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
250         tmp2 = psLPC_Q14[ 0 ];
251         tmp1 = NSQ->sAR2_Q14[ 0 ];
252         NSQ->sAR2_Q14[ 0 ] = tmp2;
253         n_AR_Q10 = silk_SMULWB( tmp2, AR_shp_Q13[ 0 ] );
254         for( j = 2; j < shapingLPCOrder; j += 2 ) {
255             tmp2 = NSQ->sAR2_Q14[ j - 1 ];
256             NSQ->sAR2_Q14[ j - 1 ] = tmp1;
257             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );
258             tmp1 = NSQ->sAR2_Q14[ j + 0 ];
259             NSQ->sAR2_Q14[ j + 0 ] = tmp2;
260             n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );
261         }
262         NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
263         n_AR_Q10 = silk_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
264
265         n_AR_Q10 = silk_RSHIFT( n_AR_Q10, 1 );   /* Q11 -> Q10 */
266         n_AR_Q10 = silk_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );
267
268         n_LF_Q10 = silk_LSHIFT( silk_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 );
269         n_LF_Q10 = silk_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );
270
271         silk_assert( lag > 0 || signalType != TYPE_VOICED );
272
273         /* Long-term shaping */
274         if( lag > 0 ) {
275             /* Symmetric, packed FIR coefficients */
276             n_LTP_Q14 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
277             n_LTP_Q14 = silk_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
278             n_LTP_Q14 = silk_LSHIFT( n_LTP_Q14, 6 );
279             shp_lag_ptr++;
280
281             tmp1 = silk_SUB32( silk_LSHIFT32( LTP_pred_Q13, 1 ), n_LTP_Q14 );                   /* Add Q14 stuff */
282             tmp1 = silk_RSHIFT( tmp1, 4 );                                       /* convert to Q10  */
283             tmp1 = silk_ADD32( tmp1, LPC_pred_Q10 );                             /* add Q10 stuff */
284             tmp1 = silk_SUB32( tmp1, n_AR_Q10 );                                 /* subtract Q10 stuff */
285         } else {
286             tmp1 = silk_SUB32( LPC_pred_Q10, n_AR_Q10 );                         /* subtract Q10 stuff */
287         }
288
289         /* Input minus prediction plus noise feedback  */
290         /*r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP;*/
291         tmp1  = silk_SUB32( tmp1, n_LF_Q10 );                                    /* subtract Q10 stuff */
292         r_Q10 = silk_SUB32( x_sc_Q10[ i ], tmp1 );
293
294         /* Flip sign depending on dither */
295         r_Q10 = r_Q10 ^ dither;
296         r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
297
298         /* Find two quantization level candidates and measure their rate-distortion */
299         q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
300         q1_Q10 = silk_RSHIFT( q1_Q10, 10 );
301         if( q1_Q10 > 0 ) {
302             q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
303             q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
304             q2_Q10  = silk_ADD32( q1_Q10, 1024 );
305             rd1_Q10 = silk_SMULBB( q1_Q10, Lambda_Q10 );
306             rd2_Q10 = silk_SMULBB( q2_Q10, Lambda_Q10 );
307         } else if( q1_Q10 == 0 ) {
308             q1_Q10  = offset_Q10;
309             q2_Q10  = silk_ADD32( q1_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 if( q1_Q10 == -1 ) {
313             q2_Q10  = offset_Q10;
314             q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
315             rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
316             rd2_Q10 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
317         } else {            /* Q1_Q10 < -1 */
318             q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );
319             q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
320             q2_Q10  = silk_ADD32( q1_Q10, 1024 );
321             rd1_Q10 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
322             rd2_Q10 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
323         }
324         rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
325         rd1_Q10 = silk_RSHIFT( silk_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );
326         rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
327         rd2_Q10 = silk_RSHIFT( silk_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );
328
329         if( rd2_Q10 < rd1_Q10 ) {
330             q1_Q10 = q2_Q10;
331         }
332
333         pulses[ i ] = (opus_int8)silk_RSHIFT_ROUND( q1_Q10, 10 );
334
335         /* Excitation */
336         exc_Q10 = q1_Q10 ^ dither;
337
338         /* Add predictions */
339         LPC_exc_Q10 = silk_ADD32( exc_Q10, silk_RSHIFT_ROUND( LTP_pred_Q13, 3 ) );
340         xq_Q10      = silk_ADD32( LPC_exc_Q10, LPC_pred_Q10 );
341
342         /* Scale XQ back to normal level before saving */
343         xq[ i ] = ( opus_int16 )silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( xq_Q10, Gain_Q16 ), 10 ) );
344
345         /* Update states */
346         psLPC_Q14++;
347         *psLPC_Q14 = silk_LSHIFT( xq_Q10, 4 );
348         sLF_AR_shp_Q10 = silk_SUB32( xq_Q10, n_AR_Q10 );
349         NSQ->sLF_AR_shp_Q12 = silk_LSHIFT( sLF_AR_shp_Q10, 2 );
350
351         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx ] = silk_SUB32( sLF_AR_shp_Q10, n_LF_Q10 );
352         sLTP_Q15[ NSQ->sLTP_buf_idx ] = silk_LSHIFT( LPC_exc_Q10, 5 );
353         NSQ->sLTP_shp_buf_idx++;
354         NSQ->sLTP_buf_idx++;
355
356         /* Make dither dependent on quantized signal */
357         NSQ->rand_seed = silk_ADD32_ovflw(NSQ->rand_seed, pulses[ i ]);
358     }
359
360     /* Update LPC synth buffer */
361     silk_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
362 }
363
364 static inline void silk_nsq_scale_states(
365     const silk_encoder_state *psEncC,           /* I    Encoder State                   */
366     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                       */
367     const opus_int32    x_Q3[],                 /* I    input in Q3                     */
368     opus_int32          x_sc_Q10[],             /* O    input scaled with 1/Gain        */
369     const opus_int16    sLTP[],                 /* I    re-whitened LTP state in Q0     */
370     opus_int32          sLTP_Q15[],             /* O    LTP state matching scaled input */
371     opus_int            subfr,                  /* I    subframe number                 */
372     const opus_int      LTP_scale_Q14,          /* I                                    */
373     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ], /* I                                 */
374     const opus_int      pitchL[ MAX_NB_SUBFR ], /* I    Pitch lag                       */
375     const opus_int      signal_type             /* I    Signal type                     */
376 )
377 {
378     opus_int   i, lag;
379     opus_int32 gain_adj_Q16, inv_gain_Q31, inv_gain_Q23;
380
381     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
382     lag          = pitchL[ subfr ];
383
384     /* Calculate gain adjustment factor */
385     if( inv_gain_Q31 != NSQ->prev_inv_gain_Q31 ) {
386         gain_adj_Q16 =  silk_DIV32_varQ( inv_gain_Q31, NSQ->prev_inv_gain_Q31, 16 );
387     } else {
388         gain_adj_Q16 = 1 << 16;
389     }
390
391     /* Scale input */
392     inv_gain_Q23 = silk_RSHIFT_ROUND( inv_gain_Q31, 8 );
393     for( i = 0; i < psEncC->subfr_length; i++ ) {
394         x_sc_Q10[ i ] = silk_SMULWW( x_Q3[ i ], inv_gain_Q23 );
395     }
396
397     /* Save inverse gain */
398     silk_assert( inv_gain_Q31 != 0 );
399     NSQ->prev_inv_gain_Q31 = inv_gain_Q31;
400
401     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
402     if( NSQ->rewhite_flag ) {
403         if( subfr == 0 ) {
404             /* Do LTP downscaling */
405             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
406         }
407         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
408             silk_assert( i < MAX_FRAME_LENGTH );
409             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
410         }
411     }
412
413     /* Adjust for changing gain */
414     if( gain_adj_Q16 != 1 << 16 ) {
415         /* Scale long-term shaping state */
416         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
417             NSQ->sLTP_shp_Q10[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );
418         }
419
420         /* Scale long-term prediction state */
421         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
422             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
423                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
424             }
425         }
426
427         NSQ->sLF_AR_shp_Q12 = silk_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q12 );
428
429         /* Scale short-term prediction and shaping states */
430         for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
431             NSQ->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );
432         }
433         for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
434             NSQ->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );
435         }
436     }
437 }