Convert all CRLF in the SILK code, tabs to spaces, and trailing
[opus.git] / silk / fixed / silk_find_LTP_FIX.c
1 /***********************************************************************
2 Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3 Redistribution and use in source and binary forms, with or without
4 modification, (subject to the limitations in the disclaimer below)
5 are permitted provided that the following conditions are met:
6 - Redistributions of source code must retain the above copyright notice,
7 this list of conditions and the following disclaimer.
8 - Redistributions in binary form must reproduce the above copyright
9 notice, this list of conditions and the following disclaimer in the
10 documentation and/or other materials provided with the distribution.
11 - Neither the name of Skype Limited, nor the names of specific
12 contributors, may be used to endorse or promote products derived from
13 this software without specific prior written permission.
14 NO EXPRESS OR IMPLIED LICENSES TO ANY PARTY'S PATENT RIGHTS ARE GRANTED
15 BY THIS LICENSE. THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND
16 CONTRIBUTORS ''AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING,
17 BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND
18 FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
19 COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
20 INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT
21 NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF
22 USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON
23 ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
25 OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 ***********************************************************************/
27
28 #include "silk_main_FIX.h"
29 #include "silk_tuning_parameters.h"
30
31 /* Head room for correlations                           */
32 #define LTP_CORRS_HEAD_ROOM                             2
33
34 void silk_fit_LTP(
35     opus_int32 LTP_coefs_Q16[ LTP_ORDER ],
36     opus_int16 LTP_coefs_Q14[ LTP_ORDER ]
37 );
38
39 void silk_find_LTP_FIX(
40     opus_int16           b_Q14[ MAX_NB_SUBFR * LTP_ORDER ],              /* O    LTP coefs                                                   */
41     opus_int32           WLTP[ MAX_NB_SUBFR * LTP_ORDER * LTP_ORDER ],   /* O    Weight for LTP quantization                                 */
42     opus_int             *LTPredCodGain_Q7,                              /* O    LTP coding gain                                             */
43     const opus_int16     r_lpc[]  ,                                      /* I    residual signal after LPC signal + state for first 10 ms    */
44     const opus_int       lag[ MAX_NB_SUBFR ],                            /* I    LTP lags                                                    */
45     const opus_int32     Wght_Q15[ MAX_NB_SUBFR ],                       /* I    weights                                                     */
46     const opus_int       subfr_length,                                   /* I    subframe length                                             */
47     const opus_int       nb_subfr,                                       /* I    number of subframes                                         */
48     const opus_int       mem_offset,                                     /* I    number of samples in LTP memory                             */
49     opus_int             corr_rshifts[ MAX_NB_SUBFR ]                    /* O    right shifts applied to correlations                        */
50 )
51 {
52     opus_int   i, k, lshift;
53     const opus_int16 *r_ptr, *lag_ptr;
54     opus_int16 *b_Q14_ptr;
55
56     opus_int32 regu;
57     opus_int32 *WLTP_ptr;
58     opus_int32 b_Q16[ LTP_ORDER ], delta_b_Q14[ LTP_ORDER ], d_Q14[ MAX_NB_SUBFR ], nrg[ MAX_NB_SUBFR ], g_Q26;
59     opus_int32 w[ MAX_NB_SUBFR ], WLTP_max, max_abs_d_Q14, max_w_bits;
60
61     opus_int32 temp32, denom32;
62     opus_int   extra_shifts;
63     opus_int   rr_shifts, maxRshifts, maxRshifts_wxtra, LZs;
64     opus_int32 LPC_res_nrg, LPC_LTP_res_nrg, div_Q16;
65     opus_int32 Rr[ LTP_ORDER ], rr[ MAX_NB_SUBFR ];
66     opus_int32 wd, m_Q12;
67
68     b_Q14_ptr = b_Q14;
69     WLTP_ptr  = WLTP;
70     r_ptr     = &r_lpc[ mem_offset ];
71     for( k = 0; k < nb_subfr; k++ ) {
72         lag_ptr = r_ptr - ( lag[ k ] + LTP_ORDER / 2 );
73
74         silk_sum_sqr_shift( &rr[ k ], &rr_shifts, r_ptr, subfr_length ); /* rr[ k ] in Q( -rr_shifts ) */
75
76         /* Assure headroom */
77         LZs = silk_CLZ32( rr[k] );
78         if( LZs < LTP_CORRS_HEAD_ROOM ) {
79             rr[ k ] = SKP_RSHIFT_ROUND( rr[ k ], LTP_CORRS_HEAD_ROOM - LZs );
80             rr_shifts += ( LTP_CORRS_HEAD_ROOM - LZs );
81         }
82         corr_rshifts[ k ] = rr_shifts;
83         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 ] ) */
84
85         /* The correlation vector always has lower max abs value than rr and/or RR so head room is assured */
86         silk_corrVector_FIX( lag_ptr, r_ptr, subfr_length, LTP_ORDER, Rr, corr_rshifts[ k ] );  /* Rr_fix_ptr   in Q( -corr_rshifts[ k ] ) */
87         if( corr_rshifts[ k ] > rr_shifts ) {
88             rr[ k ] = SKP_RSHIFT( rr[ k ], corr_rshifts[ k ] - rr_shifts ); /* rr[ k ] in Q( -corr_rshifts[ k ] ) */
89         }
90         SKP_assert( rr[ k ] >= 0 );
91
92         regu = 1;
93         regu = SKP_SMLAWB( regu, rr[ k ], SILK_FIX_CONST( LTP_DAMPING/3, 16 ) );
94         regu = SKP_SMLAWB( regu, matrix_ptr( WLTP_ptr, 0, 0, LTP_ORDER ), SILK_FIX_CONST( LTP_DAMPING/3, 16 ) );
95         regu = SKP_SMLAWB( regu, matrix_ptr( WLTP_ptr, LTP_ORDER-1, LTP_ORDER-1, LTP_ORDER ), SILK_FIX_CONST( LTP_DAMPING/3, 16 ) );
96         silk_regularize_correlations_FIX( WLTP_ptr, &rr[k], regu, LTP_ORDER );
97
98         silk_solve_LDL_FIX( WLTP_ptr, LTP_ORDER, Rr, b_Q16 ); /* WLTP_fix_ptr and Rr_fix_ptr both in Q(-corr_rshifts[k]) */
99
100         /* Limit and store in Q14 */
101         silk_fit_LTP( b_Q16, b_Q14_ptr );
102
103         /* Calculate residual energy */
104         nrg[ k ] = silk_residual_energy16_covar_FIX( b_Q14_ptr, WLTP_ptr, Rr, rr[ k ], LTP_ORDER, 14 ); /* nrg_fix in Q( -corr_rshifts[ k ] ) */
105
106         /* temp = Wght[ k ] / ( nrg[ k ] * Wght[ k ] + 0.01f * subfr_length ); */
107         extra_shifts = SKP_min_int( corr_rshifts[ k ], LTP_CORRS_HEAD_ROOM );
108         denom32 = SKP_LSHIFT_SAT32( SKP_SMULWB( nrg[ k ], Wght_Q15[ k ] ), 1 + extra_shifts ) + /* Q( -corr_rshifts[ k ] + extra_shifts ) */
109             SKP_RSHIFT( SKP_SMULWB( subfr_length, 655 ), corr_rshifts[ k ] - extra_shifts );    /* Q( -corr_rshifts[ k ] + extra_shifts ) */
110         denom32 = SKP_max( denom32, 1 );
111         SKP_assert( ((opus_int64)Wght_Q15[ k ] << 16 ) < SKP_int32_MAX );                        /* Wght always < 0.5 in Q0 */
112         temp32 = SKP_DIV32( SKP_LSHIFT( ( opus_int32 )Wght_Q15[ k ], 16 ), denom32 );            /* Q( 15 + 16 + corr_rshifts[k] - extra_shifts ) */
113         temp32 = SKP_RSHIFT( temp32, 31 + corr_rshifts[ k ] - extra_shifts - 26 );              /* Q26 */
114
115         /* Limit temp such that the below scaling never wraps around */
116         WLTP_max = 0;
117         for( i = 0; i < LTP_ORDER * LTP_ORDER; i++ ) {
118             WLTP_max = SKP_max( WLTP_ptr[ i ], WLTP_max );
119         }
120         lshift = silk_CLZ32( WLTP_max ) - 1 - 3; /* keep 3 bits free for vq_nearest_neighbor_fix */
121         SKP_assert( 26 - 18 + lshift >= 0 );
122         if( 26 - 18 + lshift < 31 ) {
123             temp32 = SKP_min_32( temp32, SKP_LSHIFT( ( opus_int32 )1, 26 - 18 + lshift ) );
124         }
125
126         silk_scale_vector32_Q26_lshift_18( WLTP_ptr, temp32, LTP_ORDER * LTP_ORDER ); /* WLTP_ptr in Q( 18 - corr_rshifts[ k ] ) */
127
128         w[ k ] = matrix_ptr( WLTP_ptr, LTP_ORDER/2, LTP_ORDER/2, LTP_ORDER ); /* w in Q( 18 - corr_rshifts[ k ] ) */
129         SKP_assert( w[k] >= 0 );
130
131         r_ptr     += subfr_length;
132         b_Q14_ptr += LTP_ORDER;
133         WLTP_ptr  += LTP_ORDER * LTP_ORDER;
134     }
135
136     maxRshifts = 0;
137     for( k = 0; k < nb_subfr; k++ ) {
138         maxRshifts = SKP_max_int( corr_rshifts[ k ], maxRshifts );
139     }
140
141     /* Compute LTP coding gain */
142     if( LTPredCodGain_Q7 != NULL ) {
143         LPC_LTP_res_nrg = 0;
144         LPC_res_nrg     = 0;
145         SKP_assert( LTP_CORRS_HEAD_ROOM >= 2 ); /* Check that no overflow will happen when adding */
146         for( k = 0; k < nb_subfr; k++ ) {
147             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 ) */
148             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 ) */
149         }
150         LPC_LTP_res_nrg = SKP_max( LPC_LTP_res_nrg, 1 ); /* avoid division by zero */
151
152         div_Q16 = silk_DIV32_varQ( LPC_res_nrg, LPC_LTP_res_nrg, 16 );
153         *LTPredCodGain_Q7 = ( opus_int )SKP_SMULBB( 3, silk_lin2log( div_Q16 ) - ( 16 << 7 ) );
154
155         SKP_assert( *LTPredCodGain_Q7 == ( opus_int )SKP_SAT16( SKP_MUL( 3, silk_lin2log( div_Q16 ) - ( 16 << 7 ) ) ) );
156     }
157
158     /* smoothing */
159     /* d = sum( B, 1 ); */
160     b_Q14_ptr = b_Q14;
161     for( k = 0; k < nb_subfr; k++ ) {
162         d_Q14[ k ] = 0;
163         for( i = 0; i < LTP_ORDER; i++ ) {
164             d_Q14[ k ] += b_Q14_ptr[ i ];
165         }
166         b_Q14_ptr += LTP_ORDER;
167     }
168
169     /* m = ( w * d' ) / ( sum( w ) + 1e-3 ); */
170
171     /* Find maximum absolute value of d_Q14 and the bits used by w in Q0 */
172     max_abs_d_Q14 = 0;
173     max_w_bits    = 0;
174     for( k = 0; k < nb_subfr; k++ ) {
175         max_abs_d_Q14 = SKP_max_32( max_abs_d_Q14, SKP_abs( d_Q14[ k ] ) );
176         /* w[ k ] is in Q( 18 - corr_rshifts[ k ] ) */
177         /* Find bits needed in Q( 18 - maxRshifts ) */
178         max_w_bits = SKP_max_32( max_w_bits, 32 - silk_CLZ32( w[ k ] ) + corr_rshifts[ k ] - maxRshifts );
179     }
180
181     /* max_abs_d_Q14 = (5 << 15); worst case, i.e. LTP_ORDER * -SKP_int16_MIN */
182     SKP_assert( max_abs_d_Q14 <= ( 5 << 15 ) );
183
184     /* 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 */
185     extra_shifts = max_w_bits + 32 - silk_CLZ32( max_abs_d_Q14 ) - 14;
186
187     /* Subtract what we got available; bits in output var plus maxRshifts */
188     extra_shifts -= ( 32 - 1 - 2 + maxRshifts ); /* Keep sign bit free as well as 2 bits for accumulation */
189     extra_shifts = SKP_max_int( extra_shifts, 0 );
190
191     maxRshifts_wxtra = maxRshifts + extra_shifts;
192
193     temp32 = SKP_RSHIFT( 262, maxRshifts + extra_shifts ) + 1; /* 1e-3f in Q( 18 - (maxRshifts + extra_shifts) ) */
194     wd = 0;
195     for( k = 0; k < nb_subfr; k++ ) {
196         /* w has at least 2 bits of headroom so no overflow should happen */
197         temp32 = SKP_ADD32( temp32,                     SKP_RSHIFT( w[ k ], maxRshifts_wxtra - corr_rshifts[ k ] ) );                    /* Q( 18 - maxRshifts_wxtra ) */
198         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 ) */
199     }
200     m_Q12 = silk_DIV32_varQ( wd, temp32, 12 );
201
202     b_Q14_ptr = b_Q14;
203     for( k = 0; k < nb_subfr; k++ ) {
204         /* w_fix[ k ] from Q( 18 - corr_rshifts[ k ] ) to Q( 16 ) */
205         if( 2 - corr_rshifts[k] > 0 ) {
206             temp32 = SKP_RSHIFT( w[ k ], 2 - corr_rshifts[ k ] );
207         } else {
208             temp32 = SKP_LSHIFT_SAT32( w[ k ], corr_rshifts[ k ] - 2 );
209         }
210
211         g_Q26 = SKP_MUL(
212             SKP_DIV32(
213                 SILK_FIX_CONST( LTP_SMOOTHING, 26 ),
214                 SKP_RSHIFT( SILK_FIX_CONST( LTP_SMOOTHING, 26 ), 10 ) + temp32 ),                                       /* Q10 */
215             SKP_LSHIFT_SAT32( SKP_SUB_SAT32( ( opus_int32 )m_Q12, SKP_RSHIFT( d_Q14[ k ], 2 ) ), 4 ) );  /* Q16 */
216
217         temp32 = 0;
218         for( i = 0; i < LTP_ORDER; i++ ) {
219             delta_b_Q14[ i ] = SKP_max_16( b_Q14_ptr[ i ], 1638 );  /* 1638_Q14 = 0.1_Q0 */
220             temp32 += delta_b_Q14[ i ];                          /* Q14 */
221         }
222         temp32 = SKP_DIV32( g_Q26, temp32 ); /* Q14->Q12 */
223         for( i = 0; i < LTP_ORDER; i++ ) {
224             b_Q14_ptr[ i ] = SKP_LIMIT_32( ( opus_int32 )b_Q14_ptr[ i ] + SKP_SMULWB( SKP_LSHIFT_SAT32( temp32, 4 ), delta_b_Q14[ i ] ), -16000, 28000 );
225         }
226         b_Q14_ptr += LTP_ORDER;
227     }
228 TOC(find_LTP_FIX)
229 }
230
231 void silk_fit_LTP(
232     opus_int32 LTP_coefs_Q16[ LTP_ORDER ],
233     opus_int16 LTP_coefs_Q14[ LTP_ORDER ]
234 )
235 {
236     opus_int i;
237
238     for( i = 0; i < LTP_ORDER; i++ ) {
239         LTP_coefs_Q14[ i ] = ( opus_int16 )SKP_SAT16( SKP_RSHIFT_ROUND( LTP_coefs_Q16[ i ], 2 ) );
240     }
241 }