Tristan Matthews | 0a329cc | 2013-07-17 13:20:14 -0400 | [diff] [blame] | 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 |