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