SILK update
[opus.git] / src_FIX / SKP_Silk_find_pred_coefs_FIX.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_FIX.h"\r
29 \r
30 #ifdef SAVE_ALL_INTERNAL_DATA\r
31 #include <math.h>\r
32 \r
33 void SKP_Silk_LTP_ana_core(\r
34     SKP_float                       r_LPC[],        /* I    LPC residual            */\r
35     SKP_float                       r_LTP[],        /* O    LTP residual            */\r
36     const SKP_int                   pitchL[],       /* I    pitch lags              */\r
37     const SKP_float                 LTPCoef[],      /* I    LTP Coeficients         */\r
38     SKP_int                         subfr_length,   /* I    smpls in one sub frame  */\r
39     SKP_int                         LTP_mem_length  /* I    Length of LTP state of input */\r
40 );\r
41 \r
42 void SKP_Silk_LPC_analysis_filter_FLP(\r
43     SKP_float                       r_LPC[],        /* O    LPC residual signal             */\r
44     const SKP_float                 PredCoef[],     /* I    LPC coeficicnts                 */\r
45     const SKP_float                 s[],            /* I    Input Signal                    */\r
46     SKP_int                         length,         /* I    length of signal                */\r
47     SKP_int                         Order           /* I    LPC order                       */\r
48 );\r
49 \r
50 double SKP_Silk_energy_FLP( \r
51     const SKP_float     *data, \r
52     SKP_int             dataSize\r
53 );\r
54 \r
55 /* integer to floating-point conversion */\r
56 SKP_INLINE void SKP_short2float_array(\r
57         SKP_float               *out, \r
58         const SKP_int16 *in, \r
59         SKP_int32               length\r
60 );\r
61 \r
62 SKP_INLINE SKP_float SKP_Silk_log2( double x ) { return ( SKP_float )( 3.32192809488736 * log10( x ) ); }\r
63 \r
64 #endif\r
65 \r
66 void SKP_Silk_find_pred_coefs_FIX(\r
67     SKP_Silk_encoder_state_FIX      *psEnc,         /* I/O  encoder state                               */\r
68     SKP_Silk_encoder_control_FIX    *psEncCtrl,     /* I/O  encoder control                             */\r
69     const SKP_int16                 res_pitch[]     /* I    Residual from pitch analysis                */\r
70 )\r
71 {\r
72     SKP_int         i;\r
73     SKP_int32       WLTP[ MAX_NB_SUBFR * LTP_ORDER * LTP_ORDER ];\r
74     SKP_int32       invGains_Q16[ MAX_NB_SUBFR ], local_gains[ MAX_NB_SUBFR ], Wght_Q15[ MAX_NB_SUBFR ];\r
75     SKP_int         NLSF_Q15[ MAX_LPC_ORDER ];\r
76     const SKP_int16 *x_ptr;\r
77     SKP_int16       *x_pre_ptr, LPC_in_pre[ MAX_NB_SUBFR * MAX_LPC_ORDER + MAX_FRAME_LENGTH ];\r
78     SKP_int32       tmp, min_gain_Q16;\r
79     SKP_int         LTP_corrs_rshift[ MAX_NB_SUBFR ];\r
80 \r
81 #ifdef SAVE_ALL_INTERNAL_DATA\r
82     SKP_int16 uq_PredCoef_Q12[ MAX_NB_SUBFR >> 1 ][ MAX_LPC_ORDER ];\r
83     SKP_float uq_PredCoef[     MAX_NB_SUBFR >> 1 ][ MAX_LPC_ORDER ];\r
84     SKP_float uq_LTPCoef[ MAX_NB_SUBFR * LTP_ORDER ];\r
85 #endif\r
86 \r
87     /* weighting for weighted least squares */\r
88     min_gain_Q16 = SKP_int32_MAX >> 6;\r
89     for( i = 0; i < psEnc->sCmn.nb_subfr; i++ ) {\r
90         min_gain_Q16 = SKP_min( min_gain_Q16, psEncCtrl->Gains_Q16[ i ] );\r
91     }\r
92     for( i = 0; i < psEnc->sCmn.nb_subfr; i++ ) {\r
93         /* Divide to Q16 */\r
94         SKP_assert( psEncCtrl->Gains_Q16[ i ] > 0 );\r
95         /* Invert and normalize gains, and ensure that maximum invGains_Q16 is within range of a 16 bit int */\r
96         invGains_Q16[ i ] = SKP_DIV32_varQ( min_gain_Q16, psEncCtrl->Gains_Q16[ i ], 16 - 2 );\r
97 \r
98         /* Ensure Wght_Q15 a minimum value 1 */\r
99         invGains_Q16[ i ] = SKP_max( invGains_Q16[ i ], 363 ); \r
100         \r
101         /* Square the inverted gains */\r
102         SKP_assert( invGains_Q16[ i ] == SKP_SAT16( invGains_Q16[ i ] ) );\r
103         tmp = SKP_SMULWB( invGains_Q16[ i ], invGains_Q16[ i ] );\r
104         Wght_Q15[ i ] = SKP_RSHIFT( tmp, 1 );\r
105 \r
106         /* Invert the inverted and normalized gains */\r
107         local_gains[ i ] = SKP_DIV32( ( 1 << 16 ), invGains_Q16[ i ] );\r
108     }\r
109 \r
110     if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {\r
111         /**********/\r
112         /* VOICED */\r
113         /**********/\r
114         SKP_assert( psEnc->sCmn.ltp_mem_length - psEnc->sCmn.predictLPCOrder >= psEncCtrl->sCmn.pitchL[ 0 ] + LTP_ORDER / 2 );\r
115 \r
116         /* LTP analysis */\r
117         SKP_Silk_find_LTP_FIX( psEncCtrl->LTPCoef_Q14, WLTP, &psEncCtrl->LTPredCodGain_Q7, \r
118             res_pitch, psEncCtrl->sCmn.pitchL, Wght_Q15, psEnc->sCmn.subfr_length, \r
119             psEnc->sCmn.nb_subfr, psEnc->sCmn.ltp_mem_length, LTP_corrs_rshift );\r
120 \r
121 #ifdef SAVE_ALL_INTERNAL_DATA\r
122         /* Save unquantized LTP coefficients */\r
123         for( i = 0; i < LTP_ORDER * psEnc->sCmn.nb_subfr; i++ ) {\r
124             uq_LTPCoef[ i ] = (SKP_float)psEncCtrl->LTPCoef_Q14[ i ] / 16384.0f;\r
125         }\r
126 #endif\r
127 \r
128         /* Quantize LTP gain parameters */\r
129         SKP_Silk_quant_LTP_gains_FIX( psEncCtrl->LTPCoef_Q14, psEncCtrl->sCmn.LTPIndex, &psEncCtrl->sCmn.PERIndex, \r
130             WLTP, psEnc->mu_LTP_Q8, psEnc->sCmn.LTPQuantLowComplexity, psEnc->sCmn.nb_subfr);\r
131 \r
132         /* Control LTP scaling */\r
133         SKP_Silk_LTP_scale_ctrl_FIX( psEnc, psEncCtrl );\r
134 \r
135         /* Create LTP residual */\r
136         SKP_Silk_LTP_analysis_filter_FIX( LPC_in_pre, psEnc->x_buf + psEnc->sCmn.ltp_mem_length - psEnc->sCmn.predictLPCOrder, \r
137             psEncCtrl->LTPCoef_Q14, psEncCtrl->sCmn.pitchL, invGains_Q16, psEnc->sCmn.subfr_length, psEnc->sCmn.nb_subfr, psEnc->sCmn.predictLPCOrder );\r
138 \r
139     } else {\r
140         /************/\r
141         /* UNVOICED */\r
142         /************/\r
143         /* Create signal with prepended subframes, scaled by inverse gains */\r
144         x_ptr     = psEnc->x_buf + psEnc->sCmn.ltp_mem_length - psEnc->sCmn.predictLPCOrder;\r
145         x_pre_ptr = LPC_in_pre;\r
146         for( i = 0; i < psEnc->sCmn.nb_subfr; i++ ) {\r
147             SKP_Silk_scale_copy_vector16( x_pre_ptr, x_ptr, invGains_Q16[ i ], \r
148                 psEnc->sCmn.subfr_length + psEnc->sCmn.predictLPCOrder );\r
149             x_pre_ptr += psEnc->sCmn.subfr_length + psEnc->sCmn.predictLPCOrder;\r
150             x_ptr     += psEnc->sCmn.subfr_length;\r
151         }\r
152 \r
153         SKP_memset( psEncCtrl->LTPCoef_Q14, 0, psEnc->sCmn.nb_subfr * LTP_ORDER * sizeof( SKP_int16 ) );\r
154         psEncCtrl->LTPredCodGain_Q7 = 0;\r
155     }\r
156 \r
157     /* LPC_in_pre contains the LTP-filtered input for voiced, and the unfiltered input for unvoiced */\r
158     TIC(FIND_LPC)\r
159     SKP_Silk_find_LPC_FIX( NLSF_Q15, &psEncCtrl->sCmn.NLSFInterpCoef_Q2, psEnc->sPred.prev_NLSFq_Q15, \r
160         psEnc->sCmn.useInterpolatedNLSFs * ( 1 - psEnc->sCmn.first_frame_after_reset ), psEnc->sCmn.predictLPCOrder, \r
161         LPC_in_pre, psEnc->sCmn.subfr_length + psEnc->sCmn.predictLPCOrder, psEnc->sCmn.nb_subfr );\r
162     TOC(FIND_LPC)\r
163 \r
164 #ifdef SAVE_ALL_INTERNAL_DATA /* Save unquantized LPC's */\r
165     if( psEnc->sCmn.useInterpolatedNLSFs == 0 ) {\r
166         /* Convert back to filter representation */\r
167         SKP_Silk_NLSF2A_stable( uq_PredCoef_Q12[ 0 ], NLSF_Q15, psEnc->sCmn.predictLPCOrder );\r
168         SKP_memcpy( uq_PredCoef_Q12[ 1 ], uq_PredCoef_Q12[ 0 ], psEnc->sCmn.predictLPCOrder * sizeof( SKP_int16 ) );\r
169     } else { /* i.e. if( psEnc->useInterpolatedLSFs != 0 ) */\r
170         SKP_int iNLSF_Q15[ MAX_LPC_ORDER ];\r
171 \r
172         /* Update interpolated LSF0 coefficients taking quantization of LSF1 coefficients into account */\r
173         SKP_Silk_interpolate( iNLSF_Q15, psEnc->sPred.prev_NLSFq_Q15, NLSF_Q15, \r
174             psEncCtrl->sCmn.NLSFInterpCoef_Q2, psEnc->sCmn.predictLPCOrder );\r
175 \r
176         /* Convert back to filter representation */\r
177         SKP_Silk_NLSF2A_stable( uq_PredCoef_Q12[ 0 ], iNLSF_Q15, psEnc->sCmn.predictLPCOrder );\r
178 \r
179         /* Convert back to filter representation */\r
180         SKP_Silk_NLSF2A_stable( uq_PredCoef_Q12[ 1 ], NLSF_Q15, psEnc->sCmn.predictLPCOrder );\r
181     }\r
182     \r
183     /* Convert to FLP */\r
184     for( i = 0; i < psEnc->sCmn.predictLPCOrder; i++ ) {\r
185         uq_PredCoef[ 0 ][ i ] = (SKP_float)uq_PredCoef_Q12[ 0 ][ i ] / 4096.0f;\r
186         uq_PredCoef[ 1 ][ i ] = (SKP_float)uq_PredCoef_Q12[ 1 ][ i ] / 4096.0f;\r
187     }\r
188 #endif\r
189 \r
190     /* Quantize LSFs */\r
191     TIC(PROCESS_LSFS)\r
192         SKP_Silk_process_NLSFs_FIX( psEnc, psEncCtrl, NLSF_Q15 );\r
193     TOC(PROCESS_LSFS)\r
194 \r
195     /* Calculate residual energy using quantized LPC coefficients */\r
196     SKP_Silk_residual_energy_FIX( psEncCtrl->ResNrg, psEncCtrl->ResNrgQ, LPC_in_pre, psEncCtrl->PredCoef_Q12, local_gains,\r
197         psEnc->sCmn.subfr_length, psEnc->sCmn.nb_subfr, psEnc->sCmn.predictLPCOrder );\r
198 \r
199     /* Copy to prediction struct for use in next frame for fluctuation reduction */\r
200     SKP_memcpy( psEnc->sPred.prev_NLSFq_Q15, NLSF_Q15, psEnc->sCmn.predictLPCOrder * sizeof( SKP_int ) );\r
201 \r
202 #ifdef SAVE_ALL_INTERNAL_DATA\r
203     {\r
204         SKP_int   j, k;\r
205         SKP_float in_nrg, *in_ptr;\r
206         SKP_float LPC_res_nrg, qLPC_res_nrg, LTP_res_nrg, qLTP_res_nrg;\r
207         SKP_float LPC_predCodGain, QLPC_predCodGain, QLTP_predCodGain, LTPredCodGain, predCodGain;\r
208         SKP_float LPC_res[ MAX_FRAME_LENGTH << 1 ], LTP_res[ MAX_FRAME_LENGTH ];\r
209         SKP_float SF_resNrg[ MAX_NB_SUBFR ];\r
210 \r
211         SKP_float x_flp[ 2 * MAX_FRAME_LENGTH ];\r
212         SKP_float Wght[ MAX_NB_SUBFR ];\r
213         SKP_float PredCoef[ 2 ][ MAX_LPC_ORDER ];\r
214         SKP_float LTPCoef[ MAX_NB_SUBFR * LTP_ORDER ];\r
215 \r
216         /* Convert various FIX data to FLP */\r
217         SKP_short2float_array( x_flp, psEnc->x_buf, psEnc->sCmn.ltp_mem_length + psEnc->sCmn.frame_length );\r
218         for( k = 0; k < psEnc->sCmn.nb_subfr; k++ ) {\r
219             Wght[ k ] = ( (SKP_float)Wght_Q15[ k ] / 32768.0f );\r
220         }\r
221         for( i = 0; i < psEnc->sCmn.predictLPCOrder; i++ ) {\r
222             PredCoef[ 0 ][ i ] = (SKP_float)psEncCtrl->PredCoef_Q12[ 0 ][ i ] / 4096.0f;\r
223             PredCoef[ 1 ][ i ] = (SKP_float)psEncCtrl->PredCoef_Q12[ 1 ][ i ] / 4096.0f;\r
224         }\r
225         for( i = 0; i < psEnc->sCmn.nb_subfr * LTP_ORDER; i++ ) {\r
226             LTPCoef[ i ] = (SKP_float)psEncCtrl->LTPCoef_Q14[ i ] / 16384.0f;\r
227         }\r
228 \r
229         /* Weighted input energy */\r
230         in_ptr = &x_flp[ psEnc->sCmn.ltp_mem_length ];\r
231         DEBUG_STORE_DATA( x_flp.dat,  x_flp,  psEnc->sCmn.frame_length * sizeof( SKP_float ) );\r
232         DEBUG_STORE_DATA( in_ptr.dat, in_ptr, psEnc->sCmn.frame_length * sizeof( SKP_float ) );\r
233         in_nrg = 0.0f;\r
234         for( k = 0; k < psEnc->sCmn.nb_subfr; k++ ) {\r
235             in_nrg += (SKP_float)SKP_Silk_energy_FLP( in_ptr, psEnc->sCmn.subfr_length ) * Wght[ k ];\r
236             in_ptr += psEnc->sCmn.subfr_length;\r
237         }\r
238 \r
239         if( psEnc->sCmn.useInterpolatedNLSFs == 0 ) {\r
240             SKP_memcpy( PredCoef[ 0 ], PredCoef[ 1 ], psEnc->sCmn.predictLPCOrder * sizeof( SKP_float ) );\r
241         }\r
242 \r
243         DEBUG_STORE_DATA( uq_PredCoef.dat, uq_PredCoef[0], psEnc->sCmn.predictLPCOrder * sizeof( SKP_float ) );\r
244         DEBUG_STORE_DATA( PredCoef.dat,    PredCoef[0],    psEnc->sCmn.predictLPCOrder * sizeof( SKP_float ) );\r
245 \r
246         LPC_res_nrg  = 0.0f;\r
247         LTP_res_nrg  = 0.0f;\r
248         qLPC_res_nrg = 0.0f;\r
249         qLTP_res_nrg = 0.0f;\r
250         for( j = 0; j < psEnc->sCmn.nb_subfr; j += 2 ) {\r
251             /* Calculate LPC residual with unquantized LPC */\r
252             SKP_Silk_LPC_analysis_filter_FLP( LPC_res, uq_PredCoef[ j >> 1 ], x_flp + j * psEnc->sCmn.subfr_length,\r
253                 ( psEnc->sCmn.ltp_mem_length + ( psEnc->sCmn.subfr_length << 1 ) ), psEnc->sCmn.predictLPCOrder );\r
254 \r
255             /* Weighted energy */\r
256             in_ptr = &LPC_res[ psEnc->sCmn.ltp_mem_length ];\r
257             for( k = 0; k < 2; k++ ) {\r
258                 LPC_res_nrg += (SKP_float)SKP_Silk_energy_FLP( in_ptr, psEnc->sCmn.subfr_length ) * Wght[ j + k ];\r
259                 in_ptr      += psEnc->sCmn.subfr_length;\r
260             }\r
261                 \r
262             if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {\r
263                 /* Calculate LTP residual with unquantized LTP and unquantized LPC */\r
264                 SKP_Silk_LTP_ana_core( LPC_res, LTP_res, &psEncCtrl->sCmn.pitchL[ j ],\r
265                     &uq_LTPCoef[ j * LTP_ORDER ], psEnc->sCmn.subfr_length, psEnc->sCmn.ltp_mem_length );\r
266 \r
267                 /* Weighted energy */\r
268                 in_ptr = LTP_res;\r
269                 for( k = 0; k < 2; k++ ) {\r
270                     LTP_res_nrg += (SKP_float)SKP_Silk_energy_FLP( in_ptr, psEnc->sCmn.subfr_length ) * Wght[ j + k ];\r
271                     in_ptr      += psEnc->sCmn.subfr_length;\r
272                 }\r
273             }\r
274 \r
275             /* Calculate LPC residual with quantized LPC */\r
276             SKP_Silk_LPC_analysis_filter_FLP( LPC_res, PredCoef[ j >> 1 ], x_flp + j * psEnc->sCmn.subfr_length,\r
277                 ( psEnc->sCmn.ltp_mem_length + ( psEnc->sCmn.subfr_length << 1 ) ), psEnc->sCmn.predictLPCOrder );\r
278 \r
279             /* Weighted energy */\r
280             in_ptr = &LPC_res[ psEnc->sCmn.ltp_mem_length ];\r
281             for( k = 0; k < 2; k++ ) {\r
282                 SF_resNrg[ k + j ] = (SKP_float)SKP_Silk_energy_FLP( in_ptr, psEnc->sCmn.subfr_length );\r
283                 qLPC_res_nrg += SF_resNrg[ k + j ] * Wght[ j + k ];\r
284                 in_ptr       += psEnc->sCmn.subfr_length;\r
285             }\r
286 \r
287             if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {\r
288                 /* Calculate LTP residual with unquantized LTP and unquantized LPC */\r
289                 SKP_Silk_LTP_ana_core( LPC_res, LTP_res, &psEncCtrl->sCmn.pitchL[ j ],\r
290                     &LTPCoef[ j * LTP_ORDER ], psEnc->sCmn.subfr_length, psEnc->sCmn.ltp_mem_length );\r
291 \r
292                 /* Weighted energy */\r
293                 in_ptr = LTP_res;\r
294                 for( k = 0; k < 2; k++ ) {\r
295                     SF_resNrg[ k + j ] = (SKP_float)SKP_Silk_energy_FLP( in_ptr, psEnc->sCmn.subfr_length );\r
296                     qLTP_res_nrg += SF_resNrg[ k + j ] * Wght[ j + k ];\r
297                     in_ptr       += psEnc->sCmn.subfr_length;\r
298                 }\r
299             } else {\r
300                 SKP_memcpy( LTP_res, &LPC_res[ psEnc->sCmn.ltp_mem_length ], ( psEnc->sCmn.subfr_length << 1 ) * sizeof( SKP_float ) );\r
301             }\r
302             /* Save residual */\r
303             DEBUG_STORE_DATA( LPC_res.dat, &LPC_res[ psEnc->sCmn.ltp_mem_length ], ( psEnc->sCmn.subfr_length << 1 ) * sizeof( SKP_float ) );\r
304             DEBUG_STORE_DATA( res.dat,     LTP_res,                                ( psEnc->sCmn.subfr_length << 1 ) * sizeof( SKP_float ) );\r
305         }\r
306         if( psEncCtrl->sCmn.sigtype == SIG_TYPE_VOICED ) {\r
307             LPC_predCodGain  = 3.0f * SKP_Silk_log2( in_nrg       / LPC_res_nrg  );\r
308             QLPC_predCodGain = 3.0f * SKP_Silk_log2( in_nrg       / qLPC_res_nrg );\r
309             LTPredCodGain    = 3.0f * SKP_Silk_log2( LPC_res_nrg  / LTP_res_nrg  );\r
310             QLTP_predCodGain = 3.0f * SKP_Silk_log2( qLPC_res_nrg / qLTP_res_nrg );\r
311         } else {\r
312             LPC_predCodGain  = 3.0f * SKP_Silk_log2( in_nrg       / LPC_res_nrg  );\r
313             QLPC_predCodGain = 3.0f * SKP_Silk_log2( in_nrg       / qLPC_res_nrg );\r
314             LTPredCodGain    = 0.0f;\r
315             QLTP_predCodGain = 0.0f;\r
316         }\r
317         predCodGain = QLPC_predCodGain + QLTP_predCodGain;\r
318 \r
319         DEBUG_STORE_DATA( LTPredCodGain.dat,    &LTPredCodGain,                                        sizeof( SKP_float ) );\r
320         DEBUG_STORE_DATA( QLTP_predCodGain.dat, &QLTP_predCodGain,                                     sizeof( SKP_float ) ); \r
321         DEBUG_STORE_DATA( LPC_predCodGain.dat,  &LPC_predCodGain,                                      sizeof( SKP_float ) );\r
322         DEBUG_STORE_DATA( QLPC_predCodGain.dat, &QLPC_predCodGain,                                     sizeof( SKP_float ) );\r
323         DEBUG_STORE_DATA( predCodGain.dat,      &predCodGain,                                          sizeof( SKP_float ) ); \r
324         DEBUG_STORE_DATA( ResNrg.dat,           SF_resNrg,                      psEnc->sCmn.nb_subfr * sizeof( SKP_float ) );\r
325     }\r
326 #endif\r
327 }\r
328 \r
329 #ifdef SAVE_ALL_INTERNAL_DATA\r
330 /****************************************************/\r
331 /* LTP analysis filter. Filters two subframes       */\r
332 /****************************************************/\r
333 void SKP_Silk_LTP_ana_core(\r
334     SKP_float                       r_LPC[],        /* I    LPC residual            */\r
335     SKP_float                       r_LTP[],        /* O    LTP residual            */\r
336     const SKP_int                   pitchL[],       /* I    pitch lags              */\r
337     const SKP_float                 LTPCoef[],      /* I    LTP Coeficients         */\r
338     SKP_int                         subfr_length,   /* I    smpls in one sub frame  */\r
339     SKP_int                         LTP_mem_length  /* I    Length of LTP state of input */\r
340 )\r
341 {\r
342     SKP_int   k, i;\r
343     SKP_float LTP_pred;\r
344     const SKP_float *r, *b_ptr, *lag_ptr;\r
345 \r
346     r = &r_LPC[ LTP_mem_length ];\r
347     b_ptr = LTPCoef;\r
348     for( k = 0; k < (MAX_NB_SUBFR >> 1); k++ ) {\r
349         lag_ptr = r - pitchL[k];\r
350         /* LTP analysis FIR filter */\r
351         for( i = 0; i < subfr_length; i++ ) {\r
352             /* long-term prediction */\r
353             LTP_pred  = lag_ptr[LTP_ORDER/2]     * b_ptr[0];\r
354             LTP_pred += lag_ptr[LTP_ORDER/2 - 1] * b_ptr[1];\r
355             LTP_pred += lag_ptr[LTP_ORDER/2 - 2] * b_ptr[2];\r
356             LTP_pred += lag_ptr[LTP_ORDER/2 - 3] * b_ptr[3];\r
357             LTP_pred += lag_ptr[LTP_ORDER/2 - 4] * b_ptr[4];\r
358 \r
359             /* subtract prediction */\r
360             r_LTP[i] = r[i] - LTP_pred;\r
361             lag_ptr++;\r
362         }\r
363         r += subfr_length;\r
364         r_LTP += subfr_length;\r
365         b_ptr += LTP_ORDER;\r
366     }\r
367 }\r
368 \r
369 /*******************************************/\r
370 /* LPC analysis filter                     */\r
371 /* NB! State is kept internally and the    */\r
372 /* filter always starts with zero state    */\r
373 /* first Order output samples are not set  */\r
374 /*******************************************/\r
375 void SKP_Silk_LPC_analysis_filter_FLP(\r
376     SKP_float                       r_LPC[],        /* O    LPC residual signal             */\r
377     const SKP_float                 PredCoef[],     /* I    LPC coeficicnts                 */\r
378     const SKP_float                 s[],            /* I    Input Signal                    */\r
379     SKP_int                         length,         /* I    length of signal                */\r
380     SKP_int                         Order           /* I    LPC order                       */\r
381 )\r
382 {\r
383     SKP_int   i, j;\r
384     SKP_float LPC_pred;\r
385     const SKP_float *s_ptr;\r
386 \r
387     for ( i = Order; i < length; i++ ) {\r
388         s_ptr = &s[i - 1];\r
389 \r
390         LPC_pred = 0;\r
391         /* short-term prediction */\r
392         for( j = 0; j < Order; j++ ) {\r
393             LPC_pred += s_ptr[ -j ] * PredCoef[ j ];\r
394         }\r
395 \r
396         /* prediction error */\r
397         r_LPC[ i ] = s_ptr[ 1 ] - LPC_pred;\r
398     }\r
399 }\r
400 \r
401 /* sum of squares of a SKP_float array, with result as double */\r
402 double SKP_Silk_energy_FLP( \r
403     const SKP_float     *data, \r
404     SKP_int             dataSize\r
405 )\r
406 {\r
407     SKP_int  i, dataSize4;\r
408     double   result;\r
409 \r
410     /* 4x unrolled loop */\r
411     result = 0.0f;\r
412     dataSize4 = dataSize & 0xFFFC;\r
413     for( i = 0; i < dataSize4; i += 4 ) {\r
414         result += data[ i + 0 ] * data[ i + 0 ] + \r
415                   data[ i + 1 ] * data[ i + 1 ] +\r
416                   data[ i + 2 ] * data[ i + 2 ] +\r
417                   data[ i + 3 ] * data[ i + 3 ];\r
418     }\r
419 \r
420     /* add any remaining products */\r
421     for( ; i < dataSize; i++ ) {\r
422         result += data[ i ] * data[ i ];\r
423     }\r
424 \r
425     SKP_assert( result >= 0.0 );\r
426     return result;\r
427 }\r
428 #endif\r