diff options
author | Sean Bartell <wingedtachikoma@gmail.com> | 2011-06-25 21:32:25 -0400 |
---|---|---|
committer | Nils Wallménius <nils@rockbox.org> | 2012-04-25 22:13:20 +0200 |
commit | f40bfc9267b13b54e6379dfe7539447662879d24 (patch) | |
tree | 9b20069d5e62809ff434061ad730096836f916f2 /lib/rbcodec/codecs/libspeex/lsp.c | |
parent | a0009907de7a0107d49040d8a180f140e2eff299 (diff) | |
download | rockbox-f40bfc9267b13b54e6379dfe7539447662879d24.tar.gz rockbox-f40bfc9267b13b54e6379dfe7539447662879d24.zip |
Add codecs to librbcodec.
Change-Id: Id7f4717d51ed02d67cb9f9cb3c0ada4a81843f97
Reviewed-on: http://gerrit.rockbox.org/137
Reviewed-by: Nils Wallménius <nils@rockbox.org>
Tested-by: Nils Wallménius <nils@rockbox.org>
Diffstat (limited to 'lib/rbcodec/codecs/libspeex/lsp.c')
-rw-r--r-- | lib/rbcodec/codecs/libspeex/lsp.c | 661 |
1 files changed, 661 insertions, 0 deletions
diff --git a/lib/rbcodec/codecs/libspeex/lsp.c b/lib/rbcodec/codecs/libspeex/lsp.c new file mode 100644 index 0000000000..8408d782aa --- /dev/null +++ b/lib/rbcodec/codecs/libspeex/lsp.c | |||
@@ -0,0 +1,661 @@ | |||
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-speex.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 | #ifndef SPEEX_DISABLE_ENCODER | ||
138 | |||
139 | #ifdef FIXED_POINT | ||
140 | |||
141 | #ifndef OVERRIDE_CHEB_POLY_EVA | ||
142 | static inline spx_word32_t cheb_poly_eva( | ||
143 | spx_word16_t *coef, /* P or Q coefs in Q13 format */ | ||
144 | spx_word16_t x, /* cos of freq (-1.0 to 1.0) in Q14 format */ | ||
145 | int m, /* LPC order/2 */ | ||
146 | char *stack | ||
147 | ) | ||
148 | { | ||
149 | int i; | ||
150 | spx_word16_t b0, b1; | ||
151 | spx_word32_t sum; | ||
152 | |||
153 | /*Prevents overflows*/ | ||
154 | if (x>16383) | ||
155 | x = 16383; | ||
156 | if (x<-16383) | ||
157 | x = -16383; | ||
158 | |||
159 | /* Initialise values */ | ||
160 | b1=16384; | ||
161 | b0=x; | ||
162 | |||
163 | /* Evaluate Chebyshev series formulation usin g iterative approach */ | ||
164 | sum = ADD32(EXTEND32(coef[m]), EXTEND32(MULT16_16_P14(coef[m-1],x))); | ||
165 | for(i=2;i<=m;i++) | ||
166 | { | ||
167 | spx_word16_t tmp=b0; | ||
168 | b0 = SUB16(MULT16_16_Q13(x,b0), b1); | ||
169 | b1 = tmp; | ||
170 | sum = ADD32(sum, EXTEND32(MULT16_16_P14(coef[m-i],b0))); | ||
171 | } | ||
172 | |||
173 | return sum; | ||
174 | } | ||
175 | #endif | ||
176 | |||
177 | #else | ||
178 | |||
179 | static float cheb_poly_eva(spx_word32_t *coef, spx_word16_t x, int m, char *stack) | ||
180 | { | ||
181 | int k; | ||
182 | float b0, b1, tmp; | ||
183 | |||
184 | /* Initial conditions */ | ||
185 | b0=0; /* b_(m+1) */ | ||
186 | b1=0; /* b_(m+2) */ | ||
187 | |||
188 | x*=2; | ||
189 | |||
190 | /* Calculate the b_(k) */ | ||
191 | for(k=m;k>0;k--) | ||
192 | { | ||
193 | tmp=b0; /* tmp holds the previous value of b0 */ | ||
194 | b0=x*b0-b1+coef[m-k]; /* b0 holds its new value based on b0 and b1 */ | ||
195 | b1=tmp; /* b1 holds the previous value of b0 */ | ||
196 | } | ||
197 | |||
198 | return(-b1+.5*x*b0+coef[m]); | ||
199 | } | ||
200 | #endif | ||
201 | |||
202 | /*---------------------------------------------------------------------------*\ | ||
203 | |||
204 | FUNCTION....: lpc_to_lsp() | ||
205 | |||
206 | AUTHOR......: David Rowe | ||
207 | DATE CREATED: 24/2/93 | ||
208 | |||
209 | This function converts LPC coefficients to LSP | ||
210 | coefficients. | ||
211 | |||
212 | \*---------------------------------------------------------------------------*/ | ||
213 | |||
214 | #ifdef FIXED_POINT | ||
215 | #define SIGN_CHANGE(a,b) (((a)&0x70000000)^((b)&0x70000000)||(b==0)) | ||
216 | #else | ||
217 | #define SIGN_CHANGE(a,b) (((a)*(b))<0.0) | ||
218 | #endif | ||
219 | |||
220 | |||
221 | int lpc_to_lsp (spx_coef_t *a,int lpcrdr,spx_lsp_t *freq,int nb,spx_word16_t delta, char *stack) | ||
222 | /* float *a lpc coefficients */ | ||
223 | /* int lpcrdr order of LPC coefficients (10) */ | ||
224 | /* float *freq LSP frequencies in the x domain */ | ||
225 | /* int nb number of sub-intervals (4) */ | ||
226 | /* float delta grid spacing interval (0.02) */ | ||
227 | |||
228 | |||
229 | { | ||
230 | spx_word16_t temp_xr,xl,xr,xm=0; | ||
231 | spx_word32_t psuml,psumr,psumm,temp_psumr/*,temp_qsumr*/; | ||
232 | int i,j,m,flag,k; | ||
233 | VARDECL(spx_word32_t *Q); /* ptrs for memory allocation */ | ||
234 | VARDECL(spx_word32_t *P); | ||
235 | VARDECL(spx_word16_t *Q16); /* ptrs for memory allocation */ | ||
236 | VARDECL(spx_word16_t *P16); | ||
237 | spx_word32_t *px; /* ptrs of respective P'(z) & Q'(z) */ | ||
238 | spx_word32_t *qx; | ||
239 | spx_word32_t *p; | ||
240 | spx_word32_t *q; | ||
241 | spx_word16_t *pt; /* ptr used for cheb_poly_eval() | ||
242 | whether P' or Q' */ | ||
243 | int roots=0; /* DR 8/2/94: number of roots found */ | ||
244 | flag = 1; /* program is searching for a root when, | ||
245 | 1 else has found one */ | ||
246 | m = lpcrdr/2; /* order of P'(z) & Q'(z) polynomials */ | ||
247 | |||
248 | /* Allocate memory space for polynomials */ | ||
249 | ALLOC(Q, (m+1), spx_word32_t); | ||
250 | ALLOC(P, (m+1), spx_word32_t); | ||
251 | |||
252 | /* determine P'(z)'s and Q'(z)'s coefficients where | ||
253 | P'(z) = P(z)/(1 + z^(-1)) and Q'(z) = Q(z)/(1-z^(-1)) */ | ||
254 | |||
255 | px = P; /* initialise ptrs */ | ||
256 | qx = Q; | ||
257 | p = px; | ||
258 | q = qx; | ||
259 | |||
260 | #ifdef FIXED_POINT | ||
261 | *px++ = LPC_SCALING; | ||
262 | *qx++ = LPC_SCALING; | ||
263 | for(i=0;i<m;i++){ | ||
264 | *px++ = SUB32(ADD32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *p++); | ||
265 | *qx++ = ADD32(SUB32(EXTEND32(a[i]),EXTEND32(a[lpcrdr-i-1])), *q++); | ||
266 | } | ||
267 | px = P; | ||
268 | qx = Q; | ||
269 | for(i=0;i<m;i++) | ||
270 | { | ||
271 | /*if (fabs(*px)>=32768) | ||
272 | speex_warning_int("px", *px); | ||
273 | if (fabs(*qx)>=32768) | ||
274 | speex_warning_int("qx", *qx);*/ | ||
275 | *px = PSHR32(*px,2); | ||
276 | *qx = PSHR32(*qx,2); | ||
277 | px++; | ||
278 | qx++; | ||
279 | } | ||
280 | /* The reason for this lies in the way cheb_poly_eva() is implemented for fixed-point */ | ||
281 | P[m] = PSHR32(P[m],3); | ||
282 | Q[m] = PSHR32(Q[m],3); | ||
283 | #else | ||
284 | *px++ = LPC_SCALING; | ||
285 | *qx++ = LPC_SCALING; | ||
286 | for(i=0;i<m;i++){ | ||
287 | *px++ = (a[i]+a[lpcrdr-1-i]) - *p++; | ||
288 | *qx++ = (a[i]-a[lpcrdr-1-i]) + *q++; | ||
289 | } | ||
290 | px = P; | ||
291 | qx = Q; | ||
292 | for(i=0;i<m;i++){ | ||
293 | *px = 2**px; | ||
294 | *qx = 2**qx; | ||
295 | px++; | ||
296 | qx++; | ||
297 | } | ||
298 | #endif | ||
299 | |||
300 | px = P; /* re-initialise ptrs */ | ||
301 | qx = Q; | ||
302 | |||
303 | /* now that we have computed P and Q convert to 16 bits to | ||
304 | speed up cheb_poly_eval */ | ||
305 | |||
306 | ALLOC(P16, m+1, spx_word16_t); | ||
307 | ALLOC(Q16, m+1, spx_word16_t); | ||
308 | |||
309 | for (i=0;i<m+1;i++) | ||
310 | { | ||
311 | P16[i] = P[i]; | ||
312 | Q16[i] = Q[i]; | ||
313 | } | ||
314 | |||
315 | /* Search for a zero in P'(z) polynomial first and then alternate to Q'(z). | ||
316 | Keep alternating between the two polynomials as each zero is found */ | ||
317 | |||
318 | xr = 0; /* initialise xr to zero */ | ||
319 | xl = FREQ_SCALE; /* start at point xl = 1 */ | ||
320 | |||
321 | for(j=0;j<lpcrdr;j++){ | ||
322 | if(j&1) /* determines whether P' or Q' is eval. */ | ||
323 | pt = Q16; | ||
324 | else | ||
325 | pt = P16; | ||
326 | |||
327 | psuml = cheb_poly_eva(pt,xl,m,stack); /* evals poly. at xl */ | ||
328 | flag = 1; | ||
329 | while(flag && (xr >= -FREQ_SCALE)){ | ||
330 | spx_word16_t dd; | ||
331 | /* Modified by JMV to provide smaller steps around x=+-1 */ | ||
332 | #ifdef FIXED_POINT | ||
333 | dd = MULT16_16_Q15(delta,SUB16(FREQ_SCALE, MULT16_16_Q14(MULT16_16_Q14(xl,xl),14000))); | ||
334 | if (psuml<512 && psuml>-512) | ||
335 | dd = PSHR16(dd,1); | ||
336 | #else | ||
337 | dd=delta*(1-.9*xl*xl); | ||
338 | if (fabs(psuml)<.2) | ||
339 | dd *= .5; | ||
340 | #endif | ||
341 | xr = SUB16(xl, dd); /* interval spacing */ | ||
342 | psumr = cheb_poly_eva(pt,xr,m,stack);/* poly(xl-delta_x) */ | ||
343 | temp_psumr = psumr; | ||
344 | temp_xr = xr; | ||
345 | |||
346 | /* if no sign change increment xr and re-evaluate poly(xr). Repeat til | ||
347 | sign change. | ||
348 | if a sign change has occurred the interval is bisected and then | ||
349 | checked again for a sign change which determines in which | ||
350 | interval the zero lies in. | ||
351 | If there is no sign change between poly(xm) and poly(xl) set interval | ||
352 | between xm and xr else set interval between xl and xr and repeat till | ||
353 | root is located within the specified limits */ | ||
354 | |||
355 | if(SIGN_CHANGE(psumr,psuml)) | ||
356 | { | ||
357 | roots++; | ||
358 | |||
359 | psumm=psuml; | ||
360 | for(k=0;k<=nb;k++){ | ||
361 | #ifdef FIXED_POINT | ||
362 | xm = ADD16(PSHR16(xl,1),PSHR16(xr,1)); /* bisect the interval */ | ||
363 | #else | ||
364 | xm = .5*(xl+xr); /* bisect the interval */ | ||
365 | #endif | ||
366 | psumm=cheb_poly_eva(pt,xm,m,stack); | ||
367 | /*if(psumm*psuml>0.)*/ | ||
368 | if(!SIGN_CHANGE(psumm,psuml)) | ||
369 | { | ||
370 | psuml=psumm; | ||
371 | xl=xm; | ||
372 | } else { | ||
373 | psumr=psumm; | ||
374 | xr=xm; | ||
375 | } | ||
376 | } | ||
377 | |||
378 | /* once zero is found, reset initial interval to xr */ | ||
379 | freq[j] = X2ANGLE(xm); | ||
380 | xl = xm; | ||
381 | flag = 0; /* reset flag for next search */ | ||
382 | } | ||
383 | else{ | ||
384 | psuml=temp_psumr; | ||
385 | xl=temp_xr; | ||
386 | } | ||
387 | } | ||
388 | } | ||
389 | return(roots); | ||
390 | } | ||
391 | |||
392 | #endif /* SPEEX_DISABLE_ENCODER */ | ||
393 | |||
394 | /*---------------------------------------------------------------------------*\ | ||
395 | |||
396 | FUNCTION....: lsp_to_lpc() | ||
397 | |||
398 | AUTHOR......: David Rowe | ||
399 | DATE CREATED: 24/2/93 | ||
400 | |||
401 | Converts LSP coefficients to LPC coefficients. | ||
402 | |||
403 | \*---------------------------------------------------------------------------*/ | ||
404 | |||
405 | #ifdef FIXED_POINT | ||
406 | |||
407 | void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack) | ||
408 | /* float *freq array of LSP frequencies in the x domain */ | ||
409 | /* float *ak array of LPC coefficients */ | ||
410 | /* int lpcrdr order of LPC coefficients */ | ||
411 | { | ||
412 | (void)stack; | ||
413 | int i,j; | ||
414 | spx_word32_t xout1,xout2,xin; | ||
415 | spx_word32_t mult, a; | ||
416 | VARDECL(spx_word16_t *freqn); | ||
417 | VARDECL(spx_word32_t **xp); | ||
418 | VARDECL(spx_word32_t *xpmem); | ||
419 | VARDECL(spx_word32_t **xq); | ||
420 | VARDECL(spx_word32_t *xqmem); | ||
421 | int m = lpcrdr>>1; | ||
422 | |||
423 | /* | ||
424 | |||
425 | Reconstruct P(z) and Q(z) by cascading second order polynomials | ||
426 | in form 1 - 2cos(w)z(-1) + z(-2), where w is the LSP frequency. | ||
427 | In the time domain this is: | ||
428 | |||
429 | y(n) = x(n) - 2cos(w)x(n-1) + x(n-2) | ||
430 | |||
431 | This is what the ALLOCS below are trying to do: | ||
432 | |||
433 | int xp[m+1][lpcrdr+1+2]; // P matrix in QIMP | ||
434 | int xq[m+1][lpcrdr+1+2]; // Q matrix in QIMP | ||
435 | |||
436 | These matrices store the output of each stage on each row. The | ||
437 | final (m-th) row has the output of the final (m-th) cascaded | ||
438 | 2nd order filter. The first row is the impulse input to the | ||
439 | system (not written as it is known). | ||
440 | |||
441 | The version below takes advantage of the fact that a lot of the | ||
442 | outputs are zero or known, for example if we put an inpulse | ||
443 | into the first section the "clock" it 10 times only the first 3 | ||
444 | outputs samples are non-zero (it's an FIR filter). | ||
445 | */ | ||
446 | |||
447 | ALLOC(xp, (m+1), spx_word32_t*); | ||
448 | ALLOC(xpmem, (m+1)*(lpcrdr+1+2), spx_word32_t); | ||
449 | |||
450 | ALLOC(xq, (m+1), spx_word32_t*); | ||
451 | ALLOC(xqmem, (m+1)*(lpcrdr+1+2), spx_word32_t); | ||
452 | |||
453 | for(i=0; i<=m; i++) { | ||
454 | xp[i] = xpmem + i*(lpcrdr+1+2); | ||
455 | xq[i] = xqmem + i*(lpcrdr+1+2); | ||
456 | } | ||
457 | |||
458 | /* work out 2cos terms in Q14 */ | ||
459 | |||
460 | ALLOC(freqn, lpcrdr, spx_word16_t); | ||
461 | for (i=0;i<lpcrdr;i++) | ||
462 | freqn[i] = ANGLE2X(freq[i]); | ||
463 | |||
464 | #define QIMP 21 /* scaling for impulse */ | ||
465 | |||
466 | xin = SHL32(EXTEND32(1), (QIMP-1)); /* 0.5 in QIMP format */ | ||
467 | |||
468 | /* first col and last non-zero values of each row are trivial */ | ||
469 | |||
470 | for(i=0;i<=m;i++) { | ||
471 | xp[i][1] = 0; | ||
472 | xp[i][2] = xin; | ||
473 | xp[i][2+2*i] = xin; | ||
474 | xq[i][1] = 0; | ||
475 | xq[i][2] = xin; | ||
476 | xq[i][2+2*i] = xin; | ||
477 | } | ||
478 | |||
479 | /* 2nd row (first output row) is trivial */ | ||
480 | |||
481 | xp[1][3] = -MULT16_32_Q14(freqn[0],xp[0][2]); | ||
482 | xq[1][3] = -MULT16_32_Q14(freqn[1],xq[0][2]); | ||
483 | |||
484 | xout1 = xout2 = 0; | ||
485 | |||
486 | /* now generate remaining rows */ | ||
487 | |||
488 | for(i=1;i<m;i++) { | ||
489 | |||
490 | for(j=1;j<2*(i+1)-1;j++) { | ||
491 | mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]); | ||
492 | xp[i+1][j+2] = ADD32(SUB32(xp[i][j+2], mult), xp[i][j]); | ||
493 | mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]); | ||
494 | xq[i+1][j+2] = ADD32(SUB32(xq[i][j+2], mult), xq[i][j]); | ||
495 | } | ||
496 | |||
497 | /* for last col xp[i][j+2] = xq[i][j+2] = 0 */ | ||
498 | |||
499 | mult = MULT16_32_Q14(freqn[2*i],xp[i][j+1]); | ||
500 | xp[i+1][j+2] = SUB32(xp[i][j], mult); | ||
501 | mult = MULT16_32_Q14(freqn[2*i+1],xq[i][j+1]); | ||
502 | xq[i+1][j+2] = SUB32(xq[i][j], mult); | ||
503 | } | ||
504 | |||
505 | /* process last row to extra a{k} */ | ||
506 | |||
507 | for(j=1;j<=lpcrdr;j++) { | ||
508 | int shift = QIMP-13; | ||
509 | |||
510 | /* final filter sections */ | ||
511 | a = PSHR32(xp[m][j+2] + xout1 + xq[m][j+2] - xout2, shift); | ||
512 | xout1 = xp[m][j+2]; | ||
513 | xout2 = xq[m][j+2]; | ||
514 | |||
515 | /* hard limit ak's to +/- 32767 */ | ||
516 | |||
517 | if (a < -32767) a = -32767; | ||
518 | if (a > 32767) a = 32767; | ||
519 | ak[j-1] = (short)a; | ||
520 | |||
521 | } | ||
522 | |||
523 | } | ||
524 | |||
525 | #else | ||
526 | |||
527 | void lsp_to_lpc(spx_lsp_t *freq,spx_coef_t *ak,int lpcrdr, char *stack) | ||
528 | /* float *freq array of LSP frequencies in the x domain */ | ||
529 | /* float *ak array of LPC coefficients */ | ||
530 | /* int lpcrdr order of LPC coefficients */ | ||
531 | |||
532 | |||
533 | { | ||
534 | int i,j; | ||
535 | float xout1,xout2,xin1,xin2; | ||
536 | VARDECL(float *Wp); | ||
537 | float *pw,*n1,*n2,*n3,*n4=NULL; | ||
538 | VARDECL(float *x_freq); | ||
539 | int m = lpcrdr>>1; | ||
540 | |||
541 | ALLOC(Wp, 4*m+2, float); | ||
542 | pw = Wp; | ||
543 | |||
544 | /* initialise contents of array */ | ||
545 | |||
546 | for(i=0;i<=4*m+1;i++){ /* set contents of buffer to 0 */ | ||
547 | *pw++ = 0.0; | ||
548 | } | ||
549 | |||
550 | /* Set pointers up */ | ||
551 | |||
552 | pw = Wp; | ||
553 | xin1 = 1.0; | ||
554 | xin2 = 1.0; | ||
555 | |||
556 | ALLOC(x_freq, lpcrdr, float); | ||
557 | for (i=0;i<lpcrdr;i++) | ||
558 | x_freq[i] = ANGLE2X(freq[i]); | ||
559 | |||
560 | /* reconstruct P(z) and Q(z) by cascading second order | ||
561 | polynomials in form 1 - 2xz(-1) +z(-2), where x is the | ||
562 | LSP coefficient */ | ||
563 | |||
564 | for(j=0;j<=lpcrdr;j++){ | ||
565 | int i2=0; | ||
566 | for(i=0;i<m;i++,i2+=2){ | ||
567 | n1 = pw+(i*4); | ||
568 | n2 = n1 + 1; | ||
569 | n3 = n2 + 1; | ||
570 | n4 = n3 + 1; | ||
571 | xout1 = xin1 - 2.f*x_freq[i2] * *n1 + *n2; | ||
572 | xout2 = xin2 - 2.f*x_freq[i2+1] * *n3 + *n4; | ||
573 | *n2 = *n1; | ||
574 | *n4 = *n3; | ||
575 | *n1 = xin1; | ||
576 | *n3 = xin2; | ||
577 | xin1 = xout1; | ||
578 | xin2 = xout2; | ||
579 | } | ||
580 | xout1 = xin1 + *(n4+1); | ||
581 | xout2 = xin2 - *(n4+2); | ||
582 | if (j>0) | ||
583 | ak[j-1] = (xout1 + xout2)*0.5f; | ||
584 | *(n4+1) = xin1; | ||
585 | *(n4+2) = xin2; | ||
586 | |||
587 | xin1 = 0.0; | ||
588 | xin2 = 0.0; | ||
589 | } | ||
590 | |||
591 | } | ||
592 | #endif | ||
593 | |||
594 | |||
595 | #ifdef FIXED_POINT | ||
596 | |||
597 | /*Makes sure the LSPs are stable*/ | ||
598 | void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin) | ||
599 | { | ||
600 | int i; | ||
601 | spx_word16_t m = margin; | ||
602 | spx_word16_t m2 = 25736-margin; | ||
603 | |||
604 | if (lsp[0]<m) | ||
605 | lsp[0]=m; | ||
606 | if (lsp[len-1]>m2) | ||
607 | lsp[len-1]=m2; | ||
608 | for (i=1;i<len-1;i++) | ||
609 | { | ||
610 | if (lsp[i]<lsp[i-1]+m) | ||
611 | lsp[i]=lsp[i-1]+m; | ||
612 | |||
613 | if (lsp[i]>lsp[i+1]-m) | ||
614 | lsp[i]= SHR16(lsp[i],1) + SHR16(lsp[i+1]-m,1); | ||
615 | } | ||
616 | } | ||
617 | |||
618 | |||
619 | 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) | ||
620 | { | ||
621 | int i; | ||
622 | spx_word16_t tmp = DIV32_16(SHL32(EXTEND32(1 + subframe),14),nb_subframes); | ||
623 | spx_word16_t tmp2 = 16384-tmp; | ||
624 | for (i=0;i<len;i++) | ||
625 | { | ||
626 | interp_lsp[i] = MULT16_16_P14(tmp2,old_lsp[i]) + MULT16_16_P14(tmp,new_lsp[i]); | ||
627 | } | ||
628 | } | ||
629 | |||
630 | #else | ||
631 | |||
632 | /*Makes sure the LSPs are stable*/ | ||
633 | void lsp_enforce_margin(spx_lsp_t *lsp, int len, spx_word16_t margin) | ||
634 | { | ||
635 | int i; | ||
636 | if (lsp[0]<LSP_SCALING*margin) | ||
637 | lsp[0]=LSP_SCALING*margin; | ||
638 | if (lsp[len-1]>LSP_SCALING*(M_PI-margin)) | ||
639 | lsp[len-1]=LSP_SCALING*(M_PI-margin); | ||
640 | for (i=1;i<len-1;i++) | ||
641 | { | ||
642 | if (lsp[i]<lsp[i-1]+LSP_SCALING*margin) | ||
643 | lsp[i]=lsp[i-1]+LSP_SCALING*margin; | ||
644 | |||
645 | if (lsp[i]>lsp[i+1]-LSP_SCALING*margin) | ||
646 | lsp[i]= .5f* (lsp[i] + lsp[i+1]-LSP_SCALING*margin); | ||
647 | } | ||
648 | } | ||
649 | |||
650 | |||
651 | 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) | ||
652 | { | ||
653 | int i; | ||
654 | float tmp = (1.0f + subframe)/nb_subframes; | ||
655 | for (i=0;i<len;i++) | ||
656 | { | ||
657 | interp_lsp[i] = (1-tmp)*old_lsp[i] + tmp*new_lsp[i]; | ||
658 | } | ||
659 | } | ||
660 | |||
661 | #endif | ||