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