Moving the SILK fixed-point and float files
[opus.git] / silk / SKP_Silk_burg_modified.c
1 /***********************************************************************\r
2 Copyright (c) 2006-2011, 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 /*                                                                      *\r
29  * SKP_Silk_burg_modified.c                                           *\r
30  *                                                                      *\r
31  * Calculates the reflection coefficients from the input vector         *\r
32  * Input vector contains nb_subfr sub vectors of length L_sub + D       *\r
33  *                                                                      *\r
34  * Copyright 2009 (c), Skype Limited                                    *\r
35  * Date: 100105                                                         *\r
36  */\r
37 \r
38 #include "SKP_Silk_SigProc_FIX.h"\r
39 \r
40 #define MAX_FRAME_SIZE              384 // subfr_length * nb_subfr = ( 0.005 * 16000 + 16 ) * 4 = 384\r
41 #define MAX_NB_SUBFR                4\r
42 \r
43 #define QA                          25\r
44 #define N_BITS_HEAD_ROOM            2\r
45 #define MIN_RSHIFTS                 -16\r
46 #define MAX_RSHIFTS                 (32 - QA)\r
47 \r
48 /* Compute reflection coefficients from input signal */\r
49 void SKP_Silk_burg_modified(\r
50     SKP_int32       *res_nrg,           /* O    residual energy                                                 */\r
51     SKP_int         *res_nrg_Q,         /* O    residual energy Q value                                         */\r
52     SKP_int32       A_Q16[],            /* O    prediction coefficients (length order)                          */\r
53     const SKP_int16 x[],                /* I    input signal, length: nb_subfr * ( D + subfr_length )           */\r
54     const SKP_int   subfr_length,       /* I    input signal subframe length (including D preceeding samples)   */\r
55     const SKP_int   nb_subfr,           /* I    number of subframes stacked in x                                */\r
56     const SKP_int32 WhiteNoiseFrac_Q32, /* I    fraction added to zero-lag autocorrelation                      */\r
57     const SKP_int   D                   /* I    order                                                           */\r
58 )\r
59 {\r
60     SKP_int         k, n, s, lz, rshifts, rshifts_extra;\r
61     SKP_int32       C0, num, nrg, rc_Q31, Atmp_QA, Atmp1, tmp1, tmp2, x1, x2;\r
62     const SKP_int16 *x_ptr;\r
63 \r
64     SKP_int32       C_first_row[ SKP_Silk_MAX_ORDER_LPC ];\r
65     SKP_int32       C_last_row[  SKP_Silk_MAX_ORDER_LPC ];\r
66     SKP_int32       Af_QA[       SKP_Silk_MAX_ORDER_LPC ];\r
67 \r
68     SKP_int32       CAf[ SKP_Silk_MAX_ORDER_LPC + 1 ];\r
69     SKP_int32       CAb[ SKP_Silk_MAX_ORDER_LPC + 1 ];\r
70 \r
71     SKP_assert( subfr_length * nb_subfr <= MAX_FRAME_SIZE );\r
72     SKP_assert( nb_subfr <= MAX_NB_SUBFR );\r
73 \r
74 \r
75     /* Compute autocorrelations, added over subframes */\r
76     SKP_Silk_sum_sqr_shift( &C0, &rshifts, x, nb_subfr * subfr_length );\r
77     if( rshifts > MAX_RSHIFTS ) {\r
78         C0 = SKP_LSHIFT32( C0, rshifts - MAX_RSHIFTS );\r
79         SKP_assert( C0 > 0 );\r
80         rshifts = MAX_RSHIFTS;\r
81     } else {\r
82         lz = SKP_Silk_CLZ32( C0 ) - 1;\r
83         rshifts_extra = N_BITS_HEAD_ROOM - lz;\r
84         if( rshifts_extra > 0 ) {\r
85             rshifts_extra = SKP_min( rshifts_extra, MAX_RSHIFTS - rshifts );\r
86             C0 = SKP_RSHIFT32( C0, rshifts_extra );\r
87         } else {\r
88             rshifts_extra = SKP_max( rshifts_extra, MIN_RSHIFTS - rshifts );\r
89             C0 = SKP_LSHIFT32( C0, -rshifts_extra );\r
90         }\r
91         rshifts += rshifts_extra;\r
92     }\r
93     SKP_memset( C_first_row, 0, SKP_Silk_MAX_ORDER_LPC * sizeof( SKP_int32 ) );\r
94     if( rshifts > 0 ) {\r
95         for( s = 0; s < nb_subfr; s++ ) {\r
96             x_ptr = x + s * subfr_length;\r
97             for( n = 1; n < D + 1; n++ ) {\r
98                 C_first_row[ n - 1 ] += (SKP_int32)SKP_RSHIFT64( \r
99                     SKP_Silk_inner_prod16_aligned_64( x_ptr, x_ptr + n, subfr_length - n ), rshifts );\r
100             }\r
101         }\r
102     } else {\r
103         for( s = 0; s < nb_subfr; s++ ) {\r
104             x_ptr = x + s * subfr_length;\r
105             for( n = 1; n < D + 1; n++ ) {\r
106                 C_first_row[ n - 1 ] += SKP_LSHIFT32( \r
107                     SKP_Silk_inner_prod_aligned( x_ptr, x_ptr + n, subfr_length - n ), -rshifts );\r
108             }\r
109         }\r
110     }\r
111     SKP_memcpy( C_last_row, C_first_row, SKP_Silk_MAX_ORDER_LPC * sizeof( SKP_int32 ) );\r
112     \r
113     /* Initialize */\r
114     CAb[ 0 ] = CAf[ 0 ] = C0 + SKP_SMMUL( WhiteNoiseFrac_Q32, C0 ) + 1;         // Q(-rshifts)\r
115 \r
116     for( n = 0; n < D; n++ ) {\r
117         /* Update first row of correlation matrix (without first element) */\r
118         /* Update last row of correlation matrix (without last element, stored in reversed order) */\r
119         /* Update C * Af */\r
120         /* Update C * flipud(Af) (stored in reversed order) */\r
121         if( rshifts > -2 ) {\r
122             for( s = 0; s < nb_subfr; s++ ) {\r
123                 x_ptr = x + s * subfr_length;\r
124                 x1  = -SKP_LSHIFT32( (SKP_int32)x_ptr[ n ],                    16 - rshifts );      // Q(16-rshifts)\r
125                 x2  = -SKP_LSHIFT32( (SKP_int32)x_ptr[ subfr_length - n - 1 ], 16 - rshifts );      // Q(16-rshifts)\r
126                 tmp1 = SKP_LSHIFT32( (SKP_int32)x_ptr[ n ],                    QA - 16 );           // Q(QA-16)\r
127                 tmp2 = SKP_LSHIFT32( (SKP_int32)x_ptr[ subfr_length - n - 1 ], QA - 16 );           // Q(QA-16)\r
128                 for( k = 0; k < n; k++ ) {\r
129                     C_first_row[ k ] = SKP_SMLAWB( C_first_row[ k ], x1, x_ptr[ n - k - 1 ]            ); // Q( -rshifts )\r
130                     C_last_row[ k ]  = SKP_SMLAWB( C_last_row[ k ],  x2, x_ptr[ subfr_length - n + k ] ); // Q( -rshifts )\r
131                     Atmp_QA = Af_QA[ k ];\r
132                     tmp1 = SKP_SMLAWB( tmp1, Atmp_QA, x_ptr[ n - k - 1 ]            );              // Q(QA-16)\r
133                     tmp2 = SKP_SMLAWB( tmp2, Atmp_QA, x_ptr[ subfr_length - n + k ] );              // Q(QA-16)\r
134                 }\r
135                 tmp1 = SKP_LSHIFT32( -tmp1, 32 - QA - rshifts );                                    // Q(16-rshifts)\r
136                 tmp2 = SKP_LSHIFT32( -tmp2, 32 - QA - rshifts );                                    // Q(16-rshifts)\r
137                 for( k = 0; k <= n; k++ ) {\r
138                     CAf[ k ] = SKP_SMLAWB( CAf[ k ], tmp1, x_ptr[ n - k ]                    );     // Q( -rshift )\r
139                     CAb[ k ] = SKP_SMLAWB( CAb[ k ], tmp2, x_ptr[ subfr_length - n + k - 1 ] );     // Q( -rshift )\r
140                 }\r
141             }\r
142         } else {\r
143             for( s = 0; s < nb_subfr; s++ ) {\r
144                 x_ptr = x + s * subfr_length;\r
145                 x1  = -SKP_LSHIFT32( (SKP_int32)x_ptr[ n ],                    -rshifts );          // Q( -rshifts )\r
146                 x2  = -SKP_LSHIFT32( (SKP_int32)x_ptr[ subfr_length - n - 1 ], -rshifts );          // Q( -rshifts )\r
147                 tmp1 = SKP_LSHIFT32( (SKP_int32)x_ptr[ n ],                    17 );                // Q17\r
148                 tmp2 = SKP_LSHIFT32( (SKP_int32)x_ptr[ subfr_length - n - 1 ], 17 );                // Q17\r
149                 for( k = 0; k < n; k++ ) {\r
150                     C_first_row[ k ] = SKP_MLA( C_first_row[ k ], x1, x_ptr[ n - k - 1 ]            ); // Q( -rshifts )\r
151                     C_last_row[ k ]  = SKP_MLA( C_last_row[ k ],  x2, x_ptr[ subfr_length - n + k ] ); // Q( -rshifts )\r
152                     Atmp1 = SKP_RSHIFT_ROUND( Af_QA[ k ], QA - 17 );                                // Q17\r
153                     tmp1 = SKP_MLA( tmp1, x_ptr[ n - k - 1 ],            Atmp1 );                   // Q17\r
154                     tmp2 = SKP_MLA( tmp2, x_ptr[ subfr_length - n + k ], Atmp1 );                   // Q17\r
155                 }\r
156                 tmp1 = -tmp1;                                                                       // Q17\r
157                 tmp2 = -tmp2;                                                                       // Q17\r
158                 for( k = 0; k <= n; k++ ) {\r
159                     CAf[ k ] = SKP_SMLAWW( CAf[ k ], tmp1, \r
160                         SKP_LSHIFT32( (SKP_int32)x_ptr[ n - k ], -rshifts - 1 ) );                  // Q( -rshift )\r
161                     CAb[ k ] = SKP_SMLAWW( CAb[ k ], tmp2, \r
162                         SKP_LSHIFT32( (SKP_int32)x_ptr[ subfr_length - n + k - 1 ], -rshifts - 1 ) );// Q( -rshift )\r
163                 }\r
164             }\r
165         }\r
166 \r
167         /* Calculate nominator and denominator for the next order reflection (parcor) coefficient */\r
168         tmp1 = C_first_row[ n ];                                                            // Q( -rshifts )\r
169         tmp2 = C_last_row[ n ];                                                             // Q( -rshifts )\r
170         num  = 0;                                                                           // Q( -rshifts )\r
171         nrg  = SKP_ADD32( CAb[ 0 ], CAf[ 0 ] );                                             // Q( 1-rshifts )\r
172         for( k = 0; k < n; k++ ) {\r
173             Atmp_QA = Af_QA[ k ];\r
174             lz = SKP_Silk_CLZ32( SKP_abs( Atmp_QA ) ) - 1;\r
175             lz = SKP_min( 32 - QA, lz );\r
176             Atmp1 = SKP_LSHIFT32( Atmp_QA, lz );                                            // Q( QA + lz )\r
177 \r
178             tmp1 = SKP_ADD_LSHIFT32( tmp1, SKP_SMMUL( C_last_row[  n - k - 1 ], Atmp1 ), 32 - QA - lz );    // Q( -rshifts )\r
179             tmp2 = SKP_ADD_LSHIFT32( tmp2, SKP_SMMUL( C_first_row[ n - k - 1 ], Atmp1 ), 32 - QA - lz );    // Q( -rshifts )\r
180             num  = SKP_ADD_LSHIFT32( num,  SKP_SMMUL( CAb[ n - k ],             Atmp1 ), 32 - QA - lz );    // Q( -rshifts )\r
181             nrg  = SKP_ADD_LSHIFT32( nrg,  SKP_SMMUL( SKP_ADD32( CAb[ k + 1 ], CAf[ k + 1 ] ), \r
182                                                                                 Atmp1 ), 32 - QA - lz );    // Q( 1-rshifts )\r
183         }\r
184         CAf[ n + 1 ] = tmp1;                                                                // Q( -rshifts )\r
185         CAb[ n + 1 ] = tmp2;                                                                // Q( -rshifts )\r
186         num = SKP_ADD32( num, tmp2 );                                                       // Q( -rshifts )\r
187         num = SKP_LSHIFT32( -num, 1 );                                                      // Q( 1-rshifts )\r
188 \r
189         /* Calculate the next order reflection (parcor) coefficient */\r
190         if( SKP_abs( num ) < nrg ) {\r
191             rc_Q31 = SKP_DIV32_varQ( num, nrg, 31 );\r
192         } else {\r
193             /* Negative energy or ratio too high; set remaining coefficients to zero and exit loop */\r
194             SKP_memset( &Af_QA[ n ], 0, ( D - n ) * sizeof( SKP_int32 ) );\r
195             SKP_assert( 0 );\r
196             break;\r
197         }\r
198 \r
199         /* Update the AR coefficients */\r
200         for( k = 0; k < (n + 1) >> 1; k++ ) {\r
201             tmp1 = Af_QA[ k ];                                                              // QA\r
202             tmp2 = Af_QA[ n - k - 1 ];                                                      // QA\r
203             Af_QA[ k ]         = SKP_ADD_LSHIFT32( tmp1, SKP_SMMUL( tmp2, rc_Q31 ), 1 );    // QA\r
204             Af_QA[ n - k - 1 ] = SKP_ADD_LSHIFT32( tmp2, SKP_SMMUL( tmp1, rc_Q31 ), 1 );    // QA\r
205         }\r
206         Af_QA[ n ] = SKP_RSHIFT32( rc_Q31, 31 - QA );                                       // QA\r
207 \r
208         /* Update C * Af and C * Ab */\r
209         for( k = 0; k <= n + 1; k++ ) {\r
210             tmp1 = CAf[ k ];                                                                // Q( -rshifts )\r
211             tmp2 = CAb[ n - k + 1 ];                                                        // Q( -rshifts )\r
212             CAf[ k ]         = SKP_ADD_LSHIFT32( tmp1, SKP_SMMUL( tmp2, rc_Q31 ), 1 );      // Q( -rshifts )\r
213             CAb[ n - k + 1 ] = SKP_ADD_LSHIFT32( tmp2, SKP_SMMUL( tmp1, rc_Q31 ), 1 );      // Q( -rshifts )\r
214         }\r
215     }\r
216 \r
217     /* Return residual energy */\r
218     nrg  = CAf[ 0 ];                                                                        // Q( -rshifts )\r
219     tmp1 = 1 << 16;                                                                         // Q16\r
220     for( k = 0; k < D; k++ ) {\r
221         Atmp1 = SKP_RSHIFT_ROUND( Af_QA[ k ], QA - 16 );                                    // Q16\r
222         nrg  = SKP_SMLAWW( nrg, CAf[ k + 1 ], Atmp1 );                                      // Q( -rshifts )\r
223         tmp1 = SKP_SMLAWW( tmp1, Atmp1, Atmp1 );                                            // Q16\r
224         A_Q16[ k ] = -Atmp1;\r
225     }\r
226     *res_nrg = SKP_SMLAWW( nrg, SKP_SMMUL( WhiteNoiseFrac_Q32, C0 ), -tmp1 );               // Q( -rshifts )\r
227     *res_nrg_Q = -rshifts;\r
228 }\r