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