Home | History | Annotate | Download | only in libspeex
      1 /*---------------------------------------------------------------------------*\
      2 Original copyright
      3 	FILE........: lsp.c
      4 	AUTHOR......: David Rowe
      5 	DATE CREATED: 24/2/93
      6 
      7 Heavily modified by Jean-Marc Valin (c) 2002-2006 (fixed-point,
      8                        optimizations, additional functions, ...)
      9 
     10    This file contains functions for converting Linear Prediction
     11    Coefficients (LPC) to Line Spectral Pair (LSP) and back. Note that the
     12    LSP coefficients are not in radians format but in the x domain of the
     13    unit circle.
     14 
     15    Speex License:
     16 
     17    Redistribution and use in source and binary forms, with or without
     18    modification, are permitted provided that the following conditions
     19    are met:
     20 
     21    - Redistributions of source code must retain the above copyright
     22    notice, this list of conditions and the following disclaimer.
     23 
     24    - Redistributions in binary form must reproduce the above copyright
     25    notice, this list of conditions and the following disclaimer in the
     26    documentation and/or other materials provided with the distribution.
     27 
     28    - Neither the name of the Xiph.org Foundation nor the names of its
     29    contributors may be used to endorse or promote products derived from
     30    this software without specific prior written permission.
     31 
     32    THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
     33    ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
     34    LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
     35    A PARTICULAR PURPOSE ARE DISCLAIMED.  IN NO EVENT SHALL THE FOUNDATION OR
     36    CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
     37    EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
     38    PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
     39    PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
     40    LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
     41    NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
     42    SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
     43 */
     44 
     45 /*---------------------------------------------------------------------------*\
     46 
     47   Introduction to Line Spectrum Pairs (LSPs)
     48   ------------------------------------------
     49 
     50   LSPs are used to encode the LPC filter coefficients {ak} for
     51   transmission over the channel.  LSPs have several properties (like
     52   less sensitivity to quantisation noise) that make them superior to
     53   direct quantisation of {ak}.
     54 
     55   A(z) is a polynomial of order lpcrdr with {ak} as the coefficients.
     56 
     57   A(z) is transformed to P(z) and Q(z) (using a substitution and some
     58   algebra), to obtain something like:
     59 
     60     A(z) = 0.5[P(z)(z+z^-1) + Q(z)(z-z^-1)]  (1)
     61 
     62   As you can imagine A(z) has complex zeros all over the z-plane. P(z)
     63   and Q(z) have the very neat property of only having zeros _on_ the
     64   unit circle.  So to find them we take a test point z=exp(jw) and
     65   evaluate P (exp(jw)) and Q(exp(jw)) using a grid of points between 0
     66   and pi.
     67 
     68   The zeros (roots) of P(z) also happen to alternate, which is why we
     69   swap coefficients as we find roots.  So the process of finding the
     70   LSP frequencies is basically finding the roots of 5th order
     71   polynomials.
     72 
     73   The root so P(z) and Q(z) occur in symmetrical pairs at +/-w, hence
     74   the name Line Spectrum Pairs (LSPs).
     75 
     76   To convert back to ak we just evaluate (1), "clocking" an impulse
     77   thru it lpcrdr times gives us the impulse response of A(z) which is
     78   {ak}.
     79 
     80 \*---------------------------------------------------------------------------*/
     81 
     82 #ifdef HAVE_CONFIG_H
     83 #include "config.h"
     84 #endif
     85 
     86 #include <math.h>
     87 #include "lsp.h"
     88 #include "stack_alloc.h"
     89 #include "math_approx.h"
     90 
     91 #ifndef M_PI
     92 #define M_PI           3.14159265358979323846  /* pi */
     93 #endif
     94 
     95 #ifndef NULL
     96 #define NULL 0
     97 #endif
     98 
     99 #ifdef FIXED_POINT
    100 
    101 #define FREQ_SCALE 16384
    102 
    103 /*#define ANGLE2X(a) (32768*cos(((a)/8192.)))*/
    104 #define ANGLE2X(a) (SHL16(spx_cos(a),2))
    105 
    106 /*#define X2ANGLE(x) (acos(.00006103515625*(x))*LSP_SCALING)*/
    107 #define X2ANGLE(x) (spx_acos(x))
    108 
    109 #ifdef BFIN_ASM
    110 #include "lsp_bfin.h"
    111 #endif
    112 
    113 #else
    114 
    115 /*#define C1 0.99940307
    116 #define C2 -0.49558072
    117 #define C3 0.03679168*/
    118 
    119 #define FREQ_SCALE 1.
    120 #define ANGLE2X(a) (spx_cos(a))
    121 #define X2ANGLE(x) (acos(x))
    122 
    123 #endif
    124 
    125 
    126 /*---------------------------------------------------------------------------*\
    127 
    128    FUNCTION....: cheb_poly_eva()
    129 
    130    AUTHOR......: David Rowe
    131    DATE CREATED: 24/2/93
    132 
    133    This function evaluates a series of Chebyshev polynomials
    134 
    135 \*---------------------------------------------------------------------------*/
    136 
    137 #ifdef FIXED_POINT
    138 
    139 #ifndef OVERRIDE_CHEB_POLY_EVA
    140 static inline spx_word32_t cheb_poly_eva(
    141   spx_word16_t *coef, /* P or Q coefs in Q13 format               */
    142   spx_word16_t     x, /* cos of freq (-1.0 to 1.0) in Q14 format  */
    143   int              m, /* LPC order/2                              */
    144   char         *stack
    145 )
    146 {
    147     int i;
    148     spx_word16_t b0, b1;
    149     spx_word32_t sum;
    150 
    151     /*Prevents overflows*/
    152     if (x>16383)
    153        x = 16383;
    154     if (x<-16383)
    155        x = -16383;
    156 
    157     /* Initialise values */
    158     b1=16384;
    159     b0=x;
    160 
    161     /* Evaluate Chebyshev series formulation usin g iterative approach  */
    162     sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x)));
    163     for(i=2;i<=m;i++)
    164     {
    165        spx_word16_t tmp=b0;
    166        b0 = SUB16(MULT16_16_Q13(x,b0), b1);
    167        b1 = tmp;
    168        sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0)));
    169     }
    170 
    171     return sum;
    172 }
    173 #endif
    174 
    175 #else
    176 
    177 static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack)
    178 {
    179    int k;
    180    float b0, b1, tmp;
    181 
    182    /* Initial conditions */
    183    b0=0; /* b_(m+1) */
    184    b1=0; /* b_(m+2) */
    185 
    186    x*=2;
    187 
    188    /* Calculate the b_(k) */
    189    for(k=m;k>0;k--)
    190    {
    191       tmp=b0;                           /* tmp holds the previous value of b0 */
    192       b0=x*b0-b1+coef[m-k];    /* b0 holds its new value based on b0 and b1 */
    193       b1=tmp;                           /* b1 holds the previous value of b0 */
    194    }
    195 
    196    return(-b1+.5*x*b0+coef[m]);
    197 }
    198 #endif
    199 
    200 /*---------------------------------------------------------------------------*\
    201 
    202     FUNCTION....: lpc_to_lsp()
    203 
    204     AUTHOR......: David Rowe
    205     DATE CREATED: 24/2/93
    206 
    207     This function converts LPC coefficients to LSP
    208     coefficients.
    209 
    210 \*---------------------------------------------------------------------------*/
    211 
    212 #ifdef FIXED_POINT
    213 #define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0))
    214 #else
    215 #define SIGN_CHANGE(a,b) (((a)*(b))<0.0)
    216 #endif
    217 
    218 
    219 int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack)
    220 /*  float *a 		     	lpc coefficients			*/
    221 /*  int lpcrdr			order of LPC coefficients (10) 		*/
    222 /*  float *freq 	      	LSP frequencies in the x domain       	*/
    223 /*  int nb			number of sub-intervals (4) 		*/
    224 /*  float delta			grid spacing interval (0.02) 		*/
    225 
    226 
    227 {
    228     spx_word16_t temp_xr,xl,xr,xm=0;
    229     spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/;
    230     int i,j,m,flag,k;
    231     VARDECL(spx_word32_t *Q);                 	/* ptrs for memory allocation 		*/
    232     VARDECL(spx_word32_t *P);
    233     VARDECL(spx_word16_t *Q16);         /* ptrs for memory allocation 		*/
    234     VARDECL(spx_word16_t *P16);
    235     spx_word32_t *px;                	/* ptrs of respective P'(z) & Q'(z)	*/
    236     spx_word32_t *qx;
    237     spx_word32_t *p;
    238     spx_word32_t *q;
    239     spx_word16_t *pt;                	/* ptr used for cheb_poly_eval()
    240 				whether P' or Q' 			*/
    241     int roots=0;              	/* DR 8/2/94: number of roots found 	*/
    242     flag = 1;                	/*  program is searching for a root when,
    243 				1 else has found one 			*/
    244     m = lpcrdr/2;            	/* order of P'(z) & Q'(z) polynomials 	*/
    245 
    246     /* Allocate memory space for polynomials */
    247     ALLOC(Q, (m+1), spx_word32_t);
    248     ALLOC(P, (m+1), spx_word32_t);
    249 
    250     /* determine P'(z)'s and Q'(z)'s coefficients where
    251       P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */
    252 
    253     px = P;                      /* initialise ptrs 			*/
    254     qx = Q;
    255     p = px;
    256     q = qx;
    257 
    258 #ifdef FIXED_POINT
    259     *px++ = LPC_SCALING;
    260     *qx++ = LPC_SCALING;
    261     for(i=0;i<m;i++){
    262        *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++);
    263        *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++);
    264     }
    265     px = P;
    266     qx = Q;
    267     for(i=0;i<m;i++)
    268     {
    269        /*if (fabs(*px)>=32768)
    270           speex_warning_int("px", *px);
    271        if (fabs(*qx)>=32768)
    272        speex_warning_int("qx", *qx);*/
    273        *px = PSHR32(*px,2);
    274        *qx = PSHR32(*qx,2);
    275        px++;
    276        qx++;
    277     }
    278     /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */
    279     P[m] = PSHR32(P[m],3);
    280     Q[m] = PSHR32(Q[m],3);
    281 #else
    282     *px++ = LPC_SCALING;
    283     *qx++ = LPC_SCALING;
    284     for(i=0;i<m;i++){
    285        *px++ = (a[i]+a[lpcrdr-1-i]) - *p++;
    286        *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++;
    287     }
    288     px = P;
    289     qx = Q;
    290     for(i=0;i<m;i++){
    291        *px = 2**px;
    292        *qx = 2**qx;
    293        px++;
    294        qx++;
    295     }
    296 #endif
    297 
    298     px = P;             	/* re-initialise ptrs 			*/
    299     qx = Q;
    300 
    301     /* now that we have computed P and Q convert to 16 bits to
    302        speed up cheb_poly_eval */
    303 
    304     ALLOC(P16, m+1, spx_word16_t);
    305     ALLOC(Q16, m+1, spx_word16_t);
    306 
    307     for (i=0;i<m+1;i++)
    308     {
    309        P16[i] = P[i];
    310        Q16[i] = Q[i];
    311     }
    312 
    313     /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z).
    314     Keep alternating between the two polynomials as each zero is found 	*/
    315 
    316     xr = 0;             	/* initialise xr to zero 		*/
    317     xl = FREQ_SCALE;               	/* start at point xl = 1 		*/
    318 
    319     for(j=0;j<lpcrdr;j++){
    320 	if(j&1)            	/* determines whether P' or Q' is eval. */
    321 	    pt = Q16;
    322 	else
    323 	    pt = P16;
    324 
    325 	psuml = cheb_poly_eva(pt,xl,m,stack);	/* evals poly. at xl 	*/
    326 	flag = 1;
    327 	while(flag && (xr >= -FREQ_SCALE)){
    328            spx_word16_t dd;
    329            /* Modified by JMV to provide smaller steps around x=+-1 */
    330 #ifdef FIXED_POINT
    331            dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000)));
    332            if (psuml<512 && psuml>-512)
    333               dd = PSHR16(dd,1);
    334 #else
    335            dd=delta*(1-.9*xl*xl);
    336            if (fabs(psuml)<.2)
    337               dd *= .5;
    338 #endif
    339            xr = SUB16(xl, dd);                        	/* interval spacing 	*/
    340 	    psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x) 	*/
    341 	    temp_psumr = psumr;
    342 	    temp_xr = xr;
    343 
    344     /* if no sign change increment xr and re-evaluate poly(xr). Repeat til
    345     sign change.
    346     if a sign change has occurred the interval is bisected and then
    347     checked again for a sign change which determines in which
    348     interval the zero lies in.
    349     If there is no sign change between poly(xm) and poly(xl) set interval
    350     between xm and xr else set interval between xl and xr and repeat till
    351     root is located within the specified limits 			*/
    352 
    353 	    if(SIGN_CHANGE(psumr,psuml))
    354             {
    355 		roots++;
    356 
    357 		psumm=psuml;
    358 		for(k=0;k<=nb;k++){
    359 #ifdef FIXED_POINT
    360 		    xm = ADD16(PSHR16(xl,1),PSHR16(xr,1));        	/* bisect the interval 	*/
    361 #else
    362                     xm = .5*(xl+xr);        	/* bisect the interval 	*/
    363 #endif
    364 		    psumm=cheb_poly_eva(pt,xm,m,stack);
    365 		    /*if(psumm*psuml>0.)*/
    366 		    if(!SIGN_CHANGE(psumm,psuml))
    367                     {
    368 			psuml=psumm;
    369 			xl=xm;
    370 		    } else {
    371 			psumr=psumm;
    372 			xr=xm;
    373 		    }
    374 		}
    375 
    376 	       /* once zero is found, reset initial interval to xr 	*/
    377 	       freq[j] = X2ANGLE(xm);
    378 	       xl = xm;
    379 	       flag = 0;       		/* reset flag for next search 	*/
    380 	    }
    381 	    else{
    382 		psuml=temp_psumr;
    383 		xl=temp_xr;
    384 	    }
    385 	}
    386     }
    387     return(roots);
    388 }
    389 
    390 /*---------------------------------------------------------------------------*\
    391 
    392 	FUNCTION....: lsp_to_lpc()
    393 
    394 	AUTHOR......: David Rowe
    395 	DATE CREATED: 24/2/93
    396 
    397         Converts LSP coefficients to LPC coefficients.
    398 
    399 \*---------------------------------------------------------------------------*/
    400 
    401 #ifdef FIXED_POINT
    402 
    403 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
    404 /*  float *freq 	array of LSP frequencies in the x domain	*/
    405 /*  float *ak 		array of LPC coefficients 			*/
    406 /*  int lpcrdr  	order of LPC coefficients 			*/
    407 {
    408     int i,j;
    409     spx_word32_t xout1,xout2,xin;
    410     spx_word32_t mult, a;
    411     VARDECL(spx_word16_t *freqn);
    412     VARDECL(spx_word32_t **xp);
    413     VARDECL(spx_word32_t *xpmem);
    414     VARDECL(spx_word32_t **xq);
    415     VARDECL(spx_word32_t *xqmem);
    416     int m = lpcrdr>>1;
    417 
    418     /*
    419 
    420        Reconstruct P(z) and Q(z) by cascading second order polynomials
    421        in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency.
    422        In the time domain this is:
    423 
    424        y(n) = x(n) - 2cos(w)x(n-1) + x(n-2)
    425 
    426        This is what the ALLOCS below are trying to do:
    427 
    428          int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP
    429          int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP
    430 
    431        These matrices store the output of each stage on each row.  The
    432        final (m-th) row has the output of the final (m-th) cascaded
    433        2nd order filter.  The first row is the impulse input to the
    434        system (not written as it is known).
    435 
    436        The version below takes advantage of the fact that a lot of the
    437        outputs are zero or known, for example if we put an inpulse
    438        into the first section the "clock" it 10 times only the first 3
    439        outputs samples are non-zero (it's an FIR filter).
    440     */
    441 
    442     ALLOC(xp, (m+1), spx_word32_t*);
    443     ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
    444 
    445     ALLOC(xq, (m+1), spx_word32_t*);
    446     ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t);
    447 
    448     for(i=0; i<=m; i++) {
    449       xp[i] = xpmem + i*(lpcrdr+1+2);
    450       xq[i] = xqmem + i*(lpcrdr+1+2);
    451     }
    452 
    453     /* work out 2cos terms in Q14 */
    454 
    455     ALLOC(freqn, lpcrdr, spx_word16_t);
    456     for (i=0;i<lpcrdr;i++)
    457        freqn[i] = ANGLE2X(freq[i]);
    458 
    459     #define QIMP  21   /* scaling for impulse */
    460 
    461     xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */
    462 
    463     /* first col and last non-zero values of each row are trivial */
    464 
    465     for(i=0;i<=m;i++) {
    466      xp[i][1] = 0;
    467      xp[i][2] = xin;
    468      xp[i][2+2*i] = xin;
    469      xq[i][1] = 0;
    470      xq[i][2] = xin;
    471      xq[i][2+2*i] = xin;
    472     }
    473 
    474     /* 2nd row (first output row) is trivial */
    475 
    476     xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]);
    477     xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]);
    478 
    479     xout1 = xout2 = 0;
    480 
    481     /* now generate remaining rows */
    482 
    483     for(i=1;i<m;i++) {
    484 
    485       for(j=1;j<2*(i+1)-1;j++) {
    486 	mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
    487 	xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]);
    488 	mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
    489 	xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]);
    490       }
    491 
    492       /* for last col xp[i][j+2] = xq[i][j+2] = 0 */
    493 
    494       mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]);
    495       xp[i+1][j+2] = SUB32(xp[i][j], mult);
    496       mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]);
    497       xq[i+1][j+2] = SUB32(xq[i][j], mult);
    498     }
    499 
    500     /* process last row to extra a{k} */
    501 
    502     for(j=1;j<=lpcrdr;j++) {
    503       int shift = QIMP-13;
    504 
    505       /* final filter sections */
    506       a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift);
    507       xout1 = xp[m][j+2];
    508       xout2 = xq[m][j+2];
    509 
    510       /* hard limit ak's to +/- 32767 */
    511 
    512       if (a < -32767) a = -32767;
    513       if (a > 32767) a = 32767;
    514       ak[j-1] = (short)a;
    515 
    516     }
    517 
    518 }
    519 
    520 #else
    521 
    522 void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack)
    523 /*  float *freq 	array of LSP frequencies in the x domain	*/
    524 /*  float *ak 		array of LPC coefficients 			*/
    525 /*  int lpcrdr  	order of LPC coefficients 			*/
    526 
    527 
    528 {
    529     int i,j;
    530     float xout1,xout2,xin1,xin2;
    531     VARDECL(float *Wp);
    532     float *pw,*n1,*n2,*n3,*n4=NULL;
    533     VARDECL(float *x_freq);
    534     int m = lpcrdr>>1;
    535 
    536     ALLOC(Wp, 4*m+2, float);
    537     pw = Wp;
    538 
    539     /* initialise contents of array */
    540 
    541     for(i=0;i<=4*m+1;i++){       	/* set contents of buffer to 0 */
    542 	*pw++ = 0.0;
    543     }
    544 
    545     /* Set pointers up */
    546 
    547     pw = Wp;
    548     xin1 = 1.0;
    549     xin2 = 1.0;
    550 
    551     ALLOC(x_freq, lpcrdr, float);
    552     for (i=0;i<lpcrdr;i++)
    553        x_freq[i] = ANGLE2X(freq[i]);
    554 
    555     /* reconstruct P(z) and Q(z) by  cascading second order
    556       polynomials in form 1 - 2xz(-1) +z(-2), where x is the
    557       LSP coefficient */
    558 
    559     for(j=0;j<=lpcrdr;j++){
    560        int i2=0;
    561 	for(i=0;i<m;i++,i2+=2){
    562 	    n1 = pw+(i*4);
    563 	    n2 = n1 + 1;
    564 	    n3 = n2 + 1;
    565 	    n4 = n3 + 1;
    566 	    xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2;
    567 	    xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4;
    568 	    *n2 = *n1;
    569 	    *n4 = *n3;
    570 	    *n1 = xin1;
    571 	    *n3 = xin2;
    572 	    xin1 = xout1;
    573 	    xin2 = xout2;
    574 	}
    575 	xout1 = xin1 + *(n4+1);
    576 	xout2 = xin2 - *(n4+2);
    577 	if (j>0)
    578 	   ak[j-1] = (xout1 + xout2)*0.5f;
    579 	*(n4+1) = xin1;
    580 	*(n4+2) = xin2;
    581 
    582 	xin1 = 0.0;
    583 	xin2 = 0.0;
    584     }
    585 
    586 }
    587 #endif
    588 
    589 
    590 #ifdef FIXED_POINT
    591 
    592 /*Makes sure the LSPs are stable*/
    593 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
    594 {
    595    int i;
    596    spx_word16_t m = margin;
    597    spx_word16_t m2 = 25736-margin;
    598 
    599    if (lsp[0]<m)
    600       lsp[0]=m;
    601    if (lsp[len-1]>m2)
    602       lsp[len-1]=m2;
    603    for (i=1;i<len-1;i++)
    604    {
    605       if (lsp[i]<lsp[i-1]+m)
    606          lsp[i]=lsp[i-1]+m;
    607 
    608       if (lsp[i]>lsp[i+1]-m)
    609          lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1);
    610    }
    611 }
    612 
    613 
    614 void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
    615 {
    616    int i;
    617    spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes);
    618    spx_word16_t tmp2 = 16384-tmp;
    619    for (i=0;i<len;i++)
    620    {
    621       interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]);
    622    }
    623 }
    624 
    625 #else
    626 
    627 /*Makes sure the LSPs are stable*/
    628 void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin)
    629 {
    630    int i;
    631    if (lsp[0]<LSP_SCALING*margin)
    632       lsp[0]=LSP_SCALING*margin;
    633    if (lsp[len-1]>LSP_SCALING*(M_PI-margin))
    634       lsp[len-1]=LSP_SCALING*(M_PI-margin);
    635    for (i=1;i<len-1;i++)
    636    {
    637       if (lsp[i]<lsp[i-1]+LSP_SCALING*margin)
    638          lsp[i]=lsp[i-1]+LSP_SCALING*margin;
    639 
    640       if (lsp[i]>lsp[i+1]-LSP_SCALING*margin)
    641          lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin);
    642    }
    643 }
    644 
    645 
    646 void lsp_interpolate(spx_lsp_t *old_lsp, spx_lsp_t *new_lsp, spx_lsp_t *interp_lsp, int len, int subframe, int nb_subframes)
    647 {
    648    int i;
    649    float tmp = (1.0f + subframe)/nb_subframes;
    650    for (i=0;i<len;i++)
    651    {
    652       interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i];
    653    }
    654 }
    655 
    656 #endif
    657