Move misplaced RESTORE_STACK.
[opus.git] / 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, are permitted provided that the following conditions
5 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 Internet Society, IETF or IETF Trust, nor the 
12 names of specific contributors, may be used to endorse or promote
13 products derived from this software without specific prior written
14 permission.
15 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16 AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17 IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18 ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19 LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20 CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21 SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22 INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23 CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24 ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25 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 #ifdef HAVE_CONFIG_H
35 #include "config.h"
36 #endif
37
38 #include "SigProc_FIX.h"
39 #include "tables.h"
40
41 /* Number of binary divisions, when not in low complexity mode */
42 #define BIN_DIV_STEPS_A2NLSF_FIX      3 /* must be no higher than 16 - log2( LSF_COS_TAB_SZ_FIX ) */
43 #define MAX_ITERATIONS_A2NLSF_FIX    30
44
45 /* Helper function for A2NLSF(..)                    */
46 /* Transforms polynomials from cos(n*f) to cos(f)^n  */
47 static 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 ] -= silk_LSHIFT( p[ k ], 1 );
59     }
60 }
61 /* Helper function for A2NLSF(..) */
62 /* Polynomial evaluation          */
63 static inline opus_int32 silk_A2NLSF_eval_poly( /* return the polynomial evaluation, in Q16     */
64     opus_int32          *p,                     /* I    Polynomial, Q16                         */
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 ];                                  /* Q16 */
73     x_Q16 = silk_LSHIFT( x, 4 );
74     for( n = dd - 1; n >= 0; n-- ) {
75         y32 = silk_SMLAWW( p[ n ], y32, x_Q16 );    /* Q16 */
76     }
77     return y32;
78 }
79
80 static 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] = silk_LSHIFT( 1, 16 );
91     Q[dd] = silk_LSHIFT( 1, 16 );
92     for( k = 0; k < dd; k++ ) {
93         P[ k ] = -a_Q16[ dd - k - 1 ] - a_Q16[ dd + k ];    /* Q16 */
94         Q[ k ] = -a_Q16[ dd - k - 1 ] + a_Q16[ dd + k ];    /* Q16 */
95     }
96
97     /* Divide out zeros as we have that for even filter orders, */
98     /* z =  1 is always a root in Q, and                        */
99     /* z = -1 is always a root in P                             */
100     for( k = dd; k > 0; k-- ) {
101         P[ k - 1 ] -= P[ k ];
102         Q[ k - 1 ] += Q[ k ];
103     }
104
105     /* Transform polynomials from cos(n*f) to cos(f)^n */
106     silk_A2NLSF_trans_poly( P, dd );
107     silk_A2NLSF_trans_poly( Q, dd );
108 }
109
110 /* Compute Normalized Line Spectral Frequencies (NLSFs) from whitening filter coefficients      */
111 /* If not all roots are found, the a_Q16 coefficients are bandwidth expanded until convergence. */
112 void silk_A2NLSF(
113     opus_int16                  *NLSF,              /* O    Normalized Line Spectral Frequencies in Q15 (0..2^15-1) [d] */
114     opus_int32                  *a_Q16,             /* I/O  Monic whitening filter coefficients in Q16 [d]              */
115     const opus_int              d                   /* I    Filter order (must be even)                                 */
116 )
117 {
118     opus_int      i, k, m, dd, root_ix, ffrac;
119     opus_int32 xlo, xhi, xmid;
120     opus_int32 ylo, yhi, ymid, thr;
121     opus_int32 nom, den;
122     opus_int32 P[ SILK_MAX_ORDER_LPC / 2 + 1 ];
123     opus_int32 Q[ SILK_MAX_ORDER_LPC / 2 + 1 ];
124     opus_int32 *PQ[ 2 ];
125     opus_int32 *p;
126
127     /* Store pointers to array */
128     PQ[ 0 ] = P;
129     PQ[ 1 ] = Q;
130
131     dd = silk_RSHIFT( d, 1 );
132
133     silk_A2NLSF_init( a_Q16, P, Q, dd );
134
135     /* Find roots, alternating between P and Q */
136     p = P;                          /* Pointer to polynomial */
137
138     xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
139     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
140
141     if( ylo < 0 ) {
142         /* Set the first NLSF to zero and move on to the next */
143         NLSF[ 0 ] = 0;
144         p = Q;                      /* Pointer to polynomial */
145         ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
146         root_ix = 1;                /* Index of current root */
147     } else {
148         root_ix = 0;                /* Index of current root */
149     }
150     k = 1;                          /* Loop counter */
151     i = 0;                          /* Counter for bandwidth expansions applied */
152     thr = 0;
153     while( 1 ) {
154         /* Evaluate polynomial */
155         xhi = silk_LSFCosTab_FIX_Q12[ k ]; /* Q12 */
156         yhi = silk_A2NLSF_eval_poly( p, xhi, dd );
157
158         /* Detect zero crossing */
159         if( ( ylo <= 0 && yhi >= thr ) || ( ylo >= 0 && yhi <= -thr ) ) {
160             if( yhi == 0 ) {
161                 /* If the root lies exactly at the end of the current       */
162                 /* interval, look for the next root in the next interval    */
163                 thr = 1;
164             } else {
165                 thr = 0;
166             }
167             /* Binary division */
168             ffrac = -256;
169             for( m = 0; m < BIN_DIV_STEPS_A2NLSF_FIX; m++ ) {
170                 /* Evaluate polynomial */
171                 xmid = silk_RSHIFT_ROUND( xlo + xhi, 1 );
172                 ymid = silk_A2NLSF_eval_poly( p, xmid, dd );
173
174                 /* Detect zero crossing */
175                 if( ( ylo <= 0 && ymid >= 0 ) || ( ylo >= 0 && ymid <= 0 ) ) {
176                     /* Reduce frequency */
177                     xhi = xmid;
178                     yhi = ymid;
179                 } else {
180                     /* Increase frequency */
181                     xlo = xmid;
182                     ylo = ymid;
183                     ffrac = silk_ADD_RSHIFT( ffrac, 128, m );
184                 }
185             }
186
187             /* Interpolate */
188             if( silk_abs( ylo ) < 65536 ) {
189                 /* Avoid dividing by zero */
190                 den = ylo - yhi;
191                 nom = silk_LSHIFT( ylo, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) + silk_RSHIFT( den, 1 );
192                 if( den != 0 ) {
193                     ffrac += silk_DIV32( nom, den );
194                 }
195             } else {
196                 /* No risk of dividing by zero because abs(ylo - yhi) >= abs(ylo) >= 65536 */
197                 ffrac += silk_DIV32( ylo, silk_RSHIFT( ylo - yhi, 8 - BIN_DIV_STEPS_A2NLSF_FIX ) );
198             }
199             NLSF[ root_ix ] = (opus_int16)silk_min_32( silk_LSHIFT( (opus_int32)k, 8 ) + ffrac, silk_int16_MAX );
200
201             silk_assert( NLSF[ root_ix ] >= 0 );
202
203             root_ix++;        /* Next root */
204             if( root_ix >= d ) {
205                 /* Found all roots */
206                 break;
207             }
208             /* Alternate pointer to polynomial */
209             p = PQ[ root_ix & 1 ];
210
211             /* Evaluate polynomial */
212             xlo = silk_LSFCosTab_FIX_Q12[ k - 1 ]; /* Q12*/
213             ylo = silk_LSHIFT( 1 - ( root_ix & 2 ), 12 );
214         } else {
215             /* Increment loop counter */
216             k++;
217             xlo = xhi;
218             ylo = yhi;
219             thr = 0;
220
221             if( k > LSF_COS_TAB_SZ_FIX ) {
222                 i++;
223                 if( i > MAX_ITERATIONS_A2NLSF_FIX ) {
224                     /* Set NLSFs to white spectrum and exit */
225                     NLSF[ 0 ] = (opus_int16)silk_DIV32_16( 1 << 15, d + 1 );
226                     for( k = 1; k < d; k++ ) {
227                         NLSF[ k ] = (opus_int16)silk_SMULBB( k + 1, NLSF[ 0 ] );
228                     }
229                     return;
230                 }
231
232                 /* Error: Apply progressively more bandwidth expansion and run again */
233                 silk_bwexpander_32( a_Q16, d, 65536 - silk_SMULBB( 10 + i, i ) ); /* 10_Q16 = 0.00015*/
234
235                 silk_A2NLSF_init( a_Q16, P, Q, dd );
236                 p = P;                            /* Pointer to polynomial */
237                 xlo = silk_LSFCosTab_FIX_Q12[ 0 ]; /* Q12*/
238                 ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
239                 if( ylo < 0 ) {
240                     /* Set the first NLSF to zero and move on to the next */
241                     NLSF[ 0 ] = 0;
242                     p = Q;                        /* Pointer to polynomial */
243                     ylo = silk_A2NLSF_eval_poly( p, xlo, dd );
244                     root_ix = 1;                  /* Index of current root */
245                 } else {
246                     root_ix = 0;                  /* Index of current root */
247                 }
248                 k = 1;                            /* Reset loop counter */
249             }
250         }
251     }
252 }