2f0881e9ffaeee704a322c09f2a2d8954fe453a8
[opus.git] / src_FIX / SKP_Silk_find_LTP_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 void SKP_Silk_fit_LTP(\r
31     SKP_int32 LTP_coefs_Q16[ LTP_ORDER ],\r
32     SKP_int16 LTP_coefs_Q14[ LTP_ORDER ]\r
33 );\r
34 \r
35 void SKP_Silk_find_LTP_FIX(\r
36     SKP_int16           b_Q14[ MAX_NB_SUBFR * LTP_ORDER ],              /* O    LTP coefs                                                   */\r
37     SKP_int32           WLTP[ MAX_NB_SUBFR * LTP_ORDER * LTP_ORDER ],   /* O    Weight for LTP quantization                                 */\r
38     SKP_int             *LTPredCodGain_Q7,                              /* O    LTP coding gain                                             */\r
39     const SKP_int16     r_lpc[]  ,                                      /* I    residual signal after LPC signal + state for first 10 ms    */\r
40     const SKP_int       lag[ MAX_NB_SUBFR ],                            /* I    LTP lags                                                    */\r
41     const SKP_int32     Wght_Q15[ MAX_NB_SUBFR ],                       /* I    weights                                                     */\r
42     const SKP_int       subfr_length,                                   /* I    subframe length                                             */\r
43     const SKP_int       nb_subfr,                                       /* I    number of subframes                                         */\r
44     const SKP_int       mem_offset,                                     /* I    number of samples in LTP memory                             */\r
45     SKP_int             corr_rshifts[ MAX_NB_SUBFR ]                    /* O    right shifts applied to correlations                        */\r
46 )\r
47 {\r
48     SKP_int   i, k, lshift;\r
49     const SKP_int16 *r_ptr, *lag_ptr;\r
50     SKP_int16 *b_Q14_ptr;\r
51 \r
52     SKP_int32 regu;\r
53     SKP_int32 *WLTP_ptr;\r
54     SKP_int32 b_Q16[ LTP_ORDER ], delta_b_Q14[ LTP_ORDER ], d_Q14[ MAX_NB_SUBFR ], nrg[ MAX_NB_SUBFR ], g_Q26;\r
55     SKP_int32 w[ MAX_NB_SUBFR ], WLTP_max, max_abs_d_Q14, max_w_bits;\r
56 \r
57     SKP_int32 temp32, denom32;\r
58     SKP_int   extra_shifts;\r
59     SKP_int   rr_shifts, maxRshifts, maxRshifts_wxtra, LZs;\r
60     SKP_int32 LPC_res_nrg, LPC_LTP_res_nrg, div_Q16;\r
61     SKP_int32 Rr[ LTP_ORDER ], rr[ MAX_NB_SUBFR ];\r
62     SKP_int32 wd, m_Q12;\r
63     \r
64     b_Q14_ptr = b_Q14;\r
65     WLTP_ptr  = WLTP;\r
66     r_ptr     = &r_lpc[ mem_offset ];\r
67     for( k = 0; k < nb_subfr; k++ ) {\r
68         lag_ptr = r_ptr - ( lag[ k ] + LTP_ORDER / 2 );\r
69 \r
70         SKP_Silk_sum_sqr_shift( &rr[ k ], &rr_shifts, r_ptr, subfr_length ); /* rr[ k ] in Q( -rr_shifts ) */\r
71         /* Assure headroom */\r
72         LZs = SKP_Silk_CLZ32( rr[k] );\r
73         if( LZs < LTP_CORRS_HEAD_ROOM ) {\r
74             rr[ k ] = SKP_RSHIFT_ROUND( rr[ k ], LTP_CORRS_HEAD_ROOM - LZs );\r
75             rr_shifts += (LTP_CORRS_HEAD_ROOM - LZs);\r
76         }\r
77         corr_rshifts[ k ] = rr_shifts;\r
78         SKP_Silk_corrMatrix_FIX( lag_ptr, subfr_length, LTP_ORDER, WLTP_ptr, &corr_rshifts[ k ] );     /* WLTP_fix_ptr in Q( -corr_rshifts[ k ] ) */\r
79         /* The correlation vector always have lower max abs value than rr and/or RR so head room is assured */\r
80         SKP_Silk_corrVector_FIX( lag_ptr, r_ptr, subfr_length, LTP_ORDER, Rr, corr_rshifts[ k ] ); /* Rr_fix_ptr   in Q( -corr_rshifts[ k ] ) */\r
81         if( corr_rshifts[ k ] > rr_shifts ) {\r
82             rr[ k ] = SKP_RSHIFT( rr[ k ], corr_rshifts[ k ] - rr_shifts ); /* rr[ k ] in Q( -corr_rshifts[ k ] ) */\r
83         }\r
84         SKP_assert( rr[ k ] >= 0 );\r
85 \r
86         regu = SKP_SMULWB( rr[ k ] + 1, LTP_DAMPING_Q16 );\r
87         SKP_Silk_regularize_correlations_FIX( WLTP_ptr, &rr[k], regu, LTP_ORDER );\r
88 \r
89         SKP_Silk_solve_LDL_FIX( WLTP_ptr, LTP_ORDER, Rr, b_Q16 ); /* WLTP_fix_ptr and Rr_fix_ptr both in Q(-corr_rshifts[k]) */\r
90 \r
91         /* Limit and store in Q14 */\r
92         SKP_Silk_fit_LTP( b_Q16, b_Q14_ptr );\r
93 \r
94         /* Calculate residual energy */\r
95         nrg[ k ] = SKP_Silk_residual_energy16_covar_FIX( b_Q14_ptr, WLTP_ptr, Rr, rr[ k ], LTP_ORDER, 14 ); /* nrg_fix in Q( -corr_rshifts[ k ] ) */\r
96 \r
97         /* temp = Wght[ k ] / ( nrg[ k ] * Wght[ k ] + 0.01f * subfr_length ); */\r
98         extra_shifts = SKP_min_int( corr_rshifts[ k ], LTP_CORRS_HEAD_ROOM );\r
99         denom32 = SKP_LSHIFT_SAT32( SKP_SMULWB( nrg[ k ], Wght_Q15[ k ] ), 1 + extra_shifts ) +  /* Q( -corr_rshifts[ k ] + extra_shifts ) */\r
100             SKP_RSHIFT( SKP_SMULWB( subfr_length, 655 ), corr_rshifts[ k ] - extra_shifts );     /* Q( -corr_rshifts[ k ] + extra_shifts ) */\r
101         denom32 = SKP_max( denom32, 1 );\r
102         SKP_assert( ((SKP_int64)Wght_Q15[ k ] << 16 ) < SKP_int32_MAX ); /* Wght always < 0.5 in Q0 */\r
103         temp32 = SKP_DIV32( SKP_LSHIFT( ( SKP_int32 )Wght_Q15[ k ], 16 ), denom32 );  /* Q( 15 + 16 + corr_rshifts[k] - extra_shifts ) */\r
104         temp32 = SKP_RSHIFT( temp32, 31 + corr_rshifts[ k ] - extra_shifts - 26 );  /* Q26 */\r
105         \r
106         /* Limit temp such that the below scaling never wraps around */\r
107         WLTP_max = 0;\r
108         for( i = 0; i < LTP_ORDER * LTP_ORDER; i++ ) {\r
109             WLTP_max = SKP_max( WLTP_ptr[ i ], WLTP_max );\r
110         }\r
111         lshift = SKP_Silk_CLZ32( WLTP_max ) - 1 - 3; /* keep 3 bits free for vq_nearest_neighbor_fix */\r
112         SKP_assert( 26 - 18 + lshift >= 0 );\r
113         if( 26 - 18 + lshift < 31 ) {\r
114             temp32 = SKP_min_32( temp32, SKP_LSHIFT( ( SKP_int32 )1, 26 - 18 + lshift ) );\r
115         }\r
116 \r
117         SKP_Silk_scale_vector32_Q26_lshift_18( WLTP_ptr, temp32, LTP_ORDER * LTP_ORDER ); /* WLTP_ptr in Q( 18 - corr_rshifts[ k ] ) */\r
118         \r
119         w[ k ] = matrix_ptr( WLTP_ptr, ( LTP_ORDER >> 1 ), ( LTP_ORDER >> 1 ), LTP_ORDER ); /* w in Q( 18 - corr_rshifts[ k ] ) */\r
120         SKP_assert( w[k] >= 0 );\r
121 \r
122         r_ptr     += subfr_length;\r
123         b_Q14_ptr += LTP_ORDER;\r
124         WLTP_ptr  += LTP_ORDER * LTP_ORDER;\r
125     }\r
126 \r
127     maxRshifts = 0;\r
128     for( k = 0; k < nb_subfr; k++ ) {\r
129         maxRshifts = SKP_max_int( corr_rshifts[ k ], maxRshifts );\r
130     }\r
131 \r
132     /* compute LTP coding gain */\r
133     if( LTPredCodGain_Q7 != NULL ) {\r
134         LPC_LTP_res_nrg = 0;\r
135         LPC_res_nrg     = 0;\r
136         SKP_assert( LTP_CORRS_HEAD_ROOM >= 2 ); /* Check that no overflow will happen when adding */\r
137         for( k = 0; k < nb_subfr; k++ ) {\r
138             LPC_res_nrg     = SKP_ADD32( LPC_res_nrg,     SKP_RSHIFT( SKP_ADD32( SKP_SMULWB(  rr[ k ], Wght_Q15[ k ] ), 1 ), 1 + ( maxRshifts - corr_rshifts[ k ] ) ) ); /*  Q( -maxRshifts ) */\r
139             LPC_LTP_res_nrg = SKP_ADD32( LPC_LTP_res_nrg, SKP_RSHIFT( SKP_ADD32( SKP_SMULWB( nrg[ k ], Wght_Q15[ k ] ), 1 ), 1 + ( maxRshifts - corr_rshifts[ k ] ) ) ); /*  Q( -maxRshifts ) */\r
140         }\r
141         LPC_LTP_res_nrg = SKP_max( LPC_LTP_res_nrg, 1 ); /* avoid division by zero */\r
142 \r
143         div_Q16 = SKP_DIV32_varQ( LPC_res_nrg, LPC_LTP_res_nrg, 16 );\r
144         *LTPredCodGain_Q7 = ( SKP_int )SKP_SMULBB( 3, SKP_Silk_lin2log( div_Q16 ) - ( 16 << 7 ) );\r
145 \r
146         SKP_assert( *LTPredCodGain_Q7 == ( SKP_int )SKP_SAT16( SKP_MUL( 3, SKP_Silk_lin2log( div_Q16 ) - ( 16 << 7 ) ) ) );\r
147     }\r
148 \r
149     /* smoothing */\r
150     /* d = sum( B, 1 ); */\r
151     b_Q14_ptr = b_Q14;\r
152     for( k = 0; k < nb_subfr; k++ ) {\r
153         d_Q14[ k ] = 0;\r
154         for( i = 0; i < LTP_ORDER; i++ ) {\r
155             d_Q14[ k ] += b_Q14_ptr[ i ];\r
156         }\r
157         b_Q14_ptr += LTP_ORDER;\r
158     }\r
159 \r
160     /* m = ( w * d' ) / ( sum( w ) + 1e-3 ); */\r
161         \r
162     /* Find maximum absolute value of d_Q14 and the bits used by w in Q0 */\r
163     max_abs_d_Q14 = 0;\r
164     max_w_bits    = 0;\r
165     for( k = 0; k < nb_subfr; k++ ) {\r
166         max_abs_d_Q14 = SKP_max_32( max_abs_d_Q14, SKP_abs( d_Q14[ k ] ) );\r
167         /* w[ k ] is in Q( 18 - corr_rshifts[ k ] ) */\r
168         /* Find bits needed in Q( 18 - maxRshifts ) */\r
169         max_w_bits = SKP_max_32( max_w_bits, 32 - SKP_Silk_CLZ32( w[ k ] ) + corr_rshifts[ k ] - maxRshifts ); \r
170     }\r
171 \r
172     /* max_abs_d_Q14 = (5 << 15); worst case, i.e. LTP_ORDER * -SKP_int16_MIN */\r
173     SKP_assert( max_abs_d_Q14 <= ( 5 << 15 ) );\r
174 \r
175     /* How many bits is needed for w*d' in Q( 18 - maxRshifts ) in the worst case, of all d_Q14's being equal to max_abs_d_Q14 */\r
176     extra_shifts = max_w_bits + 32 - SKP_Silk_CLZ32( max_abs_d_Q14 ) - 14;\r
177     \r
178     /* Subtract what we got available; bits in output var plus maxRshifts */\r
179     extra_shifts -= ( 32 - 1 - 2 + maxRshifts ); /* Keep sign bit free as well as 2 bits for accumulation */\r
180     extra_shifts = SKP_max_int( extra_shifts, 0 );\r
181 \r
182     maxRshifts_wxtra = maxRshifts + extra_shifts;\r
183     \r
184     temp32 = SKP_RSHIFT( 262, maxRshifts + extra_shifts ) + 1; /* 1e-3f in Q( 18 - (maxRshifts + extra_shifts) ) */\r
185     wd = 0;\r
186     for( k = 0; k < nb_subfr; k++ ) {\r
187         /* w has at least 2 bits of headroom so no overflow should happen */\r
188         temp32 = SKP_ADD32( temp32,                     SKP_RSHIFT( w[ k ], maxRshifts_wxtra - corr_rshifts[ k ] ) );                    /* Q( 18 - maxRshifts_wxtra ) */\r
189         wd     = SKP_ADD32( wd, SKP_LSHIFT( SKP_SMULWW( SKP_RSHIFT( w[ k ], maxRshifts_wxtra - corr_rshifts[ k ] ), d_Q14[ k ] ), 2 ) ); /* Q( 18 - maxRshifts_wxtra ) */\r
190     }\r
191     m_Q12 = SKP_DIV32_varQ( wd, temp32, 12 );\r
192 \r
193     b_Q14_ptr = b_Q14;\r
194     for( k = 0; k < nb_subfr; k++ ) {\r
195         /* w_fix[ k ] from Q( 18 - corr_rshifts[ k ] ) to Q( 16 ) */\r
196         if( 2 - corr_rshifts[k] > 0 ) {\r
197             temp32 = SKP_RSHIFT( w[ k ], 2 - corr_rshifts[ k ] );\r
198         } else {\r
199             temp32 = SKP_LSHIFT_SAT32( w[ k ], corr_rshifts[ k ] - 2 );\r
200         }\r
201 \r
202         g_Q26 = SKP_MUL( \r
203             SKP_DIV32( \r
204                 LTP_SMOOTHING_Q26, \r
205                 SKP_RSHIFT( LTP_SMOOTHING_Q26, 10 ) + temp32 ),                                       /* Q10 */ \r
206             SKP_LSHIFT_SAT32( SKP_SUB_SAT32( ( SKP_int32 )m_Q12, SKP_RSHIFT( d_Q14[ k ], 2 ) ), 4 ) );  /* Q16 */\r
207 \r
208         temp32 = 0;\r
209         for( i = 0; i < LTP_ORDER; i++ ) {\r
210             delta_b_Q14[ i ] = SKP_max_16( b_Q14_ptr[ i ], 1638 );  /* 1638_Q14 = 0.1_Q0 */\r
211             temp32 += delta_b_Q14[ i ];                          /* Q14 */\r
212         }\r
213         temp32 = SKP_DIV32( g_Q26, temp32 ); /* Q14->Q12 */\r
214         for( i = 0; i < LTP_ORDER; i++ ) {\r
215             b_Q14_ptr[ i ] = SKP_LIMIT_32( ( SKP_int32 )b_Q14_ptr[ i ] + SKP_SMULWB( SKP_LSHIFT_SAT32( temp32, 4 ), delta_b_Q14[ i ] ), -16000, 28000 );\r
216         }\r
217         b_Q14_ptr += LTP_ORDER;\r
218     }\r
219 TOC(find_LTP_FIX)\r
220 }\r
221 \r
222 void SKP_Silk_fit_LTP(\r
223     SKP_int32 LTP_coefs_Q16[ LTP_ORDER ],\r
224     SKP_int16 LTP_coefs_Q14[ LTP_ORDER ]\r
225 )\r
226 {\r
227     SKP_int i;\r
228 \r
229     for( i = 0; i < LTP_ORDER; i++ ) {\r
230         LTP_coefs_Q14[ i ] = ( SKP_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( LTP_coefs_Q16[ i ], 2 ) );\r
231     }\r
232 }\r