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