Update SILK code using the CELT range coder
[opus.git] / src_common / SKP_Silk_NSQ.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2010, 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 "SKP_Silk_main.h"\r
29 \r
30 SKP_INLINE void SKP_Silk_nsq_scale_states(\r
31     SKP_Silk_nsq_state  *NSQ,               /* I/O NSQ state                        */\r
32     const SKP_int16     x[],                /* I input in Q0                        */\r
33     SKP_int32           x_sc_Q10[],         /* O input scaled with 1/Gain           */\r
34     SKP_int             length,             /* I length of input                    */\r
35     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 SKP_Silk_noise_shape_quantizer(\r
44     SKP_Silk_nsq_state  *NSQ,               /* I/O  NSQ state                       */\r
45     SKP_int             sigtype,            /* I    Signal type                     */\r
46     const SKP_int32     x_sc_Q10[],         /* I                                    */\r
47     SKP_int8            q[],                /* 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     SKP_int32           warping_Q16         /* I                                    */\r
64 #ifdef SAVE_ALL_INTERNAL_DATA\r
65     ,SKP_float          q_in_env[],         /* O                                    */\r
66     SKP_float           q_exc[],            /* O                                    */\r
67     SKP_float           q_LPC_exc[],        /* O                                    */\r
68     SKP_float           q_exc_prev[]        /* O                                    */\r
69 #endif\r
70 );\r
71 \r
72 void SKP_Silk_NSQ(\r
73     SKP_Silk_encoder_state          *psEncC,                                    /* I/O  Encoder State                       */\r
74     SKP_Silk_encoder_control        *psEncCtrlC,                                /* I    Encoder Control                     */\r
75     SKP_Silk_nsq_state              *NSQ,                                       /* I/O  NSQ state                           */\r
76     const SKP_int16                 x[],                                        /* I    prefiltered input signal            */\r
77     SKP_int8                        q[],                                        /* O    quantized qulse signal              */\r
78     const SKP_int                   LSFInterpFactor_Q2,                         /* I    LSF interpolation factor in Q2      */\r
79     const SKP_int16                 PredCoef_Q12[ 2 * MAX_LPC_ORDER ],          /* I    Short term prediction coefficients  */\r
80     const SKP_int16                 LTPCoef_Q14[ LTP_ORDER * MAX_NB_SUBFR ],        /* I    Long term prediction coefficients   */\r
81     const SKP_int16                 AR2_Q13[ MAX_NB_SUBFR * MAX_SHAPE_LPC_ORDER ],  /* I                                    */\r
82     const SKP_int                   HarmShapeGain_Q14[ MAX_NB_SUBFR ],              /* I                                    */\r
83     const SKP_int                   Tilt_Q14[ MAX_NB_SUBFR ],                       /* I    Spectral tilt                   */\r
84     const SKP_int32                 LF_shp_Q14[ MAX_NB_SUBFR ],                     /* I                                    */\r
85     const SKP_int32                 Gains_Q16[ MAX_NB_SUBFR ],                      /* I                                    */\r
86     const SKP_int                   Lambda_Q10,                                 /* I                                        */\r
87     const SKP_int                   LTP_scale_Q14                               /* I    LTP state scaling                   */\r
88 )\r
89 {\r
90     SKP_int     k, lag, start_idx, subfr_length, LSF_interpolation_flag;\r
91     const SKP_int16 *A_Q12, *B_Q14, *AR_shp_Q13;\r
92     SKP_int16   *pxq;\r
93     SKP_int32   sLTP_Q16[ 2 * MAX_FRAME_LENGTH ];\r
94     SKP_int16   sLTP[     2 * MAX_FRAME_LENGTH ];\r
95     SKP_int32   HarmShapeFIRPacked_Q14;\r
96     SKP_int     offset_Q10;\r
97     SKP_int32   FiltState[ MAX_LPC_ORDER ];\r
98     SKP_int32   x_sc_Q10[ MAX_FRAME_LENGTH / MAX_NB_SUBFR ];\r
99 #ifdef SAVE_ALL_INTERNAL_DATA\r
100     /* Date buffers for analysing */\r
101     SKP_float   q_in_env_buf[  MAX_FRAME_LENGTH ];\r
102     SKP_float   q_exc_buf[     MAX_FRAME_LENGTH ];\r
103     SKP_float   q_LPC_exc_buf[ MAX_FRAME_LENGTH ];\r
104     SKP_float   q_exc_prev_buf[     MAX_FRAME_LENGTH ];\r
105     SKP_float   *q_in_env, *q_exc, *q_LPC_exc, *q_exc_prev;\r
106 \r
107     q_in_env   = q_in_env_buf;\r
108     q_exc      = q_exc_buf;\r
109     q_LPC_exc  = q_LPC_exc_buf;\r
110     q_exc_prev = q_exc_prev_buf;\r
111 #endif\r
112 \r
113     subfr_length = psEncC->frame_length / MAX_NB_SUBFR;\r
114 \r
115     NSQ->rand_seed  =  psEncCtrlC->Seed;\r
116     /* Set unvoiced lag to the previous one, overwrite later for voiced */\r
117     lag             = NSQ->lagPrev;\r
118 \r
119     offset_Q10 = SKP_Silk_Quantization_Offsets_Q10[ psEncCtrlC->sigtype ][ psEncCtrlC->QuantOffsetType ];\r
120 \r
121     if( LSFInterpFactor_Q2 == ( 1 << 2 ) ) {\r
122         LSF_interpolation_flag = 0;\r
123     } else {\r
124         LSF_interpolation_flag = 1;\r
125     }\r
126 \r
127     /* Setup pointers to start of sub frame */\r
128     NSQ->sLTP_shp_buf_idx = psEncC->ltp_mem_length;\r
129     NSQ->sLTP_buf_idx     = psEncC->ltp_mem_length;\r
130     pxq                   = &NSQ->xq[ psEncC->ltp_mem_length ];\r
131     for( k = 0; k < psEncC->nb_subfr; k++ ) {\r
132         A_Q12      = &PredCoef_Q12[ (( k >> 1 ) | ( 1 - LSF_interpolation_flag )) * MAX_LPC_ORDER ];\r
133         B_Q14      = &LTPCoef_Q14[ k * LTP_ORDER ];\r
134         AR_shp_Q13 = &AR2_Q13[     k * MAX_SHAPE_LPC_ORDER ];\r
135 \r
136         /* Noise shape parameters */\r
137         SKP_assert( HarmShapeGain_Q14[ k ] >= 0 );\r
138         HarmShapeFIRPacked_Q14  =                        SKP_RSHIFT( HarmShapeGain_Q14[ k ], 2 );\r
139         HarmShapeFIRPacked_Q14 |= SKP_LSHIFT( ( SKP_int32 )SKP_RSHIFT( HarmShapeGain_Q14[ k ], 1 ), 16 );\r
140 \r
141         if( psEncCtrlC->sigtype == SIG_TYPE_VOICED ) {\r
142             /* Voiced */\r
143             lag = psEncCtrlC->pitchL[ k ];\r
144 \r
145             NSQ->rewhite_flag = 0;\r
146             /* Re-whitening */\r
147             if( ( k & ( 3 - SKP_LSHIFT( LSF_interpolation_flag, 1 ) ) ) == 0 ) {\r
148                 /* Rewhiten with new A coefs */\r
149                 \r
150                 start_idx = psEncC->ltp_mem_length - lag - psEncC->predictLPCOrder - LTP_ORDER / 2;\r
151                 start_idx = SKP_LIMIT_int( start_idx, 0, psEncC->ltp_mem_length - psEncC->predictLPCOrder ); /* Limit */\r
152                 \r
153                 SKP_memset( FiltState, 0, psEncC->predictLPCOrder * sizeof( SKP_int32 ) );\r
154                 SKP_Silk_MA_Prediction( &NSQ->xq[ start_idx + k * psEncC->subfr_length ], \r
155                     A_Q12, FiltState, sLTP + start_idx, psEncC->ltp_mem_length - start_idx, psEncC->predictLPCOrder );\r
156 \r
157                 NSQ->rewhite_flag = 1;\r
158                 NSQ->sLTP_buf_idx = psEncC->ltp_mem_length;\r
159             }\r
160         }\r
161         \r
162         SKP_Silk_nsq_scale_states( NSQ, x, x_sc_Q10, psEncC->subfr_length, sLTP, \r
163             sLTP_Q16, k, LTP_scale_Q14, Gains_Q16, psEncCtrlC->pitchL );\r
164 \r
165         SKP_Silk_noise_shape_quantizer( NSQ, psEncCtrlC->sigtype, x_sc_Q10, q, pxq, sLTP_Q16, A_Q12, B_Q14, \r
166             AR_shp_Q13, lag, HarmShapeFIRPacked_Q14, Tilt_Q14[ k ], LF_shp_Q14[ k ], Gains_Q16[ k ], Lambda_Q10, \r
167             offset_Q10, psEncC->subfr_length, psEncC->shapingLPCOrder, psEncC->predictLPCOrder, psEncCtrlC->warping_Q16\r
168 #ifdef SAVE_ALL_INTERNAL_DATA\r
169             , q_in_env, q_exc, q_LPC_exc, q_exc_prev\r
170 #endif\r
171         );\r
172 \r
173         x          += psEncC->subfr_length;\r
174         q          += psEncC->subfr_length;\r
175         pxq        += psEncC->subfr_length;\r
176 #ifdef SAVE_ALL_INTERNAL_DATA\r
177         q_in_env   += psEncC->subfr_length;\r
178         q_exc      += psEncC->subfr_length;\r
179         q_LPC_exc  += psEncC->subfr_length;\r
180         q_exc_prev += subfr_length;\r
181 #endif\r
182     }\r
183 \r
184     /* Update lagPrev for next frame */\r
185     NSQ->lagPrev = psEncCtrlC->pitchL[ psEncC->nb_subfr - 1 ];\r
186     /* Save quantized speech and noise shaping signals */\r
187     SKP_memcpy( NSQ->xq,           &NSQ->xq[           psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( SKP_int16 ) );\r
188     SKP_memcpy( NSQ->sLTP_shp_Q10, &NSQ->sLTP_shp_Q10[ psEncC->frame_length ], psEncC->ltp_mem_length * sizeof( SKP_int32 ) );\r
189 \r
190 #ifdef SAVE_ALL_INTERNAL_DATA\r
191     DEBUG_STORE_DATA( q_in_env.dat,  q_in_env_buf,  psEncC->frame_length * sizeof( SKP_float ) );\r
192     DEBUG_STORE_DATA( q_exc.dat,     q_exc_buf,     psEncC->frame_length * sizeof( SKP_float ) );\r
193     DEBUG_STORE_DATA( q_lpc_exc.dat, q_LPC_exc_buf, psEncC->frame_length * sizeof( SKP_float ) );\r
194     DEBUG_STORE_DATA( xq.dat,        &NSQ->xq[ psEncC->ltp_mem_length - psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int16 ) );\r
195     DEBUG_STORE_DATA( q.dat,         &q[ -psEncC->frame_length ], psEncC->frame_length * sizeof( SKP_int8 ) );\r
196 #endif\r
197 }\r
198 \r
199 /***********************************/\r
200 /* SKP_Silk_noise_shape_quantizer  */\r
201 /***********************************/\r
202 SKP_INLINE void SKP_Silk_noise_shape_quantizer(\r
203     SKP_Silk_nsq_state  *NSQ,               /* I/O  NSQ state                       */\r
204     SKP_int             sigtype,            /* I    Signal type                     */\r
205     const SKP_int32     x_sc_Q10[],         /* I                                    */\r
206     SKP_int8            q[],                /* O                                    */\r
207     SKP_int16           xq[],               /* O                                    */\r
208     SKP_int32           sLTP_Q16[],         /* I/O  LTP state                       */\r
209     const SKP_int16     a_Q12[],            /* I    Short term prediction coefs     */\r
210     const SKP_int16     b_Q14[],            /* I    Long term prediction coefs      */\r
211     const SKP_int16     AR_shp_Q13[],       /* I    Noise shaping AR coefs          */\r
212     SKP_int             lag,                /* I    Pitch lag                       */\r
213     SKP_int32           HarmShapeFIRPacked_Q14, /* I                                */\r
214     SKP_int             Tilt_Q14,           /* I    Spectral tilt                   */\r
215     SKP_int32           LF_shp_Q14,         /* I                                    */\r
216     SKP_int32           Gain_Q16,           /* I                                    */\r
217     SKP_int             Lambda_Q10,         /* I                                    */\r
218     SKP_int             offset_Q10,         /* I                                    */\r
219     SKP_int             length,             /* I    Input length                    */\r
220     SKP_int             shapingLPCOrder,    /* I    Noise shaping AR filter order   */\r
221     SKP_int             predictLPCOrder,    /* I    Prediction filter order         */\r
222     SKP_int             warping_Q16         /* I                                    */\r
223 #ifdef SAVE_ALL_INTERNAL_DATA\r
224     ,SKP_float          q_in_env[],         /* O                                    */\r
225     SKP_float           q_exc[],            /* O                                    */\r
226     SKP_float           q_LPC_exc[],        /* O                                    */\r
227     SKP_float           q_exc_prev[]        /* O                                    */\r
228 #endif\r
229 )\r
230 {\r
231     SKP_int     i, j;\r
232     SKP_int32   LTP_pred_Q14, LPC_pred_Q10, n_AR_Q10, n_LTP_Q14;\r
233     SKP_int32   n_LF_Q10, r_Q10, q_Q0, q_Q10;\r
234     SKP_int32   thr1_Q10, thr2_Q10, thr3_Q10;\r
235     SKP_int32   dither, exc_Q10, LPC_exc_Q10, xq_Q10;\r
236     SKP_int32   tmp1, tmp2, sLF_AR_shp_Q10;\r
237     SKP_int32   *psLPC_Q14, *shp_lag_ptr, *pred_lag_ptr;\r
238 \r
239     shp_lag_ptr  = &NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - lag + HARM_SHAPE_FIR_TAPS / 2 ];\r
240     pred_lag_ptr = &sLTP_Q16[ NSQ->sLTP_buf_idx - lag + LTP_ORDER / 2 ];\r
241     \r
242     /* Setup short term AR state */\r
243     psLPC_Q14     = &NSQ->sLPC_Q14[ MAX_LPC_ORDER - 1 ];\r
244 \r
245     /* Quantization thresholds */\r
246     thr1_Q10 = SKP_SUB_RSHIFT32( -1536, Lambda_Q10, 1);\r
247     thr2_Q10 = SKP_SUB_RSHIFT32( -512,  Lambda_Q10, 1);\r
248     thr2_Q10 = SKP_ADD_RSHIFT32( thr2_Q10, SKP_SMULBB( offset_Q10, Lambda_Q10 ), 10 );\r
249     thr3_Q10 = SKP_ADD_RSHIFT32(  512,  Lambda_Q10, 1);\r
250 \r
251     for( i = 0; i < length; i++ ) {\r
252         /* Generate dither */\r
253         NSQ->rand_seed = SKP_RAND( NSQ->rand_seed );\r
254 \r
255         /* dither = rand_seed < 0 ? 0xFFFFFFFF : 0; */\r
256         dither = SKP_RSHIFT( NSQ->rand_seed, 31 );\r
257                 \r
258         /* Short-term prediction */\r
259         SKP_assert( ( predictLPCOrder  & 1 ) == 0 );    /* check that order is even */\r
260         SKP_assert( ( (SKP_int64)a_Q12 & 3 ) == 0 );    /* check that array starts at 4-byte aligned address */\r
261         SKP_assert( predictLPCOrder >= 10 );            /* check that unrolling works */\r
262 \r
263         /* Partially unrolled */\r
264         LPC_pred_Q10 = SKP_SMULWB(               psLPC_Q14[  0 ], a_Q12[ 0 ] );\r
265         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -1 ], a_Q12[ 1 ] );\r
266         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -2 ], a_Q12[ 2 ] );\r
267         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -3 ], a_Q12[ 3 ] );\r
268         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -4 ], a_Q12[ 4 ] );\r
269         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -5 ], a_Q12[ 5 ] );\r
270         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -6 ], a_Q12[ 6 ] );\r
271         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -7 ], a_Q12[ 7 ] );\r
272         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -8 ], a_Q12[ 8 ] );\r
273         LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -9 ], a_Q12[ 9 ] );\r
274 \r
275         for( j = 10; j < predictLPCOrder; j ++ ) {\r
276             LPC_pred_Q10 = SKP_SMLAWB( LPC_pred_Q10, psLPC_Q14[ -j ], a_Q12[ j ] );\r
277         }\r
278 \r
279         /* Long-term prediction */\r
280         if( sigtype == SIG_TYPE_VOICED ) {\r
281             /* Unrolled loop */\r
282             LTP_pred_Q14 = SKP_SMULWB(               pred_lag_ptr[  0 ], b_Q14[ 0 ] );\r
283             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -1 ], b_Q14[ 1 ] );\r
284             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -2 ], b_Q14[ 2 ] );\r
285             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -3 ], b_Q14[ 3 ] );\r
286             LTP_pred_Q14 = SKP_SMLAWB( LTP_pred_Q14, pred_lag_ptr[ -4 ], b_Q14[ 4 ] );\r
287             pred_lag_ptr++;\r
288         } else {\r
289             LTP_pred_Q14 = 0;\r
290         }\r
291 \r
292         /* Noise shape feedback */\r
293         SKP_assert( ( shapingLPCOrder & 1 ) == 0 );   /* check that order is even */\r
294         /* Output of lowpass section */\r
295         tmp2 = SKP_SMLAWB( psLPC_Q14[ 0 ], NSQ->sAR2_Q14[ 0 ], warping_Q16 );\r
296         /* Output of allpass section */\r
297         tmp1 = SKP_SMLAWB( NSQ->sAR2_Q14[ 0 ], NSQ->sAR2_Q14[ 1 ] - tmp2, warping_Q16 );\r
298         NSQ->sAR2_Q14[ 0 ] = tmp2;\r
299         n_AR_Q10 = SKP_SMULWB( tmp2, AR_shp_Q13[ 0 ] );\r
300         /* Loop over allpass sections */\r
301         for( j = 2; j < shapingLPCOrder; j += 2 ) {\r
302             /* Output of allpass section */\r
303             tmp2 = SKP_SMLAWB( NSQ->sAR2_Q14[ j - 1 ], NSQ->sAR2_Q14[ j + 0 ] - tmp1, warping_Q16 );\r
304             NSQ->sAR2_Q14[ j - 1 ] = tmp1;\r
305             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ j - 1 ] );\r
306             /* Output of allpass section */\r
307             tmp1 = SKP_SMLAWB( NSQ->sAR2_Q14[ j + 0 ], NSQ->sAR2_Q14[ j + 1 ] - tmp2, warping_Q16 );\r
308             NSQ->sAR2_Q14[ j + 0 ] = tmp2;\r
309             n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp2, AR_shp_Q13[ j ] );\r
310         }\r
311         NSQ->sAR2_Q14[ shapingLPCOrder - 1 ] = tmp1;\r
312         n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, tmp1, AR_shp_Q13[ shapingLPCOrder - 1 ] );\r
313 \r
314         n_AR_Q10 = SKP_RSHIFT( n_AR_Q10, 1 );   /* Q11 -> Q10 */\r
315         n_AR_Q10 = SKP_SMLAWB( n_AR_Q10, NSQ->sLF_AR_shp_Q12, Tilt_Q14 );\r
316 \r
317         n_LF_Q10 = SKP_LSHIFT( SKP_SMULWB( NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx - 1 ], LF_shp_Q14 ), 2 ); \r
318         n_LF_Q10 = SKP_SMLAWT( n_LF_Q10, NSQ->sLF_AR_shp_Q12, LF_shp_Q14 );\r
319 \r
320         SKP_assert( lag > 0 || sigtype == SIG_TYPE_UNVOICED );\r
321 \r
322         /* Long-term shaping */\r
323         if( lag > 0 ) {\r
324             /* Symmetric, packed FIR coefficients */\r
325             n_LTP_Q14 = SKP_SMULWB( SKP_ADD32( shp_lag_ptr[ 0 ], shp_lag_ptr[ -2 ] ), HarmShapeFIRPacked_Q14 );\r
326             n_LTP_Q14 = SKP_SMLAWT( n_LTP_Q14, shp_lag_ptr[ -1 ],                     HarmShapeFIRPacked_Q14 );\r
327             shp_lag_ptr++;\r
328             n_LTP_Q14 = SKP_LSHIFT( n_LTP_Q14, 6 );\r
329         } else {\r
330             n_LTP_Q14 = 0;\r
331         }\r
332 \r
333         /* Input minus prediction plus noise feedback  */\r
334         //r = x[ i ] - LTP_pred - LPC_pred + n_AR + n_Tilt + n_LF + n_LTP;\r
335         tmp1  = SKP_SUB32( LTP_pred_Q14, n_LTP_Q14 );                       /* Add Q14 stuff */\r
336         tmp1  = SKP_RSHIFT( tmp1, 4 );                                      /* convert to Q10  */\r
337         tmp1  = SKP_ADD32( tmp1, LPC_pred_Q10 );                            /* add Q10 stuff */ \r
338         tmp1  = SKP_SUB32( tmp1, n_AR_Q10 );                                /* subtract Q10 stuff */ \r
339         tmp1  = SKP_SUB32( tmp1, n_LF_Q10 );                                /* subtract Q10 stuff */ \r
340         r_Q10 = SKP_SUB32( x_sc_Q10[ i ], tmp1 );\r
341 \r
342         /* Flip sign depending on dither */\r
343         r_Q10 = ( r_Q10 ^ dither ) - dither;\r
344         r_Q10 = SKP_SUB32( r_Q10, offset_Q10 );\r
345         r_Q10 = SKP_LIMIT_32( r_Q10, -64 << 10, 64 << 10 );\r
346 \r
347         /* Quantize */\r
348         if( r_Q10 < thr1_Q10 ) {\r
349             q_Q0 = SKP_RSHIFT_ROUND( SKP_ADD_RSHIFT32( r_Q10, Lambda_Q10, 1 ), 10 );\r
350             q_Q10 = SKP_LSHIFT( q_Q0, 10 );\r
351         } else if( r_Q10 < thr2_Q10 ) {\r
352             q_Q0 = -1;\r
353             q_Q10 = -1024;\r
354         } else if( r_Q10 > thr3_Q10 ) {\r
355             q_Q0 = SKP_RSHIFT_ROUND( SKP_SUB_RSHIFT32( r_Q10, Lambda_Q10, 1 ), 10 );\r
356             q_Q10 = SKP_LSHIFT( q_Q0, 10 );\r
357         } else {\r
358             q_Q0 = 0;\r
359             q_Q10 = 0;\r
360         }\r
361         q[ i ] = ( SKP_int8 )q_Q0; /* No saturation needed because max is 64 */\r
362 \r
363         /* Excitation */\r
364         exc_Q10 = SKP_ADD32( q_Q10, offset_Q10 );\r
365         exc_Q10 = ( exc_Q10 ^ dither ) - dither;\r
366 \r
367         /* Add predictions */\r
368         LPC_exc_Q10 = SKP_ADD32( exc_Q10, SKP_RSHIFT_ROUND( LTP_pred_Q14, 4 ) );\r
369         xq_Q10      = SKP_ADD32( LPC_exc_Q10, LPC_pred_Q10 );\r
370         \r
371         /* Scale XQ back to normal level before saving */\r
372         xq[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( SKP_SMULWW( xq_Q10, Gain_Q16 ), 10 ) );\r
373         \r
374 #ifdef SAVE_ALL_INTERNAL_DATA\r
375         q_in_env[   i ] = ( (SKP_float)r_Q10       * (SKP_float)Gain_Q16 ) / ( 1024.0f * 65536.0f );\r
376         q_exc[      i ] = ( (SKP_float)exc_Q10     * (SKP_float)Gain_Q16 ) / ( 1024.0f * 65536.0f );\r
377         q_LPC_exc[  i ] = ( (SKP_float)LPC_exc_Q10 * (SKP_float)Gain_Q16 ) / ( 1024.0f * 65536.0f );\r
378 #endif\r
379         \r
380         /* Update states */\r
381         psLPC_Q14++;\r
382         *psLPC_Q14 = SKP_LSHIFT( xq_Q10, 4 );\r
383         sLF_AR_shp_Q10 = SKP_SUB32( xq_Q10, n_AR_Q10 );\r
384         NSQ->sLF_AR_shp_Q12 = SKP_LSHIFT( sLF_AR_shp_Q10, 2 );\r
385 \r
386         NSQ->sLTP_shp_Q10[ NSQ->sLTP_shp_buf_idx ] = SKP_SUB32( sLF_AR_shp_Q10, n_LF_Q10 );\r
387         sLTP_Q16[ NSQ->sLTP_buf_idx ] = SKP_LSHIFT( LPC_exc_Q10, 6 );\r
388         NSQ->sLTP_shp_buf_idx++;\r
389         NSQ->sLTP_buf_idx++;\r
390 \r
391         /* Make dither dependent on quantized signal */\r
392         NSQ->rand_seed += q[ i ];\r
393     }\r
394     /* Update LPC synth buffer */\r
395     SKP_memcpy( NSQ->sLPC_Q14, &NSQ->sLPC_Q14[ length ], MAX_LPC_ORDER * sizeof( SKP_int32 ) );\r
396 }\r
397 \r
398 SKP_INLINE void SKP_Silk_nsq_scale_states(\r
399     SKP_Silk_nsq_state  *NSQ,               /* I/O NSQ state                        */\r
400     const SKP_int16     x[],                /* I input in Q0                        */\r
401     SKP_int32           x_sc_Q10[],         /* O input scaled with 1/Gain           */\r
402     SKP_int             length,             /* I length of input                    */\r
403     SKP_int16           sLTP[],             /* I re-whitened LTP state in Q0        */\r
404     SKP_int32           sLTP_Q16[],         /* O LTP state matching scaled input    */\r
405     SKP_int             subfr,              /* I subframe number                    */\r
406     const SKP_int       LTP_scale_Q14,      /* I                                    */\r
407     const SKP_int32     Gains_Q16[ MAX_NB_SUBFR ], /* I                                 */\r
408     const SKP_int       pitchL[ MAX_NB_SUBFR ]  /* I                                    */\r
409 )\r
410 {\r
411     SKP_int   i, scale_length, lag;\r
412     SKP_int32 inv_gain_Q16, gain_adj_Q16, inv_gain_Q32;\r
413 \r
414     inv_gain_Q16 = SKP_INVERSE32_varQ( SKP_max( Gains_Q16[ subfr ], 1 ), 32 );\r
415     inv_gain_Q16 = SKP_min( inv_gain_Q16, SKP_int16_MAX );\r
416     lag          = pitchL[ subfr ];\r
417 \r
418     /* After rewhitening the LTP state is un-scaled */\r
419     if( NSQ->rewhite_flag ) {\r
420         inv_gain_Q32 = SKP_LSHIFT( inv_gain_Q16, 16 );\r
421         if( subfr == 0 ) {\r
422             /* Do LTP downscaling */\r
423             inv_gain_Q32 = SKP_LSHIFT( SKP_SMULWB( inv_gain_Q32, LTP_scale_Q14 ), 2 );\r
424         }\r
425         for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {\r
426             sLTP_Q16[ i ] = SKP_SMULWB( inv_gain_Q32, sLTP[ i ] );\r
427         }\r
428     }\r
429 \r
430     /* Prepare for Worst case. Next frame starts with max lag voiced */\r
431     scale_length = length * MAX_NB_SUBFR;                                           /* approx max lag */\r
432     scale_length = scale_length - SKP_SMULBB( MAX_NB_SUBFR - (subfr + 1), length ); /* subtract samples that will be too old in next frame */\r
433     scale_length = SKP_max_int( scale_length, lag + LTP_ORDER );                /* make sure to scale whole pitch period if voiced */\r
434 \r
435     /* Adjust for changing gain */\r
436     if( inv_gain_Q16 != NSQ->prev_inv_gain_Q16 ) {\r
437         gain_adj_Q16 =  SKP_DIV32_varQ( inv_gain_Q16, NSQ->prev_inv_gain_Q16, 16 );\r
438 \r
439         for( i = NSQ->sLTP_shp_buf_idx - scale_length; i < NSQ->sLTP_shp_buf_idx; i++ ) {\r
440             NSQ->sLTP_shp_Q10[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLTP_shp_Q10[ i ] );\r
441         }\r
442 \r
443         /* Scale LTP predict state */\r
444         if( NSQ->rewhite_flag == 0 ) {\r
445             for( i = NSQ->sLTP_buf_idx - lag - LTP_ORDER / 2; i < NSQ->sLTP_buf_idx; i++ ) {\r
446                 sLTP_Q16[ i ] = SKP_SMULWW( gain_adj_Q16, sLTP_Q16[ i ] );\r
447             }\r
448         }\r
449         NSQ->sLF_AR_shp_Q12 = SKP_SMULWW( gain_adj_Q16, NSQ->sLF_AR_shp_Q12 );\r
450 \r
451         /* scale short term state */\r
452         for( i = 0; i < MAX_LPC_ORDER; i++ ) {\r
453             NSQ->sLPC_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sLPC_Q14[ i ] );\r
454         }\r
455         for( i = 0; i < MAX_SHAPE_LPC_ORDER; i++ ) {\r
456             NSQ->sAR2_Q14[ i ] = SKP_SMULWW( gain_adj_Q16, NSQ->sAR2_Q14[ i ] );\r
457         }\r
458     }\r
459 \r
460     /* Scale input */\r
461     for( i = 0; i < length; i++ ) {\r
462         x_sc_Q10[ i ] = SKP_RSHIFT( SKP_SMULBB( x[ i ], ( SKP_int16 )inv_gain_Q16 ), 6 );\r
463     }\r
464 \r
465     /* save inv_gain */\r
466     SKP_assert( inv_gain_Q16 != 0 );\r
467     NSQ->prev_inv_gain_Q16 = inv_gain_Q16;\r
468 }