Convert all CRLF in the SILK code, tabs to spaces, and trailing
[opus.git] / silk / silk_A2NLSF.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 /* Conversion between prediction filter coefficients and NLSFs  */
29 /* Requires the order to be an even number                      */
30 /* A piecewise linear approximation maps LSF <-> cos(LSF)       */
31 /* Therefore the result is not accurate NLSFs, but the two      */
32 /* functions are accurate inverses of each other                */
33
34 #include "silk_SigProc_FIX.h"
35 #include "silk_tables.h"
36
37 /* Number of binary divisions, when not in low complexity mode */
38 #define BIN_DIV_STEPS_A2NLSF_FIX      3 /* must be no higher than 16 - log2( LSF_COS_TAB_SZ_FIX ) */
39 #define QPoly                        16
40 #define MAX_ITERATIONS_A2NLSF_FIX    30
41
42 /* Flag for using 2x as many cosine sampling points, reduces the risk of missing a root */
43 #define OVERSAMPLE_COSINE_TABLE       0
44
45 /* Helper function for A2NLSF(..)                    */
46 /* Transforms polynomials from cos(n*f) to cos(f)^n  */
47 SKP_INLINE void silk_A2NLSF_trans_poly(
48     opus_int32        *p,    /* I/O    Polynomial                                */
49     const opus_int    dd     /* I      Polynomial order (= filter order / 2 )    */
50 )
51 {
52     opus_int k, n;
53
54     for( k = 2; k <= dd; k++ ) {
55         for( n = dd; n > k; n-- ) {
56             p[ n - 2 ] -= p[ n ];
57         }
58         p[ k - 2 ] -= SKP_LSHIFT( p[ k ], 1 );
59     }
60 }
61 /* Helper function for A2NLSF(..)                    */
62 /* Polynomial evaluation                             */
63 SKP_INLINE opus_int32 silk_A2NLSF_eval_poly(    /* return the polynomial evaluation, in QPoly */
64     opus_int32        *p,    /* I    Polynomial, QPoly        */
65     const opus_int32   x,    /* I    Evaluation point, Q12    */
66     const opus_int    dd     /* I    Order                    */
67 )
68 {
69     opus_int   n;
70     opus_int32 x_Q16, y32;
71
72     y32 = p[ dd ];                                    /* QPoly */
73     x_Q16 = SKP_LSHIFT( x, 4 );
74     for( n = dd - 1; n >= 0; n-- ) {
75         y32 = SKP_SMLAWW( p[ n ], y32, x_Q16 );       /* QPoly */
76     }
77     return y32;
78 }
79
80 SKP_INLINE void silk_A2NLSF_init(
81      const opus_int32    *a_Q16,
82      opus_int32          *P,
83      opus_int32          *Q,
84      const opus_int      dd
85 )
86 {
87     opus_int k;
88
89     /* Convert filter coefs to even and odd polynomials */
90     P[dd] = SKP_LSHIFT( 1, QPoly );
91     Q[dd] = SKP_LSHIFT( 1, QPoly );
92     for( k = 0; k < dd; k++ ) {
93 #if( QPoly < 16 )
94         P[ k ] = SKP_RSHIFT_ROUND( -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ], 16 - QPoly ); /* QPoly */
95         Q[ k ] = SKP_RSHIFT_ROUND( -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ], 16 - QPoly ); /* QPoly */
96 #elif( Qpoly == 16 )
97         P[ k ] = -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ]; // QPoly
98         Q[ k ] = -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ]; // QPoly
99 #else
100         P[ k ] = SKP_LSHIFT( -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ], QPoly - 16 ); /* QPoly */
101         Q[ k ] = SKP_LSHIFT( -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ], QPoly - 16 ); /* QPoly */
102 #endif
103     }
104
105     /* Divide out zeros as we have that for even filter orders, */
106     /* z =  1 is always a root in Q, and                        */
107     /* z = -1 is always a root in P                             */
108     for( k = dd; k > 0; k-- ) {
109         P[ k - 1 ] -= P[ k ];
110         Q[ k - 1 ] += Q[ k ];
111     }
112
113     /* Transform polynomials from cos(n*f) to cos(f)^n */
114     silk_A2NLSF_trans_poly( P, dd );
115     silk_A2NLSF_trans_poly( Q, dd );
116 }
117
118 /* Compute Normalized Line Spectral Frequencies (NLSFs) from whitening filter coefficients        */
119 /* If not all roots are found, the a_Q16 coefficients are bandwidth expanded until convergence.    */
120 void silk_A2NLSF(
121     opus_int16        *NLSF,                 /* O    Normalized Line Spectral Frequencies, Q15 (0 - (2^15-1)), [d]    */
122     opus_int32        *a_Q16,                /* I/O  Monic whitening filter coefficients in Q16 [d]                   */
123     const opus_int    d                      /* I    Filter order (must be even)                                      */
124 )
125 {
126     opus_int      i, k, m, dd, root_ix, ffrac;
127     opus_int32 xlo, xhi, xmid;
128     opus_int32 ylo, yhi, ymid;
129     opus_int32 nom, den;
130     opus_int32 P[ SILK_MAX_ORDER_LPC / 2 + 1 ];
131     opus_int32 Q[ SILK_MAX_ORDER_LPC / 2 + 1 ];
132     opus_int32 *PQ[ 2 ];
133     opus_int32 *p;
134
135     /* Store pointers to array */
136     PQ[ 0 ] = P;
137     PQ[ 1 ] = Q;
138
139     dd = SKP_RSHIFT( d, 1 );
140
141     silk_A2NLSF_init( a_Q16, P, Q, dd );
142
143     /* Find roots, alternating between P and Q */
144     p = P;    /* Pointer to polynomial */
145
146     xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; // Q12
147     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
148
149     if( ylo < 0 ) {
150         /* Set the first NLSF to zero and move on to the next */
151         NLSF[ 0 ] = 0;
152         p = Q;                      /* Pointer to polynomial */
153         ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
154         root_ix = 1;                /* Index of current root */
155     } else {
156         root_ix = 0;                /* Index of current root */
157     }
158     k = 1;                          /* Loop counter */
159     i = 0;                          /* Counter for bandwidth expansions applied */
160     while( 1 ) {
161         /* Evaluate polynomial */
162 #if OVERSAMPLE_COSINE_TABLE
163         xhi = silk_LSFCosTab_FIX_Q12[   k       >> 1 ] +
164           ( ( silk_LSFCosTab_FIX_Q12[ ( k + 1 ) >> 1 ] -
165               silk_LSFCosTab_FIX_Q12[   k       >> 1 ] ) >> 1 );    /* Q12 */
166 #else
167         xhi = silk_LSFCosTab_FIX_Q12[ k ]; /* Q12 */
168 #endif
169         yhi = silk_A2NLSF_eval_poly( p, xhi, dd );
170
171         /* Detect zero crossing */
172         if( ( ylo <= 0 && yhi >= 0 ) || ( ylo >= 0 && yhi <= 0 ) ) {
173             /* Binary division */
174 #if OVERSAMPLE_COSINE_TABLE
175             ffrac = -128;
176 #else
177             ffrac = -256;
178 #endif
179             for( m = 0; m < BIN_DIV_STEPS_A2NLSF_FIX; m++ ) {
180                 /* Evaluate polynomial */
181                 xmid = SKP_RSHIFT_ROUND( xlo + xhi, 1 );
182                 ymid = silk_A2NLSF_eval_poly( p, xmid, dd );
183
184                 /* Detect zero crossing */
185                 if( ( ylo <= 0 && ymid >= 0 ) || ( ylo >= 0 && ymid <= 0 ) ) {
186                     /* Reduce frequency */
187                     xhi = xmid;
188                     yhi = ymid;
189                 } else {
190                     /* Increase frequency */
191                     xlo = xmid;
192                     ylo = ymid;
193 #if OVERSAMPLE_COSINE_TABLE
194                     ffrac = SKP_ADD_RSHIFT( ffrac,  64, m );
195 #else
196                     ffrac = SKP_ADD_RSHIFT( ffrac, 128, m );
197 #endif
198                 }
199             }
200
201             /* Interpolate */
202             if( SKP_abs( ylo ) < 65536 ) {
203                 /* Avoid dividing by zero */
204                 den = ylo - yhi;
205                 nom = SKP_LSHIFT( ylo, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) + SKP_RSHIFT( den, 1 );
206                 if( den != 0 ) {
207                     ffrac += SKP_DIV32( nom, den );
208                 }
209             } else {
210                 /* No risk of dividing by zero because abs(ylo - yhi) >= abs(ylo) >= 65536 */
211                 ffrac += SKP_DIV32( ylo, SKP_RSHIFT( ylo - yhi, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) );
212             }
213 #if OVERSAMPLE_COSINE_TABLE
214             NLSF[ root_ix ] = (opus_int16)SKP_min_32( SKP_LSHIFT( (opus_int32)k, 7 ) + ffrac, SKP_int16_MAX );
215 #else
216             NLSF[ root_ix ] = (opus_int16)SKP_min_32( SKP_LSHIFT( (opus_int32)k, 8 ) + ffrac, SKP_int16_MAX );
217 #endif
218
219             SKP_assert( NLSF[ root_ix ] >=     0 );
220             SKP_assert( NLSF[ root_ix ] <= 32767 );
221
222             root_ix++;        /* Next root */
223             if( root_ix >= d ) {
224                 /* Found all roots */
225                 break;
226             }
227             /* Alternate pointer to polynomial */
228             p = PQ[ root_ix & 1 ];
229
230             /* Evaluate polynomial */
231 #if OVERSAMPLE_COSINE_TABLE
232             xlo = silk_LSFCosTab_FIX_Q12[ ( k - 1 ) >> 1 ] +
233               ( ( silk_LSFCosTab_FIX_Q12[   k       >> 1 ] -
234                   silk_LSFCosTab_FIX_Q12[ ( k - 1 ) >> 1 ] ) >> 1 ); // Q12
235 #else
236             xlo = silk_LSFCosTab_FIX_Q12[ k - 1 ]; // Q12
237 #endif
238             ylo = SKP_LSHIFT( 1 - ( root_ix & 2 ), 12 );
239         } else {
240             /* Increment loop counter */
241             k++;
242             xlo    = xhi;
243             ylo    = yhi;
244
245 #if OVERSAMPLE_COSINE_TABLE
246             if( k > 2 * LSF_COS_TAB_SZ_FIX ) {
247 #else
248             if( k > LSF_COS_TAB_SZ_FIX ) {
249 #endif
250                 i++;
251                 if( i > MAX_ITERATIONS_A2NLSF_FIX ) {
252                     /* Set NLSFs to white spectrum and exit */
253                     NLSF[ 0 ] = (opus_int16)SKP_DIV32_16( 1 << 15, d + 1 );
254                     for( k = 1; k < d; k++ ) {
255                         NLSF[ k ] = (opus_int16)SKP_SMULBB( k + 1, NLSF[ 0 ] );
256                     }
257                     return;
258                 }
259
260                 /* Error: Apply progressively more bandwidth expansion and run again */
261                 silk_bwexpander_32( a_Q16, d, 65536 - SKP_SMULBB( 10 + i, i ) ); // 10_Q16 = 0.00015
262
263                 silk_A2NLSF_init( a_Q16, P, Q, dd );
264                 p = P;                            /* Pointer to polynomial */
265                 xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; // Q12
266                 ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
267                 if( ylo < 0 ) {
268                     /* Set the first NLSF to zero and move on to the next */
269                     NLSF[ 0 ] = 0;
270                     p = Q;                        /* Pointer to polynomial */
271                     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
272                     root_ix = 1;                  /* Index of current root */
273                 } else {
274                     root_ix = 0;                  /* Index of current root */
275                 }
276                 k = 1;                            /* Reset loop counter */
277             }
278         }
279     }
280 }