Fixing silk fixed point
[opus.git] / silk / silk_NSQ.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved. \r
3 Redistribution and use in source and binary forms, with or without \r
4 modification, (subject to the limitations in the disclaimer below) \r
5 are permitted provided that the following conditions are met:\r
6 - Redistributions of source code must retain the above copyright notice,\r
7 this list of conditions and the following disclaimer.\r
8 - Redistributions in binary form must reproduce the above copyright \r
9 notice, this list of conditions and the following disclaimer in the \r
10 documentation and/or other materials provided with the distribution.\r
11 - Neither the name of Skype Limited, nor the names of specific \r
12 contributors, may be used to endorse or promote products derived from \r
13 this software without specific prior written permission.\r
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED \r
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND \r
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,\r
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND \r
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE \r
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, \r
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT\r
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF \r
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON \r
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT \r
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE \r
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.\r
26 ***********************************************************************/\r
27 \r
28 #include "silk_main.h"\r
29 \r
30 SKP_INLINE void silk_nsq_scale_states(\r
31     const silk_encoder_state *psEncC,       /* I    Encoder State                   */\r
32     silk_nsq_state      *NSQ,               /* I/O  NSQ state                       */\r
33     const SKP_int16     x[],                /* I    input in Q0                     */\r
34     SKP_int32           x_sc_Q10[],         /* O    input scaled with 1/Gain        */\r
35     const SKP_int16     sLTP[],             /* I    re-whitened LTP state in Q0     */\r
36     SKP_int32           sLTP_Q16[],         /* O    LTP state matching scaled input */\r
37     SKP_int             subfr,              /* I    subframe number                 */\r
38     const SKP_int       LTP_scale_Q14,      /* I                                    */\r
39     const SKP_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                             */\r
40     const SKP_int       pitchL[ MAX_NB_SUBFR ]  /* I                                */\r
41 );\r
42 \r
43 SKP_INLINE void silk_noise_shape_quantizer(\r
44     silk_nsq_state      *NSQ,               /* I/O  NSQ state                       */\r
45     SKP_int             signalType,         /* I    Signal type                     */\r
46     const SKP_int32     x_sc_Q10[],         /* I                                    */\r
47     SKP_int8            pulses[],           /* O                                    */\r
48     SKP_int16           xq[],               /* O                                    */\r
49     SKP_int32           sLTP_Q16[],         /* I/O  LTP state                       */\r
50     const SKP_int16     a_Q12[],            /* I    Short term prediction coefs     */\r
51     const SKP_int16     b_Q14[],            /* I    Long term prediction coefs      */\r
52     const SKP_int16     AR_shp_Q13[],       /* I    Noise shaping AR coefs          */\r
53     SKP_int             lag,                /* I    Pitch lag                       */\r
54     SKP_int32           HarmShapeFIRPacked_Q14, /* I                                */\r
55     SKP_int             Tilt_Q14,           /* I    Spectral tilt                   */\r
56     SKP_int32           LF_shp_Q14,         /* I                                    */\r
57     SKP_int32           Gain_Q16,           /* I                                    */\r
58     SKP_int             Lambda_Q10,         /* I                                    */\r
59     SKP_int             offset_Q10,         /* I                                    */\r
60     SKP_int             length,             /* I    Input length                    */\r
61     SKP_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */\r
62     SKP_int             predictLPCOrder     /* I    Prediction filter order         */\r
63 );\r
64 \r
65 void silk_NSQ(\r
66     const silk_encoder_state        *psEncC,                                    /* I/O  Encoder State                       */\r
67     silk_nsq_state                  *NSQ,                                       /* I/O  NSQ state                           */\r
68     SideInfoIndices                 *psIndices,                                 /* I/O  Quantization Indices                */\r
69     const SKP_int16                 x[],                                        /* I    prefiltered input signal            */\r
70     SKP_int8                        pulses[],                                   /* O    quantized qulse signal              */\r
71     const SKP_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefficients  */\r
72     const SKP_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],    /* I    Long term prediction coefficients   */\r
73     const SKP_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ], /* I                                     */\r
74     const SKP_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],          /* I                                        */\r
75     const SKP_int                   Tilt_Q14[ MAX_NB_SUBFR ],                   /* I    Spectral tilt                       */\r
76     const SKP_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                 /* I                                        */\r
77     const SKP_int32                 Gains_Q16[ MAX_NB_SUBFR ],                  /* I                                        */\r
78     const SKP_int                   pitchL[ MAX_NB_SUBFR ],                     /* I                                        */\r
79     const SKP_int                   Lambda_Q10,                                 /* I                                        */\r
80     const SKP_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */\r
81 )\r
82 {\r
83     SKP_int     k, lag, start_idx, LSF_interpolation_flag;\r
84     const SKP_int16 *A_Q12, *B_Q14, *AR_shp_Q13;\r
85     SKP_int16   *pxq;\r
86     SKP_int32   sLTP_Q16[ 2 * MAX_FRAME_LENGTH ];\r
87     SKP_int16   sLTP[     2 * MAX_FRAME_LENGTH ];\r
88     SKP_int32   HarmShapeFIRPacked_Q14;\r
89     SKP_int     offset_Q10;\r
90     SKP_int32   x_sc_Q10[ MAX_FRAME_LENGTH / MAX_NB_SUBFR ];\r
91 \r
92     NSQ->rand_seed = psIndices->Seed;\r
93 \r
94     /* Set unvoiced lag to the previous one, overwrite later for voiced */\r
95     lag = NSQ->lagPrev;\r
96 \r
97     SKP_assert( NSQ->prev_inv_gain_Q16 != 0 );\r
98 \r
99     offset_Q10 = silk_Quantization_Offsets_Q10[ psIndices->signalType >> 1 ][ psIndices->quantOffsetType ];\r
100 \r
101     if( psIndices->NLSFInterpCoef_Q2 == 4 ) {\r
102         LSF_interpolation_flag = 0;\r
103     } else {\r
104         LSF_interpolation_flag = 1;\r
105     }\r
106 \r
107     /* Setup pointers to start of sub frame */\r
108     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;\r
109     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;\r
110     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];\r
111     for( k = 0; k < psEncC->nb_subfr; k++ ) {\r
112         A_Q12      = &PredCoef_Q12[ (( k >> 1 ) | ( 1 - LSF_interpolation_flag )) * MAX_LPC_ORDER ];\r
113         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER ];\r
114         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];\r
115 \r
116         /* Noise shape parameters */\r
117         SKP_assert( HarmShapeGain_Q14[ k ] >= 0 );\r
118         HarmShapeFIRPacked_Q14  =                          SKP_RSHIFT( HarmShapeGain_Q14[ k ], 2 );\r
119         HarmShapeFIRPacked_Q14 |= SKP_LSHIFT( ( SKP_int32 )SKP_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );\r
120 \r
121         NSQ->rewhite_flag = 0;\r
122         if( psIndices->signalType == TYPE_VOICED ) {\r
123             /* Voiced */\r
124             lag = pitchL[ k ];\r
125 \r
126             /* Re-whitening */\r
127             if( ( k & ( 3 - SKP_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {\r
128                 /* Rewhiten with new A coefs */\r
129                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;\r
130                 SKP_assert( start_idx > 0 );\r
131                 \r
132                 silk_LPC_analysis_filter( &sLTP[ start_idx ], &NSQ->xq[ start_idx + k * psEncC->subfr_length ], \r
133                     A_Q12, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder );\r
134 \r
135                 NSQ->rewhite_flag = 1;\r
136                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;\r
137             }\r
138         }\r
139         \r
140         silk_nsq_scale_states( psEncC, NSQ, x, x_sc_Q10, sLTP, sLTP_Q16, k, LTP_scale_Q14, Gains_Q16, pitchL );\r
141 \r
142         silk_noise_shape_quantizer( NSQ, psIndices->signalType, x_sc_Q10, pulses, pxq, sLTP_Q16, A_Q12, B_Q14, \r
143             AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], Lambda_Q10, \r
144             offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder );\r
145 \r
146         x      += psEncC->subfr_length;\r
147         pulses += psEncC->subfr_length;\r
148         pxq    += psEncC->subfr_length;\r
149     }\r
150 \r
151     /* Update lagPrev for next frame */\r
152     NSQ->lagPrev = pitchL[ psEncC->nb_subfr - 1 ];\r
153 \r
154     /* Save quantized speech and noise shaping signals */\r
155     SKP_memmove( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( SKP_int16 ) );\r
156     SKP_memmove( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( SKP_int32 ) );\r
157 \r
158 #ifdef SAVE_ALL_INTERNAL_DATA\r
159     DEBUG_STORE_DATA( xq.dat,       &pxq[ -psEncC->frame_length ],       psEncC->frame_length * sizeof( SKP_int16 ) );\r
160     DEBUG_STORE_DATA( q.dat,        &pulses[ -psEncC->frame_length ],    psEncC->frame_length * sizeof( SKP_int8 ) );\r
161     DEBUG_STORE_DATA( sLTP_Q16.dat, &sLTP_Q16[ psEncC->ltp_mem_length ], psEncC->frame_length * sizeof( SKP_int32 ) );\r
162 #endif\r
163 }\r
164 \r
165 /***********************************/\r
166 /* silk_noise_shape_quantizer  */\r
167 /***********************************/\r
168 SKP_INLINE void silk_noise_shape_quantizer(\r
169     silk_nsq_state  *NSQ,               /* I/O  NSQ state                       */\r
170     SKP_int             signalType,         /* I    Signal type                     */\r
171     const SKP_int32     x_sc_Q10[],         /* I                                    */\r
172     SKP_int8            pulses[],           /* O                                    */\r
173     SKP_int16           xq[],               /* O                                    */\r
174     SKP_int32           sLTP_Q16[],         /* I/O  LTP state                       */\r
175     const SKP_int16     a_Q12[],            /* I    Short term prediction coefs     */\r
176     const SKP_int16     b_Q14[],            /* I    Long term prediction coefs      */\r
177     const SKP_int16     AR_shp_Q13[],       /* I    Noise shaping AR coefs          */\r
178     SKP_int             lag,                /* I    Pitch lag                       */\r
179     SKP_int32           HarmShapeFIRPacked_Q14, /* I                                */\r
180     SKP_int             Tilt_Q14,           /* I    Spectral tilt                   */\r
181     SKP_int32           LF_shp_Q14,         /* I                                    */\r
182     SKP_int32           Gain_Q16,           /* I                                    */\r
183     SKP_int             Lambda_Q10,         /* I                                    */\r
184     SKP_int             offset_Q10,         /* I                                    */\r
185     SKP_int             length,             /* I    Input length                    */\r
186     SKP_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */\r
187     SKP_int             predictLPCOrder     /* I    Prediction filter order         */\r
188 )\r
189 {\r
190     SKP_int     i, j;\r
191     SKP_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14;\r
192     SKP_int32   n_LF_Q10, r_Q10, rr_Q10, q1_Q10, q2_Q10, rd1_Q10, rd2_Q10;\r
193     SKP_int32   dither, exc_Q10, LPC_exc_Q10, xq_Q10;\r
194     SKP_int32   tmp1, tmp2, sLF_AR_shp_Q10;\r
195     SKP_int32   *psLPC_Q14, *shp_lag_ptr, *pred_lag_ptr;\r
196 \r
197     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];\r
198     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
199     \r
200     /* Setup short term AR state */\r
201     psLPC_Q14 = &NSQ->sLPC_Q14[ NSQ_LPC_BUF_LENGTH - 1 ];\r
202 \r
203     for( i = 0; i < length; i++ ) {\r
204         /* Generate dither */\r
205         NSQ->rand_seed = SKP_RAND( NSQ->rand_seed );\r
206 \r
207         /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */\r
208         dither = SKP_RSHIFT( NSQ->rand_seed, 31 );\r
209                 \r
210         /* Short-term prediction */\r
211         SKP_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */\r
212         SKP_assert( ( (SKP_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */\r
213         SKP_assert( predictLPCOrder >= 10 );            /* check that unrolling works */\r
214 \r
215         /* Partially unrolled */\r
216         LPC_pred_Q10 = SKP_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );\r
217         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );\r
218         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );\r
219         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );\r
220         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );\r
221         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );\r
222         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );\r
223         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );\r
224         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );\r
225         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );\r
226         for( j = 10; j < predictLPCOrder; j ++ ) {\r
227             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );\r
228         }\r
229 \r
230         /* Long-term prediction */\r
231         if( signalType == TYPE_VOICED ) {\r
232             /* Unrolled loop */\r
233             LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );\r
234             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );\r
235             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );\r
236             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );\r
237             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );\r
238             pred_lag_ptr++;\r
239         } else {\r
240             LTP_pred_Q14 = 0;\r
241         }\r
242 \r
243         /* Noise shape feedback */\r
244         SKP_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */\r
245         tmp2 = psLPC_Q14[ 0 ];\r
246         tmp1 = NSQ->sAR2_Q14[ 0 ];\r
247         NSQ->sAR2_Q14[ 0 ] = tmp2;\r
248         n_AR_Q10 = SKP_SMULWB( tmp2, AR_shp_Q13[ 0 ] );\r
249         for( j = 2; j < shapingLPCOrder; j += 2 ) {\r
250             tmp2 = NSQ->sAR2_Q14[ j - 1 ];\r
251             NSQ->sAR2_Q14[ j - 1 ] = tmp1;\r
252             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );\r
253             tmp1 = NSQ->sAR2_Q14[ j + 0 ];\r
254             NSQ->sAR2_Q14[ j + 0 ] = tmp2;\r
255             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );\r
256         }\r
257         NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;\r
258         n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );\r
259 \r
260         n_AR_Q10 = SKP_RSHIFT( n_AR_Q10, 1 );   /* Q11 -> Q10 */\r
261         n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );\r
262 \r
263         n_LF_Q10 = SKP_LSHIFT( SKP_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 ); \r
264         n_LF_Q10 = SKP_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );\r
265 \r
266         SKP_assert( lag > 0 || signalType != TYPE_VOICED );\r
267 \r
268         /* Long-term shaping */\r
269         if( lag > 0 ) {\r
270             /* Symmetric, packed FIR coefficients */\r
271             n_LTP_Q14 = SKP_SMULWB( SKP_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );\r
272             n_LTP_Q14 = SKP_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );\r
273             n_LTP_Q14 = SKP_LSHIFT( n_LTP_Q14, 6 );\r
274             shp_lag_ptr++;\r
275 \r
276             tmp1 = SKP_SUB32( LTP_pred_Q14, n_LTP_Q14 );                        /* Add Q14 stuff */\r
277             tmp1 = SKP_RSHIFT( tmp1, 4 );                                       /* convert to Q10  */\r
278             tmp1 = SKP_ADD32( tmp1, LPC_pred_Q10 );                             /* add Q10 stuff */ \r
279             tmp1 = SKP_SUB32( tmp1, n_AR_Q10 );                                 /* subtract Q10 stuff */ \r
280         } else {\r
281             tmp1 = SKP_SUB32( LPC_pred_Q10, n_AR_Q10 );                         /* subtract Q10 stuff */ \r
282         }\r
283 \r
284         /* Input minus prediction plus noise feedback  */\r
285         //r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP;\r
286         tmp1  = SKP_SUB32( tmp1, n_LF_Q10 );                                    /* subtract Q10 stuff */ \r
287         r_Q10 = SKP_SUB32( x_sc_Q10[ i ], tmp1 );\r
288 \r
289         /* Flip sign depending on dither */\r
290         r_Q10 = r_Q10 ^ dither;\r
291         r_Q10 = SKP_LIMIT_32( r_Q10, -31 << 10, 30 << 10 );\r
292 \r
293         /* Find two quantization level candidates and measure their rate-distortion */\r
294         q1_Q10 = SKP_SUB32( r_Q10, offset_Q10 );\r
295         q1_Q10 = SKP_RSHIFT( q1_Q10, 10 );\r
296         if( q1_Q10 > 0 ) {\r
297             q1_Q10  = SKP_SUB32( SKP_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );\r
298             q1_Q10  = SKP_ADD32( q1_Q10, offset_Q10 );\r
299             q2_Q10  = SKP_ADD32( q1_Q10, 1024 );\r
300             rd1_Q10 = SKP_SMULBB( q1_Q10, Lambda_Q10 );\r
301             rd2_Q10 = SKP_SMULBB( q2_Q10, Lambda_Q10 );\r
302         } else if( q1_Q10 == 0 ) {\r
303             q1_Q10  = offset_Q10;\r
304             q2_Q10  = SKP_ADD32( q1_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );\r
305             rd1_Q10 = SKP_SMULBB( q1_Q10, Lambda_Q10 );\r
306             rd2_Q10 = SKP_SMULBB( q2_Q10, Lambda_Q10 );\r
307         } else if( q1_Q10 == -1 ) {\r
308             q2_Q10  = offset_Q10;\r
309             q1_Q10  = SKP_SUB32( q2_Q10, 1024 - QUANT_LEVEL_ADJUST_Q10 );\r
310             rd1_Q10 = SKP_SMULBB( -q1_Q10, Lambda_Q10 );\r
311             rd2_Q10 = SKP_SMULBB(  q2_Q10, Lambda_Q10 );\r
312         } else {            /* Q1_Q10 < -1 */\r
313             q1_Q10  = SKP_ADD32( SKP_LSHIFT( q1_Q10, 10 ), QUANT_LEVEL_ADJUST_Q10 );\r
314             q1_Q10  = SKP_ADD32( q1_Q10, offset_Q10 );\r
315             q2_Q10  = SKP_ADD32( q1_Q10, 1024 );\r
316             rd1_Q10 = SKP_SMULBB( -q1_Q10, Lambda_Q10 );\r
317             rd2_Q10 = SKP_SMULBB( -q2_Q10, Lambda_Q10 );\r
318         }\r
319         rr_Q10  = SKP_SUB32( r_Q10, q1_Q10 );\r
320         rd1_Q10 = SKP_RSHIFT( SKP_SMLABB( rd1_Q10, rr_Q10, rr_Q10 ), 10 );\r
321         rr_Q10  = SKP_SUB32( r_Q10, q2_Q10 );\r
322         rd2_Q10 = SKP_RSHIFT( SKP_SMLABB( rd2_Q10, rr_Q10, rr_Q10 ), 10 );\r
323 \r
324         if( rd2_Q10 < rd1_Q10 ) {\r
325             q1_Q10 = q2_Q10;\r
326         }\r
327 \r
328         pulses[ i ] = ( SKP_int8 )SKP_RSHIFT_ROUND( q1_Q10, 10 );\r
329 \r
330         /* Excitation */\r
331         exc_Q10 = q1_Q10 ^ dither;\r
332 \r
333         /* Add predictions */\r
334         LPC_exc_Q10 = SKP_ADD32( exc_Q10, SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );\r
335         xq_Q10      = SKP_ADD32( LPC_exc_Q10, LPC_pred_Q10 );\r
336         \r
337         /* Scale XQ back to normal level before saving */\r
338         xq[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( xq_Q10, Gain_Q16 ), 10 ) );\r
339         \r
340         /* Update states */\r
341         psLPC_Q14++;\r
342         *psLPC_Q14 = SKP_LSHIFT( xq_Q10, 4 );\r
343         sLF_AR_shp_Q10 = SKP_SUB32( xq_Q10, n_AR_Q10 );\r
344         NSQ->sLF_AR_shp_Q12 = SKP_LSHIFT( sLF_AR_shp_Q10, 2 );\r
345 \r
346         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx ] = SKP_SUB32( sLF_AR_shp_Q10, n_LF_Q10 );\r
347         sLTP_Q16[ NSQ->sLTP_buf_idx ] = SKP_LSHIFT( LPC_exc_Q10, 6 );\r
348         NSQ->sLTP_shp_buf_idx++;\r
349         NSQ->sLTP_buf_idx++;\r
350 \r
351         /* Make dither dependent on quantized signal */\r
352         NSQ->rand_seed += pulses[ i ];\r
353     }\r
354 \r
355     /* Update LPC synth buffer */\r
356     SKP_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], NSQ_LPC_BUF_LENGTH * sizeof( SKP_int32 ) );\r
357 }\r
358 \r
359 SKP_INLINE void silk_nsq_scale_states(\r
360     const silk_encoder_state *psEncC,       /* I    Encoder State                   */\r
361     silk_nsq_state      *NSQ,               /* I/O  NSQ state                       */\r
362     const SKP_int16     x[],                /* I    input in Q0                     */\r
363     SKP_int32           x_sc_Q10[],         /* O    input scaled with 1/Gain        */\r
364     const SKP_int16     sLTP[],             /* I    re-whitened LTP state in Q0     */\r
365     SKP_int32           sLTP_Q16[],         /* O    LTP state matching scaled input */\r
366     SKP_int             subfr,              /* I    subframe number                 */\r
367     const SKP_int       LTP_scale_Q14,      /* I                                    */\r
368     const SKP_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                             */\r
369     const SKP_int       pitchL[ MAX_NB_SUBFR ]  /* I                                */\r
370 )\r
371 {\r
372     SKP_int   i, lag;\r
373     SKP_int32 inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;\r
374 \r
375     inv_gain_Q16 = silk_INVERSE32_varQ( SKP_max( Gains_Q16[ subfr ], 1 ), 32 );\r
376     inv_gain_Q16 = SKP_min( inv_gain_Q16, SKP_int16_MAX );\r
377     lag          = pitchL[ subfr ];\r
378 \r
379     /* After rewhitening the LTP state is un-scaled, so scale with inv_gain_Q16 */\r
380     if( NSQ->rewhite_flag ) {\r
381         inv_gain_Q32 = SKP_LSHIFT( inv_gain_Q16, 16 );\r
382         if( subfr == 0 ) {\r
383             /* Do LTP downscaling */\r
384             inv_gain_Q32 = SKP_LSHIFT( SKP_SMULWB( inv_gain_Q32, LTP_scale_Q14 ), 2 );\r
385         }\r
386         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {\r
387             SKP_assert( i < MAX_FRAME_LENGTH );\r
388             sLTP_Q16[ i ] = SKP_SMULWB( inv_gain_Q32, sLTP[ i ] );\r
389         }\r
390     }\r
391 \r
392     /* Adjust for changing gain */\r
393     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {\r
394         gain_adj_Q16 = silk_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );\r
395 \r
396         /* Scale long-term shaping state */\r
397         for( i = NSQ->sLTP_shp_buf_idx - psEncC->ltp_mem_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {\r
398             NSQ->sLTP_shp_Q10[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );\r
399         }\r
400 \r
401         /* Scale long-term prediction state */\r
402         if( NSQ->rewhite_flag == 0 ) {\r
403             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {\r
404                 sLTP_Q16[ i ] = SKP_SMULWW( gain_adj_Q16, sLTP_Q16[ i ] );\r
405             }\r
406         }\r
407 \r
408         NSQ->sLF_AR_shp_Q12 = SKP_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q12 );\r
409 \r
410         /* Scale short-term prediction and shaping states */\r
411         for( i = 0; i < NSQ_LPC_BUF_LENGTH; i++ ) {\r
412             NSQ->sLPC_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );\r
413         }\r
414         for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {\r
415             NSQ->sAR2_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );\r
416         }\r
417     }\r
418 \r
419     /* Scale input */\r
420     for( i = 0; i < psEncC->subfr_length; i++ ) {\r
421         x_sc_Q10[ i ] = SKP_RSHIFT( SKP_SMULBB( x[ i ], ( SKP_int16 )inv_gain_Q16 ), 6 );\r
422     }\r
423 \r
424     /* save inv_gain */\r
425     SKP_assert( inv_gain_Q16 != 0 );\r
426     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;\r
427 }\r