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