1/*---------------------------------------------------------------------------*\ 2Original copyright 3 FILE........: lsp.c 4 AUTHOR......: David Rowe 5 DATE CREATED: 24/2/93 6 7Heavily 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 140static 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 177static 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 219int 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 403void 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 522void 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*/ 593void 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 614void 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*/ 628void 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 646void 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