1/***********************************************************************
2Copyright (c) 2006-2011, Skype Limited. All rights reserved.
3Redistribution and use in source and binary forms, with or without
4modification, are permitted provided that the following conditions
5are met:
6- Redistributions of source code must retain the above copyright notice,
7this list of conditions and the following disclaimer.
8- Redistributions in binary form must reproduce the above copyright
9notice, this list of conditions and the following disclaimer in the
10documentation and/or other materials provided with the distribution.
11- Neither the name of Internet Society, IETF or IETF Trust, nor the
12names of specific contributors, may be used to endorse or promote
13products derived from this software without specific prior written
14permission.
15THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
16AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
17IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
18ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
19LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
20CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
21SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
22INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
23CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
24ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
25POSSIBILITY 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  */
47static OPUS_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          */
63static OPUS_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
80static OPUS_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. */
112void 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}
253