1 /* Copyright (C) 2002-2006 Jean-Marc Valin
2 File: ltp.c
3 Long-Term Prediction functions
4
5 Redistribution and use in source and binary forms, with or without
6 modification, are permitted provided that the following conditions
7 are met:
8
9 - Redistributions of source code must retain the above copyright
10 notice, this list of conditions and the following disclaimer.
11
12 - Redistributions in binary form must reproduce the above copyright
13 notice, this list of conditions and the following disclaimer in the
14 documentation and/or other materials provided with the distribution.
15
16 - Neither the name of the Xiph.org Foundation nor the names of its
17 contributors may be used to endorse or promote products derived from
18 this software without specific prior written permission.
19
20 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21 ``AS IS'' AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22 LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
23 A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE FOUNDATION OR
24 CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
25 EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
26 PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
27 PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
28 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
29 NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
30 SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
31 */
32
33 #ifdef HAVE_CONFIG_H
34 #include "config.h"
35 #endif
36
37 #include <math.h>
38 #include "ltp.h"
39 #include "stack_alloc.h"
40 #include "filters.h"
41 #include <speex/speex_bits.h>
42 #include "math_approx.h"
43 #include "os_support.h"
44
45 #ifndef NULL
46 #define NULL 0
47 #endif
48
49
50 #ifdef _USE_SSE
51 #include "ltp_sse.h"
52 #elif defined (ARM4_ASM) || defined(ARM5E_ASM)
53 #include "ltp_arm4.h"
54 #elif defined (BFIN_ASM)
55 #include "ltp_bfin.h"
56 #endif
57
58 #ifndef OVERRIDE_INNER_PROD
inner_prod(const spx_word16_t * x,const spx_word16_t * y,int len)59 spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
60 {
61 spx_word32_t sum=0;
62 len >>= 2;
63 while(len--)
64 {
65 spx_word32_t part=0;
66 part = MAC16_16(part,*x++,*y++);
67 part = MAC16_16(part,*x++,*y++);
68 part = MAC16_16(part,*x++,*y++);
69 part = MAC16_16(part,*x++,*y++);
70 /* HINT: If you had a 40-bit accumulator, you could shift only at the end */
71 sum = ADD32(sum,SHR32(part,6));
72 }
73 return sum;
74 }
75 #endif
76
77 #ifndef OVERRIDE_PITCH_XCORR
78 #if 0 /* HINT: Enable this for machines with enough registers (i.e. not x86) */
79 void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
80 {
81 int i,j;
82 for (i=0;i<nb_pitch;i+=4)
83 {
84 /* Compute correlation*/
85 /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/
86 spx_word32_t sum1=0;
87 spx_word32_t sum2=0;
88 spx_word32_t sum3=0;
89 spx_word32_t sum4=0;
90 const spx_word16_t *y = _y+i;
91 const spx_word16_t *x = _x;
92 spx_word16_t y0, y1, y2, y3;
93 /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/
94 y0=*y++;
95 y1=*y++;
96 y2=*y++;
97 y3=*y++;
98 for (j=0;j<len;j+=4)
99 {
100 spx_word32_t part1;
101 spx_word32_t part2;
102 spx_word32_t part3;
103 spx_word32_t part4;
104 part1 = MULT16_16(*x,y0);
105 part2 = MULT16_16(*x,y1);
106 part3 = MULT16_16(*x,y2);
107 part4 = MULT16_16(*x,y3);
108 x++;
109 y0=*y++;
110 part1 = MAC16_16(part1,*x,y1);
111 part2 = MAC16_16(part2,*x,y2);
112 part3 = MAC16_16(part3,*x,y3);
113 part4 = MAC16_16(part4,*x,y0);
114 x++;
115 y1=*y++;
116 part1 = MAC16_16(part1,*x,y2);
117 part2 = MAC16_16(part2,*x,y3);
118 part3 = MAC16_16(part3,*x,y0);
119 part4 = MAC16_16(part4,*x,y1);
120 x++;
121 y2=*y++;
122 part1 = MAC16_16(part1,*x,y3);
123 part2 = MAC16_16(part2,*x,y0);
124 part3 = MAC16_16(part3,*x,y1);
125 part4 = MAC16_16(part4,*x,y2);
126 x++;
127 y3=*y++;
128
129 sum1 = ADD32(sum1,SHR32(part1,6));
130 sum2 = ADD32(sum2,SHR32(part2,6));
131 sum3 = ADD32(sum3,SHR32(part3,6));
132 sum4 = ADD32(sum4,SHR32(part4,6));
133 }
134 corr[nb_pitch-1-i]=sum1;
135 corr[nb_pitch-2-i]=sum2;
136 corr[nb_pitch-3-i]=sum3;
137 corr[nb_pitch-4-i]=sum4;
138 }
139
140 }
141 #else
pitch_xcorr(const spx_word16_t * _x,const spx_word16_t * _y,spx_word32_t * corr,int len,int nb_pitch,char * stack)142 void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
143 {
144 int i;
145 for (i=0;i<nb_pitch;i++)
146 {
147 /* Compute correlation*/
148 corr[nb_pitch-1-i]=inner_prod(_x, _y+i, len);
149 }
150
151 }
152 #endif
153 #endif
154
155 #ifndef OVERRIDE_COMPUTE_PITCH_ERROR
compute_pitch_error(spx_word16_t * C,spx_word16_t * g,spx_word16_t pitch_control)156 static inline spx_word32_t compute_pitch_error(spx_word16_t *C, spx_word16_t *g, spx_word16_t pitch_control)
157 {
158 spx_word32_t sum = 0;
159 sum = ADD32(sum,MULT16_16(MULT16_16_16(g[0],pitch_control),C[0]));
160 sum = ADD32(sum,MULT16_16(MULT16_16_16(g[1],pitch_control),C[1]));
161 sum = ADD32(sum,MULT16_16(MULT16_16_16(g[2],pitch_control),C[2]));
162 sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[1]),C[3]));
163 sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[1]),C[4]));
164 sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[0]),C[5]));
165 sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[0]),C[6]));
166 sum = SUB32(sum,MULT16_16(MULT16_16_16(g[1],g[1]),C[7]));
167 sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[2]),C[8]));
168 return sum;
169 }
170 #endif
171
172 #ifndef OVERRIDE_OPEN_LOOP_NBEST_PITCH
open_loop_nbest_pitch(spx_word16_t * sw,int start,int end,int len,int * pitch,spx_word16_t * gain,int N,char * stack)173 void open_loop_nbest_pitch(spx_word16_t *sw, int start, int end, int len, int *pitch, spx_word16_t *gain, int N, char *stack)
174 {
175 int i,j,k;
176 VARDECL(spx_word32_t *best_score);
177 VARDECL(spx_word32_t *best_ener);
178 spx_word32_t e0;
179 VARDECL(spx_word32_t *corr);
180 #ifdef FIXED_POINT
181 /* In fixed-point, we need only one (temporary) array of 32-bit values and two (corr16, ener16)
182 arrays for (normalized) 16-bit values */
183 VARDECL(spx_word16_t *corr16);
184 VARDECL(spx_word16_t *ener16);
185 spx_word32_t *energy;
186 int cshift=0, eshift=0;
187 int scaledown = 0;
188 ALLOC(corr16, end-start+1, spx_word16_t);
189 ALLOC(ener16, end-start+1, spx_word16_t);
190 ALLOC(corr, end-start+1, spx_word32_t);
191 energy = corr;
192 #else
193 /* In floating-point, we need to float arrays and no normalized copies */
194 VARDECL(spx_word32_t *energy);
195 spx_word16_t *corr16;
196 spx_word16_t *ener16;
197 ALLOC(energy, end-start+2, spx_word32_t);
198 ALLOC(corr, end-start+1, spx_word32_t);
199 corr16 = corr;
200 ener16 = energy;
201 #endif
202
203 ALLOC(best_score, N, spx_word32_t);
204 ALLOC(best_ener, N, spx_word32_t);
205 for (i=0;i<N;i++)
206 {
207 best_score[i]=-1;
208 best_ener[i]=0;
209 pitch[i]=start;
210 }
211
212 #ifdef FIXED_POINT
213 for (i=-end;i<len;i++)
214 {
215 if (ABS16(sw[i])>16383)
216 {
217 scaledown=1;
218 break;
219 }
220 }
221 /* If the weighted input is close to saturation, then we scale it down */
222 if (scaledown)
223 {
224 for (i=-end;i<len;i++)
225 {
226 sw[i]=SHR16(sw[i],1);
227 }
228 }
229 #endif
230 energy[0]=inner_prod(sw-start, sw-start, len);
231 e0=inner_prod(sw, sw, len);
232 for (i=start;i<end;i++)
233 {
234 /* Update energy for next pitch*/
235 energy[i-start+1] = SUB32(ADD32(energy[i-start],SHR32(MULT16_16(sw[-i-1],sw[-i-1]),6)), SHR32(MULT16_16(sw[-i+len-1],sw[-i+len-1]),6));
236 if (energy[i-start+1] < 0)
237 energy[i-start+1] = 0;
238 }
239
240 #ifdef FIXED_POINT
241 eshift = normalize16(energy, ener16, 32766, end-start+1);
242 #endif
243
244 /* In fixed-point, this actually overrites the energy array (aliased to corr) */
245 pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
246
247 #ifdef FIXED_POINT
248 /* Normalize to 180 so we can square it and it still fits in 16 bits */
249 cshift = normalize16(corr, corr16, 180, end-start+1);
250 /* If we scaled weighted input down, we need to scale it up again (OK, so we've just lost the LSB, who cares?) */
251 if (scaledown)
252 {
253 for (i=-end;i<len;i++)
254 {
255 sw[i]=SHL16(sw[i],1);
256 }
257 }
258 #endif
259
260 /* Search for the best pitch prediction gain */
261 for (i=start;i<=end;i++)
262 {
263 spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
264 /* Instead of dividing the tmp by the energy, we multiply on the other side */
265 if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
266 {
267 /* We can safely put it last and then check */
268 best_score[N-1]=tmp;
269 best_ener[N-1]=ener16[i-start]+1;
270 pitch[N-1]=i;
271 /* Check if it comes in front of others */
272 for (j=0;j<N-1;j++)
273 {
274 if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
275 {
276 for (k=N-1;k>j;k--)
277 {
278 best_score[k]=best_score[k-1];
279 best_ener[k]=best_ener[k-1];
280 pitch[k]=pitch[k-1];
281 }
282 best_score[j]=tmp;
283 best_ener[j]=ener16[i-start]+1;
284 pitch[j]=i;
285 break;
286 }
287 }
288 }
289 }
290
291 /* Compute open-loop gain if necessary */
292 if (gain)
293 {
294 for (j=0;j<N;j++)
295 {
296 spx_word16_t g;
297 i=pitch[j];
298 g = DIV32(SHL32(EXTEND32(corr16[i-start]),cshift), 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(SHL32(EXTEND32(ener16[i-start]),eshift))),6));
299 /* FIXME: g = max(g,corr/energy) */
300 if (g<0)
301 g = 0;
302 gain[j]=g;
303 }
304 }
305
306
307 }
308 #endif
309
310 #ifndef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ
pitch_gain_search_3tap_vq(const signed char * gain_cdbk,int gain_cdbk_size,spx_word16_t * C16,spx_word16_t max_gain)311 static int pitch_gain_search_3tap_vq(
312 const signed char *gain_cdbk,
313 int gain_cdbk_size,
314 spx_word16_t *C16,
315 spx_word16_t max_gain
316 )
317 {
318 const signed char *ptr=gain_cdbk;
319 int best_cdbk=0;
320 spx_word32_t best_sum=-VERY_LARGE32;
321 spx_word32_t sum=0;
322 spx_word16_t g[3];
323 spx_word16_t pitch_control=64;
324 spx_word16_t gain_sum;
325 int i;
326
327 for (i=0;i<gain_cdbk_size;i++) {
328
329 ptr = gain_cdbk+4*i;
330 g[0]=ADD16((spx_word16_t)ptr[0],32);
331 g[1]=ADD16((spx_word16_t)ptr[1],32);
332 g[2]=ADD16((spx_word16_t)ptr[2],32);
333 gain_sum = (spx_word16_t)ptr[3];
334
335 sum = compute_pitch_error(C16, g, pitch_control);
336
337 if (sum>best_sum && gain_sum<=max_gain) {
338 best_sum=sum;
339 best_cdbk=i;
340 }
341 }
342
343 return best_cdbk;
344 }
345 #endif
346
347 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
pitch_gain_search_3tap(const spx_word16_t target[],const spx_coef_t ak[],const spx_coef_t awk1[],const spx_coef_t awk2[],spx_sig_t exc[],const signed char * gain_cdbk,int gain_cdbk_size,int pitch,int p,int nsf,SpeexBits * bits,char * stack,const spx_word16_t * exc2,const spx_word16_t * r,spx_word16_t * new_target,int * cdbk_index,int plc_tuning,spx_word32_t cumul_gain,int scaledown)348 static spx_word32_t pitch_gain_search_3tap(
349 const spx_word16_t target[], /* Target vector */
350 const spx_coef_t ak[], /* LPCs for this subframe */
351 const spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
352 const spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
353 spx_sig_t exc[], /* Excitation */
354 const signed char *gain_cdbk,
355 int gain_cdbk_size,
356 int pitch, /* Pitch value */
357 int p, /* Number of LPC coeffs */
358 int nsf, /* Number of samples in subframe */
359 SpeexBits *bits,
360 char *stack,
361 const spx_word16_t *exc2,
362 const spx_word16_t *r,
363 spx_word16_t *new_target,
364 int *cdbk_index,
365 int plc_tuning,
366 spx_word32_t cumul_gain,
367 int scaledown
368 )
369 {
370 int i,j;
371 VARDECL(spx_word16_t *tmp1);
372 VARDECL(spx_word16_t *e);
373 spx_word16_t *x[3];
374 spx_word32_t corr[3];
375 spx_word32_t A[3][3];
376 spx_word16_t gain[3];
377 spx_word32_t err;
378 spx_word16_t max_gain=128;
379 int best_cdbk=0;
380
381 ALLOC(tmp1, 3*nsf, spx_word16_t);
382 ALLOC(e, nsf, spx_word16_t);
383
384 if (cumul_gain > 262144)
385 max_gain = 31;
386
387 x[0]=tmp1;
388 x[1]=tmp1+nsf;
389 x[2]=tmp1+2*nsf;
390
391 for (j=0;j<nsf;j++)
392 new_target[j] = target[j];
393
394 {
395 VARDECL(spx_mem_t *mm);
396 int pp=pitch-1;
397 ALLOC(mm, p, spx_mem_t);
398 for (j=0;j<nsf;j++)
399 {
400 if (j-pp<0)
401 e[j]=exc2[j-pp];
402 else if (j-pp-pitch<0)
403 e[j]=exc2[j-pp-pitch];
404 else
405 e[j]=0;
406 }
407 #ifdef FIXED_POINT
408 /* Scale target and excitation down if needed (avoiding overflow) */
409 if (scaledown)
410 {
411 for (j=0;j<nsf;j++)
412 e[j] = SHR16(e[j],1);
413 for (j=0;j<nsf;j++)
414 new_target[j] = SHR16(new_target[j],1);
415 }
416 #endif
417 for (j=0;j<p;j++)
418 mm[j] = 0;
419 iir_mem16(e, ak, e, nsf, p, mm, stack);
420 for (j=0;j<p;j++)
421 mm[j] = 0;
422 filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack);
423 for (j=0;j<nsf;j++)
424 x[2][j] = e[j];
425 }
426 for (i=1;i>=0;i--)
427 {
428 spx_word16_t e0=exc2[-pitch-1+i];
429 #ifdef FIXED_POINT
430 /* Scale excitation down if needed (avoiding overflow) */
431 if (scaledown)
432 e0 = SHR16(e0,1);
433 #endif
434 x[i][0]=MULT16_16_Q14(r[0], e0);
435 for (j=0;j<nsf-1;j++)
436 x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0));
437 }
438
439 for (i=0;i<3;i++)
440 corr[i]=inner_prod(x[i],new_target,nsf);
441 for (i=0;i<3;i++)
442 for (j=0;j<=i;j++)
443 A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
444
445 {
446 spx_word32_t C[9];
447 #ifdef FIXED_POINT
448 spx_word16_t C16[9];
449 #else
450 spx_word16_t *C16=C;
451 #endif
452 C[0]=corr[2];
453 C[1]=corr[1];
454 C[2]=corr[0];
455 C[3]=A[1][2];
456 C[4]=A[0][1];
457 C[5]=A[0][2];
458 C[6]=A[2][2];
459 C[7]=A[1][1];
460 C[8]=A[0][0];
461
462 /*plc_tuning *= 2;*/
463 if (plc_tuning<2)
464 plc_tuning=2;
465 if (plc_tuning>30)
466 plc_tuning=30;
467 #ifdef FIXED_POINT
468 C[0] = SHL32(C[0],1);
469 C[1] = SHL32(C[1],1);
470 C[2] = SHL32(C[2],1);
471 C[3] = SHL32(C[3],1);
472 C[4] = SHL32(C[4],1);
473 C[5] = SHL32(C[5],1);
474 C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]);
475 C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]);
476 C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]);
477 normalize16(C, C16, 32767, 9);
478 #else
479 C[6]*=.5*(1+.02*plc_tuning);
480 C[7]*=.5*(1+.02*plc_tuning);
481 C[8]*=.5*(1+.02*plc_tuning);
482 #endif
483
484 best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);
485
486 #ifdef FIXED_POINT
487 gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]);
488 gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]);
489 gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]);
490 /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
491 #else
492 gain[0] = 0.015625*gain_cdbk[best_cdbk*4] + .5;
493 gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5;
494 gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5;
495 #endif
496 *cdbk_index=best_cdbk;
497 }
498
499 SPEEX_MEMSET(exc, 0, nsf);
500 for (i=0;i<3;i++)
501 {
502 int j;
503 int tmp1, tmp3;
504 int pp=pitch+1-i;
505 tmp1=nsf;
506 if (tmp1>pp)
507 tmp1=pp;
508 for (j=0;j<tmp1;j++)
509 exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]);
510 tmp3=nsf;
511 if (tmp3>pp+pitch)
512 tmp3=pp+pitch;
513 for (j=tmp1;j<tmp3;j++)
514 exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]);
515 }
516 for (i=0;i<nsf;i++)
517 {
518 spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])),
519 MULT16_16(gain[2],x[0][i]));
520 new_target[i] = SUB16(new_target[i], EXTRACT16(PSHR32(tmp,6)));
521 }
522 err = inner_prod(new_target, new_target, nsf);
523
524 return err;
525 }
526
527 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
pitch_search_3tap(spx_word16_t target[],spx_word16_t * sw,spx_coef_t ak[],spx_coef_t awk1[],spx_coef_t awk2[],spx_sig_t exc[],const void * par,int start,int end,spx_word16_t pitch_coef,int p,int nsf,SpeexBits * bits,char * stack,spx_word16_t * exc2,spx_word16_t * r,int complexity,int cdbk_offset,int plc_tuning,spx_word32_t * cumul_gain)528 int pitch_search_3tap(
529 spx_word16_t target[], /* Target vector */
530 spx_word16_t *sw,
531 spx_coef_t ak[], /* LPCs for this subframe */
532 spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
533 spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
534 spx_sig_t exc[], /* Excitation */
535 const void *par,
536 int start, /* Smallest pitch value allowed */
537 int end, /* Largest pitch value allowed */
538 spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
539 int p, /* Number of LPC coeffs */
540 int nsf, /* Number of samples in subframe */
541 SpeexBits *bits,
542 char *stack,
543 spx_word16_t *exc2,
544 spx_word16_t *r,
545 int complexity,
546 int cdbk_offset,
547 int plc_tuning,
548 spx_word32_t *cumul_gain
549 )
550 {
551 int i;
552 int cdbk_index, pitch=0, best_gain_index=0;
553 VARDECL(spx_sig_t *best_exc);
554 VARDECL(spx_word16_t *new_target);
555 VARDECL(spx_word16_t *best_target);
556 int best_pitch=0;
557 spx_word32_t err, best_err=-1;
558 int N;
559 const ltp_params *params;
560 const signed char *gain_cdbk;
561 int gain_cdbk_size;
562 int scaledown=0;
563
564 VARDECL(int *nbest);
565
566 params = (const ltp_params*) par;
567 gain_cdbk_size = 1<<params->gain_bits;
568 gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
569
570 N=complexity;
571 if (N>10)
572 N=10;
573 if (N<1)
574 N=1;
575
576 ALLOC(nbest, N, int);
577 params = (const ltp_params*) par;
578
579 if (end<start)
580 {
581 speex_bits_pack(bits, 0, params->pitch_bits);
582 speex_bits_pack(bits, 0, params->gain_bits);
583 SPEEX_MEMSET(exc, 0, nsf);
584 return start;
585 }
586
587 #ifdef FIXED_POINT
588 /* Check if we need to scale everything down in the pitch search to avoid overflows */
589 for (i=0;i<nsf;i++)
590 {
591 if (ABS16(target[i])>16383)
592 {
593 scaledown=1;
594 break;
595 }
596 }
597 for (i=-end;i<nsf;i++)
598 {
599 if (ABS16(exc2[i])>16383)
600 {
601 scaledown=1;
602 break;
603 }
604 }
605 #endif
606 if (N>end-start+1)
607 N=end-start+1;
608 if (end != start)
609 open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack);
610 else
611 nbest[0] = start;
612
613 ALLOC(best_exc, nsf, spx_sig_t);
614 ALLOC(new_target, nsf, spx_word16_t);
615 ALLOC(best_target, nsf, spx_word16_t);
616
617 for (i=0;i<N;i++)
618 {
619 pitch=nbest[i];
620 SPEEX_MEMSET(exc, 0, nsf);
621 err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf,
622 bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain, scaledown);
623 if (err<best_err || best_err<0)
624 {
625 SPEEX_COPY(best_exc, exc, nsf);
626 SPEEX_COPY(best_target, new_target, nsf);
627 best_err=err;
628 best_pitch=pitch;
629 best_gain_index=cdbk_index;
630 }
631 }
632 /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
633 speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
634 speex_bits_pack(bits, best_gain_index, params->gain_bits);
635 #ifdef FIXED_POINT
636 *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain));
637 #else
638 *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3];
639 #endif
640 /*printf ("%f\n", cumul_gain);*/
641 /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
642 SPEEX_COPY(exc, best_exc, nsf);
643 SPEEX_COPY(target, best_target, nsf);
644 #ifdef FIXED_POINT
645 /* Scale target back up if needed */
646 if (scaledown)
647 {
648 for (i=0;i<nsf;i++)
649 target[i]=SHL16(target[i],1);
650 }
651 #endif
652 return pitch;
653 }
654
pitch_unquant_3tap(spx_word16_t exc[],spx_word32_t exc_out[],int start,int end,spx_word16_t pitch_coef,const void * par,int nsf,int * pitch_val,spx_word16_t * gain_val,SpeexBits * bits,char * stack,int count_lost,int subframe_offset,spx_word16_t last_pitch_gain,int cdbk_offset)655 void pitch_unquant_3tap(
656 spx_word16_t exc[], /* Input excitation */
657 spx_word32_t exc_out[], /* Output excitation */
658 int start, /* Smallest pitch value allowed */
659 int end, /* Largest pitch value allowed */
660 spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
661 const void *par,
662 int nsf, /* Number of samples in subframe */
663 int *pitch_val,
664 spx_word16_t *gain_val,
665 SpeexBits *bits,
666 char *stack,
667 int count_lost,
668 int subframe_offset,
669 spx_word16_t last_pitch_gain,
670 int cdbk_offset
671 )
672 {
673 int i;
674 int pitch;
675 int gain_index;
676 spx_word16_t gain[3];
677 const signed char *gain_cdbk;
678 int gain_cdbk_size;
679 const ltp_params *params;
680
681 params = (const ltp_params*) par;
682 gain_cdbk_size = 1<<params->gain_bits;
683 gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
684
685 pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
686 pitch += start;
687 gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
688 /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
689 #ifdef FIXED_POINT
690 gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]);
691 gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]);
692 gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]);
693 #else
694 gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5;
695 gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5;
696 gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5;
697 #endif
698
699 if (count_lost && pitch > subframe_offset)
700 {
701 spx_word16_t gain_sum;
702 if (1) {
703 #ifdef FIXED_POINT
704 spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1);
705 if (tmp>62)
706 tmp=62;
707 #else
708 spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain;
709 if (tmp>.95)
710 tmp=.95;
711 #endif
712 gain_sum = gain_3tap_to_1tap(gain);
713
714 if (gain_sum > tmp)
715 {
716 spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum);
717 for (i=0;i<3;i++)
718 gain[i]=MULT16_16_Q14(fact,gain[i]);
719 }
720
721 }
722
723 }
724
725 *pitch_val = pitch;
726 gain_val[0]=gain[0];
727 gain_val[1]=gain[1];
728 gain_val[2]=gain[2];
729 gain[0] = SHL16(gain[0],7);
730 gain[1] = SHL16(gain[1],7);
731 gain[2] = SHL16(gain[2],7);
732 SPEEX_MEMSET(exc_out, 0, nsf);
733 for (i=0;i<3;i++)
734 {
735 int j;
736 int tmp1, tmp3;
737 int pp=pitch+1-i;
738 tmp1=nsf;
739 if (tmp1>pp)
740 tmp1=pp;
741 for (j=0;j<tmp1;j++)
742 exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]);
743 tmp3=nsf;
744 if (tmp3>pp+pitch)
745 tmp3=pp+pitch;
746 for (j=tmp1;j<tmp3;j++)
747 exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]);
748 }
749 /*for (i=0;i<nsf;i++)
750 exc[i]=PSHR32(exc32[i],13);*/
751 }
752
753
754 /** Forced pitch delay and gain */
forced_pitch_quant(spx_word16_t target[],spx_word16_t * sw,spx_coef_t ak[],spx_coef_t awk1[],spx_coef_t awk2[],spx_sig_t exc[],const void * par,int start,int end,spx_word16_t pitch_coef,int p,int nsf,SpeexBits * bits,char * stack,spx_word16_t * exc2,spx_word16_t * r,int complexity,int cdbk_offset,int plc_tuning,spx_word32_t * cumul_gain)755 int forced_pitch_quant(
756 spx_word16_t target[], /* Target vector */
757 spx_word16_t *sw,
758 spx_coef_t ak[], /* LPCs for this subframe */
759 spx_coef_t awk1[], /* Weighted LPCs #1 for this subframe */
760 spx_coef_t awk2[], /* Weighted LPCs #2 for this subframe */
761 spx_sig_t exc[], /* Excitation */
762 const void *par,
763 int start, /* Smallest pitch value allowed */
764 int end, /* Largest pitch value allowed */
765 spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
766 int p, /* Number of LPC coeffs */
767 int nsf, /* Number of samples in subframe */
768 SpeexBits *bits,
769 char *stack,
770 spx_word16_t *exc2,
771 spx_word16_t *r,
772 int complexity,
773 int cdbk_offset,
774 int plc_tuning,
775 spx_word32_t *cumul_gain
776 )
777 {
778 int i;
779 VARDECL(spx_word16_t *res);
780 ALLOC(res, nsf, spx_word16_t);
781 #ifdef FIXED_POINT
782 if (pitch_coef>63)
783 pitch_coef=63;
784 #else
785 if (pitch_coef>.99)
786 pitch_coef=.99;
787 #endif
788 for (i=0;i<nsf&&i<start;i++)
789 {
790 exc[i]=MULT16_16(SHL16(pitch_coef, 7),exc2[i-start]);
791 }
792 for (;i<nsf;i++)
793 {
794 exc[i]=MULT16_32_Q15(SHL16(pitch_coef, 9),exc[i-start]);
795 }
796 for (i=0;i<nsf;i++)
797 res[i] = EXTRACT16(PSHR32(exc[i], SIG_SHIFT-1));
798 syn_percep_zero16(res, ak, awk1, awk2, res, nsf, p, stack);
799 for (i=0;i<nsf;i++)
800 target[i]=EXTRACT16(SATURATE(SUB32(EXTEND32(target[i]),EXTEND32(res[i])),32700));
801 return start;
802 }
803
804 /** Unquantize forced pitch delay and gain */
forced_pitch_unquant(spx_word16_t exc[],spx_word32_t exc_out[],int start,int end,spx_word16_t pitch_coef,const void * par,int nsf,int * pitch_val,spx_word16_t * gain_val,SpeexBits * bits,char * stack,int count_lost,int subframe_offset,spx_word16_t last_pitch_gain,int cdbk_offset)805 void forced_pitch_unquant(
806 spx_word16_t exc[], /* Input excitation */
807 spx_word32_t exc_out[], /* Output excitation */
808 int start, /* Smallest pitch value allowed */
809 int end, /* Largest pitch value allowed */
810 spx_word16_t pitch_coef, /* Voicing (pitch) coefficient */
811 const void *par,
812 int nsf, /* Number of samples in subframe */
813 int *pitch_val,
814 spx_word16_t *gain_val,
815 SpeexBits *bits,
816 char *stack,
817 int count_lost,
818 int subframe_offset,
819 spx_word16_t last_pitch_gain,
820 int cdbk_offset
821 )
822 {
823 int i;
824 #ifdef FIXED_POINT
825 if (pitch_coef>63)
826 pitch_coef=63;
827 #else
828 if (pitch_coef>.99)
829 pitch_coef=.99;
830 #endif
831 for (i=0;i<nsf;i++)
832 {
833 exc_out[i]=MULT16_16(exc[i-start],SHL16(pitch_coef,7));
834 exc[i] = EXTRACT16(PSHR32(exc_out[i],13));
835 }
836 *pitch_val = start;
837 gain_val[0]=gain_val[2]=0;
838 gain_val[1] = pitch_coef;
839 }
840