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