a06588407002a4551ccdc244780f3c14091ca799
[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, are permitted provided that the following conditions
5 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 Internet Society, IETF or IETF Trust, nor the
12 names of specific contributors, may be used to endorse or promote
13 products derived from this software without specific prior written
14 permission.
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #ifdef HAVE_CONFIG_H
29 #include "config.h"
30 #endif
31
32 #include "main.h"
33 #include "stack_alloc.h"
34
35 static OPUS_INLINE void silk_nsq_scale_states(
36     const silk_encoder_state *psEncC,           /* I    Encoder State                   */
37     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                       */
38     const opus_int32    x_Q3[],                 /* I    input in Q3                     */
39     opus_int32          x_sc_Q10[],             /* O    input scaled with 1/Gain        */
40     const opus_int16    sLTP[],                 /* I    re-whitened LTP state in Q0     */
41     opus_int32          sLTP_Q15[],             /* O    LTP state matching scaled input */
42     opus_int            subfr,                  /* I    subframe number                 */
43     const opus_int      LTP_scale_Q14,          /* I                                    */
44     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ], /* I                                 */
45     const opus_int      pitchL[ MAX_NB_SUBFR ], /* I    Pitch lag                       */
46     const opus_int      signal_type             /* I    Signal type                     */
47 );
48
49 #if !defined(OPUS_X86_MAY_HAVE_SSE4_1)
50 static OPUS_INLINE void silk_noise_shape_quantizer(
51     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                       */
52     opus_int            signalType,             /* I    Signal type                     */
53     const opus_int32    x_sc_Q10[],             /* I                                    */
54     opus_int8           pulses[],               /* O                                    */
55     opus_int16          xq[],                   /* O                                    */
56     opus_int32          sLTP_Q15[],             /* I/O  LTP state                       */
57     const opus_int16    a_Q12[],                /* I    Short term prediction coefs     */
58     const opus_int16    b_Q14[],                /* I    Long term prediction coefs      */
59     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping AR coefs          */
60     opus_int            lag,                    /* I    Pitch lag                       */
61     opus_int32          HarmShapeFIRPacked_Q14, /* I                                    */
62     opus_int            Tilt_Q14,               /* I    Spectral tilt                   */
63     opus_int32          LF_shp_Q14,             /* I                                    */
64     opus_int32          Gain_Q16,               /* I                                    */
65     opus_int            Lambda_Q10,             /* I                                    */
66     opus_int            offset_Q10,             /* I                                    */
67     opus_int            length,                 /* I    Input length                    */
68     opus_int            shapingLPCOrder,        /* I    Noise shaping AR filter order   */
69     opus_int            predictLPCOrder         /* I    Prediction filter order         */
70 );
71 #endif
72
73 void silk_NSQ_c
74 (
75     const silk_encoder_state    *psEncC,                                    /* I/O  Encoder State                   */
76     silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                       */
77     SideInfoIndices             *psIndices,                                 /* I/O  Quantization Indices            */
78     const opus_int32            x_Q3[],                                     /* I    Prefiltered input signal        */
79     opus_int8                   pulses[],                                   /* O    Quantized pulse signal          */
80     const opus_int16            PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefs     */
81     const opus_int16            LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefs      */
82     const opus_int16            AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I Noise shaping coefs             */
83     const opus_int              HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I    Long term shaping coefs         */
84     const opus_int              Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                   */
85     const opus_int32            LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I    Low frequency shaping coefs     */
86     const opus_int32            Gains_Q16[ MAX_NB_SUBFR ],                  /* I    Quantization step sizes         */
87     const opus_int              pitchL[ MAX_NB_SUBFR ],                     /* I    Pitch lags                      */
88     const opus_int              Lambda_Q10,                                 /* I    Rate/distortion tradeoff        */
89     const opus_int              LTP_scale_Q14                               /* I    LTP state scaling               */
90 )
91 {
92     opus_int            k, lag, start_idx, LSF_interpolation_flag;
93     const opus_int16    *A_Q12, *B_Q14, *AR_shp_Q13;
94     opus_int16          *pxq;
95     VARDECL( opus_int32, sLTP_Q15 );
96     VARDECL( opus_int16, sLTP );
97     opus_int32          HarmShapeFIRPacked_Q14;
98     opus_int            offset_Q10;
99     VARDECL( opus_int32, x_sc_Q10 );
100     SAVE_STACK;
101
102     NSQ->rand_seed = psIndices->Seed;
103
104     /* Set unvoiced lag to the previous one, overwrite later for voiced */
105     lag = NSQ->lagPrev;
106
107     silk_assert( NSQ->prev_gain_Q16 != 0 );
108
109     offset_Q10 = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];
110
111     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {
112         LSF_interpolation_flag = 0;
113     } else {
114         LSF_interpolation_flag = 1;
115     }
116
117     ALLOC( sLTP_Q15,
118            psEncC->ltp_mem_length + psEncC->frame_length, opus_int32 );
119     ALLOC( sLTP, psEncC->ltp_mem_length + psEncC->frame_length, opus_int16 );
120     ALLOC( x_sc_Q10, psEncC->subfr_length, opus_int32 );
121     /* Set up pointers to start of sub frame */
122     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;
123     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;
124     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];
125     for( k = 0; k < psEncC->nb_subfr; k++ ) {
126         A_Q12      = &PredCoef_Q12[ (( k >> 1 ) | ( 1 - LSF_interpolation_flag )) * MAX_LPC_ORDER ];
127         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER ];
128         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];
129
130         /* Noise shape parameters */
131         silk_assert( HarmShapeGain_Q14[ k ] >= 0 );
132         HarmShapeFIRPacked_Q14  =                          silk_RSHIFT( HarmShapeGain_Q14[ k ], 2 );
133         HarmShapeFIRPacked_Q14 |= silk_LSHIFT( (opus_int32)silk_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );
134
135         NSQ->rewhite_flag = 0;
136         if( psIndices->signalType == TYPE_VOICED ) {
137             /* Voiced */
138             lag = pitchL[ k ];
139
140             /* Re-whitening */
141             if( ( k & ( 3 - silk_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {
142                 /* Rewhiten with new A coefs */
143                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;
144                 silk_assert( start_idx > 0 );
145
146                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ],
147                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder, psEncC->arch );
148
149                 NSQ->rewhite_flag = 1;
150                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;
151             }
152         }
153
154         silk_nsq_scale_states( psEncC, NSQ, x_Q3, x_sc_Q10, sLTP, sLTP_Q15, k, LTP_scale_Q14, Gains_Q16, pitchL, psIndices->signalType );
155
156         silk_noise_shape_quantizer( NSQ, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q15, A_Q12, B_Q14,
157             AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], Lambda_Q10,
158             offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder );
159
160         x_Q3   += psEncC->subfr_length;
161         pulses += psEncC->subfr_length;
162         pxq    += psEncC->subfr_length;
163     }
164
165     /* Update lagPrev for next frame */
166     NSQ->lagPrev = pitchL[ psEncC->nb_subfr - 1 ];
167
168     /* Save quantized speech and noise shaping signals */
169     /* DEBUG_STORE_DATA( enc.pcm, &NSQ->xq[ psEncC->ltp_mem_length ], psEncC->frame_length * sizeof( opus_int16 ) ) */
170     silk_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int16 ) );
171     silk_memmove( NSQ->sLTP_shp_Q14, &NSQ->sLTP_shp_Q14[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( opus_int32 ) );
172     RESTORE_STACK;
173 }
174
175 /***********************************/
176 /* silk_noise_shape_quantizer  */
177 /***********************************/
178
179 #if !defined(OPUS_X86_MAY_HAVE_SSE4_1)
180 static OPUS_INLINE
181 #endif
182 void silk_noise_shape_quantizer(
183     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                       */
184     opus_int            signalType,             /* I    Signal type                     */
185     const opus_int32    x_sc_Q10[],             /* I                                    */
186     opus_int8           pulses[],               /* O                                    */
187     opus_int16          xq[],                   /* O                                    */
188     opus_int32          sLTP_Q15[],             /* I/O  LTP state                       */
189     const opus_int16    a_Q12[],                /* I    Short term prediction coefs     */
190     const opus_int16    b_Q14[],                /* I    Long term prediction coefs      */
191     const opus_int16    AR_shp_Q13[],           /* I    Noise shaping AR coefs          */
192     opus_int            lag,                    /* I    Pitch lag                       */
193     opus_int32          HarmShapeFIRPacked_Q14, /* I                                    */
194     opus_int            Tilt_Q14,               /* I    Spectral tilt                   */
195     opus_int32          LF_shp_Q14,             /* I                                    */
196     opus_int32          Gain_Q16,               /* I                                    */
197     opus_int            Lambda_Q10,             /* I                                    */
198     opus_int            offset_Q10,             /* I                                    */
199     opus_int            length,                 /* I    Input length                    */
200     opus_int            shapingLPCOrder,        /* I    Noise shaping AR filter order   */
201     opus_int            predictLPCOrder         /* I    Prediction filter order         */
202 )
203 {
204     opus_int     i, j;
205     opus_int32   LTP_pred_Q13, LPC_pred_Q10, n_AR_Q12, n_LTP_Q13;
206     opus_int32   n_LF_Q12, r_Q10, rr_Q10, q1_Q0, q1_Q10, q2_Q10, rd1_Q20, rd2_Q20;
207     opus_int32   exc_Q14, LPC_exc_Q14, xq_Q14, Gain_Q10;
208     opus_int32   tmp1, tmp2, sLF_AR_shp_Q14;
209     opus_int32   *psLPC_Q14, *shp_lag_ptr, *pred_lag_ptr;
210
211     shp_lag_ptr  = &NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];
212     pred_lag_ptr = &sLTP_Q15[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];
213     Gain_Q10     = silk_RSHIFT( Gain_Q16, 6 );
214
215     /* Set up short term AR state */
216     psLPC_Q14 = &NSQ->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 ];
217
218     for( i = 0; i < length; i++ ) {
219         /* Generate dither */
220         NSQ->rand_seed = silk_RAND( NSQ->rand_seed );
221
222         /* Short-term prediction */
223         silk_assert( predictLPCOrder == 10 || predictLPCOrder == 16 );
224         /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
225         LPC_pred_Q10 = silk_RSHIFT( predictLPCOrder, 1 );
226         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[  0 ], a_Q12[ 0 ] );
227         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );
228         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );
229         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );
230         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );
231         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );
232         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );
233         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );
234         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );
235         LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );
236         if( predictLPCOrder == 16 ) {
237             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -10 ], a_Q12[ 10 ] );
238             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -11 ], a_Q12[ 11 ] );
239             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -12 ], a_Q12[ 12 ] );
240             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -13 ], a_Q12[ 13 ] );
241             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -14 ], a_Q12[ 14 ] );
242             LPC_pred_Q10 = silk_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -15 ], a_Q12[ 15 ] );
243         }
244
245         /* Long-term prediction */
246         if( signalType == TYPE_VOICED ) {
247             /* Unrolled loop */
248             /* Avoids introducing a bias because silk_SMLAWB() always rounds to -inf */
249             LTP_pred_Q13 = 2;
250             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[  0 ], b_Q14[ 0 ] );
251             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );
252             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );
253             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );
254             LTP_pred_Q13 = silk_SMLAWB( LTP_pred_Q13, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );
255             pred_lag_ptr++;
256         } else {
257             LTP_pred_Q13 = 0;
258         }
259
260         /* Noise shape feedback */
261         silk_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */
262         tmp2 = psLPC_Q14[ 0 ];
263         tmp1 = NSQ->sAR2_Q14[ 0 ];
264         NSQ->sAR2_Q14[ 0 ] = tmp2;
265         n_AR_Q12 = silk_RSHIFT( shapingLPCOrder, 1 );
266         n_AR_Q12 = silk_SMLAWB( n_AR_Q12, tmp2, AR_shp_Q13[ 0 ] );
267         for( j = 2; j < shapingLPCOrder; j += 2 ) {
268             tmp2 = NSQ->sAR2_Q14[ j - 1 ];
269             NSQ->sAR2_Q14[ j - 1 ] = tmp1;
270             n_AR_Q12 = silk_SMLAWB( n_AR_Q12, tmp1, AR_shp_Q13[ j - 1 ] );
271             tmp1 = NSQ->sAR2_Q14[ j + 0 ];
272             NSQ->sAR2_Q14[ j + 0 ] = tmp2;
273             n_AR_Q12 = silk_SMLAWB( n_AR_Q12, tmp2, AR_shp_Q13[ j ] );
274         }
275         NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;
276         n_AR_Q12 = silk_SMLAWB( n_AR_Q12, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );
277
278         n_AR_Q12 = silk_LSHIFT32( n_AR_Q12, 1 );                                /* Q11 -> Q12 */
279         n_AR_Q12 = silk_SMLAWB( n_AR_Q12, NSQ->sLF_AR_shp_Q14, Tilt_Q14 );
280
281         n_LF_Q12 = silk_SMULWB( NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 );
282         n_LF_Q12 = silk_SMLAWT( n_LF_Q12, NSQ->sLF_AR_shp_Q14, LF_shp_Q14 );
283
284         silk_assert( lag > 0 || signalType != TYPE_VOICED );
285
286         /* Combine prediction and noise shaping signals */
287         tmp1 = silk_SUB32( silk_LSHIFT32( LPC_pred_Q10, 2 ), n_AR_Q12 );        /* Q12 */
288         tmp1 = silk_SUB32( tmp1, n_LF_Q12 );                                    /* Q12 */
289         if( lag > 0 ) {
290             /* Symmetric, packed FIR coefficients */
291             n_LTP_Q13 = silk_SMULWB( silk_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );
292             n_LTP_Q13 = silk_SMLAWT( n_LTP_Q13, shp_lag_ptr[ -1 ],                      HarmShapeFIRPacked_Q14 );
293             n_LTP_Q13 = silk_LSHIFT( n_LTP_Q13, 1 );
294             shp_lag_ptr++;
295
296             tmp2 = silk_SUB32( LTP_pred_Q13, n_LTP_Q13 );                       /* Q13 */
297             tmp1 = silk_ADD_LSHIFT32( tmp2, tmp1, 1 );                          /* Q13 */
298             tmp1 = silk_RSHIFT_ROUND( tmp1, 3 );                                /* Q10 */
299         } else {
300             tmp1 = silk_RSHIFT_ROUND( tmp1, 2 );                                /* Q10 */
301         }
302
303         r_Q10 = silk_SUB32( x_sc_Q10[ i ], tmp1 );                              /* residual error Q10 */
304
305         /* Flip sign depending on dither */
306         if ( NSQ->rand_seed < 0 ) {
307            r_Q10 = -r_Q10;
308         }
309         r_Q10 = silk_LIMIT_32( r_Q10, -(31 << 10), 30 << 10 );
310
311         /* Find two quantization level candidates and measure their rate-distortion */
312         q1_Q10 = silk_SUB32( r_Q10, offset_Q10 );
313         q1_Q0 = silk_RSHIFT( q1_Q10, 10 );
314         if( q1_Q0 > 0 ) {
315             q1_Q10  = silk_SUB32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
316             q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
317             q2_Q10  = silk_ADD32( q1_Q10, 1024 );
318             rd1_Q20 = silk_SMULBB( q1_Q10, Lambda_Q10 );
319             rd2_Q20 = silk_SMULBB( q2_Q10, Lambda_Q10 );
320         } else if( q1_Q0 == 0 ) {
321             q1_Q10  = offset_Q10;
322             q2_Q10  = silk_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
323             rd1_Q20 = silk_SMULBB( q1_Q10, Lambda_Q10 );
324             rd2_Q20 = silk_SMULBB( q2_Q10, Lambda_Q10 );
325         } else if( q1_Q0 == -1 ) {
326             q2_Q10  = offset_Q10;
327             q1_Q10  = silk_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );
328             rd1_Q20 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
329             rd2_Q20 = silk_SMULBB(  q2_Q10, Lambda_Q10 );
330         } else {            /* Q1_Q0 < -1 */
331             q1_Q10  = silk_ADD32( silk_LSHIFT( q1_Q0, 10 ), QUANT_LEVEL_ADJUST_Q10 );
332             q1_Q10  = silk_ADD32( q1_Q10, offset_Q10 );
333             q2_Q10  = silk_ADD32( q1_Q10, 1024 );
334             rd1_Q20 = silk_SMULBB( -q1_Q10, Lambda_Q10 );
335             rd2_Q20 = silk_SMULBB( -q2_Q10, Lambda_Q10 );
336         }
337         rr_Q10  = silk_SUB32( r_Q10, q1_Q10 );
338         rd1_Q20 = silk_SMLABB( rd1_Q20, rr_Q10, rr_Q10 );
339         rr_Q10  = silk_SUB32( r_Q10, q2_Q10 );
340         rd2_Q20 = silk_SMLABB( rd2_Q20, rr_Q10, rr_Q10 );
341
342         if( rd2_Q20 < rd1_Q20 ) {
343             q1_Q10 = q2_Q10;
344         }
345
346         pulses[ i ] = (opus_int8)silk_RSHIFT_ROUND( q1_Q10, 10 );
347
348         /* Excitation */
349         exc_Q14 = silk_LSHIFT( q1_Q10, 4 );
350         if ( NSQ->rand_seed < 0 ) {
351            exc_Q14 = -exc_Q14;
352         }
353
354         /* Add predictions */
355         LPC_exc_Q14 = silk_ADD_LSHIFT32( exc_Q14, LTP_pred_Q13, 1 );
356         xq_Q14      = silk_ADD_LSHIFT32( LPC_exc_Q14, LPC_pred_Q10, 4 );
357
358         /* Scale XQ back to normal level before saving */
359         xq[ i ] = (opus_int16)silk_SAT16( silk_RSHIFT_ROUND( silk_SMULWW( xq_Q14, Gain_Q10 ), 8 ) );
360
361         /* Update states */
362         psLPC_Q14++;
363         *psLPC_Q14 = xq_Q14;
364         sLF_AR_shp_Q14 = silk_SUB_LSHIFT32( xq_Q14, n_AR_Q12, 2 );
365         NSQ->sLF_AR_shp_Q14 = sLF_AR_shp_Q14;
366
367         NSQ->sLTP_shp_Q14[ NSQ->sLTP_shp_buf_idx ] = silk_SUB_LSHIFT32( sLF_AR_shp_Q14, n_LF_Q12, 2 );
368         sLTP_Q15[ NSQ->sLTP_buf_idx ] = silk_LSHIFT( LPC_exc_Q14, 1 );
369         NSQ->sLTP_shp_buf_idx++;
370         NSQ->sLTP_buf_idx++;
371
372         /* Make dither dependent on quantized signal */
373         NSQ->rand_seed = silk_ADD32_ovflw( NSQ->rand_seed, pulses[ i ] );
374     }
375
376     /* Update LPC synth buffer */
377     silk_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( opus_int32 ) );
378 }
379
380 static OPUS_INLINE void silk_nsq_scale_states(
381     const silk_encoder_state *psEncC,           /* I    Encoder State                   */
382     silk_nsq_state      *NSQ,                   /* I/O  NSQ state                       */
383     const opus_int32    x_Q3[],                 /* I    input in Q3                     */
384     opus_int32          x_sc_Q10[],             /* O    input scaled with 1/Gain        */
385     const opus_int16    sLTP[],                 /* I    re-whitened LTP state in Q0     */
386     opus_int32          sLTP_Q15[],             /* O    LTP state matching scaled input */
387     opus_int            subfr,                  /* I    subframe number                 */
388     const opus_int      LTP_scale_Q14,          /* I                                    */
389     const opus_int32    Gains_Q16[ MAX_NB_SUBFR ], /* I                                 */
390     const opus_int      pitchL[ MAX_NB_SUBFR ], /* I    Pitch lag                       */
391     const opus_int      signal_type             /* I    Signal type                     */
392 )
393 {
394     opus_int   i, lag;
395     opus_int32 gain_adj_Q16, inv_gain_Q31, inv_gain_Q23;
396
397     lag          = pitchL[ subfr ];
398     inv_gain_Q31 = silk_INVERSE32_varQ( silk_max( Gains_Q16[ subfr ], 1 ), 47 );
399     silk_assert( inv_gain_Q31 != 0 );
400
401     /* Calculate gain adjustment factor */
402     if( Gains_Q16[ subfr ] != NSQ->prev_gain_Q16 ) {
403         gain_adj_Q16 =  silk_DIV32_varQ( NSQ->prev_gain_Q16, Gains_Q16[ subfr ], 16 );
404     } else {
405         gain_adj_Q16 = (opus_int32)1 << 16;
406     }
407
408     /* Scale input */
409     inv_gain_Q23 = silk_RSHIFT_ROUND( inv_gain_Q31, 8 );
410     for( i = 0; i < psEncC->subfr_length; i++ ) {
411         x_sc_Q10[ i ] = silk_SMULWW( x_Q3[ i ], inv_gain_Q23 );
412     }
413
414     /* Save inverse gain */
415     NSQ->prev_gain_Q16 = Gains_Q16[ subfr ];
416
417     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */
418     if( NSQ->rewhite_flag ) {
419         if( subfr == 0 ) {
420             /* Do LTP downscaling */
421             inv_gain_Q31 = silk_LSHIFT( silk_SMULWB( inv_gain_Q31, LTP_scale_Q14 ), 2 );
422         }
423         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
424             silk_assert( i < MAX_FRAME_LENGTH );
425             sLTP_Q15[ i ] = silk_SMULWB( inv_gain_Q31, sLTP[ i ] );
426         }
427     }
428
429     /* Adjust for changing gain */
430     if( gain_adj_Q16 != (opus_int32)1 << 16 ) {
431         /* Scale long-term shaping state */
432         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {
433             NSQ->sLTP_shp_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q14[ i ] );
434         }
435
436         /* Scale long-term prediction state */
437         if( signal_type == TYPE_VOICED && NSQ->rewhite_flag == 0 ) {
438             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {
439                 sLTP_Q15[ i ] = silk_SMULWW( gain_adj_Q16, sLTP_Q15[ i ] );
440             }
441         }
442
443         NSQ->sLF_AR_shp_Q14 = silk_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q14 );
444
445         /* Scale short-term prediction and shaping states */
446         for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {
447             NSQ->sLPC_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );
448         }
449         for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {
450             NSQ->sAR2_Q14[ i ] = silk_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );
451         }
452     }
453 }