fixed-point version of the high-pass seems to work now.
[speexdsp.git] / libspeex / ltp.c
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
44 #ifndef NULL
45 #define NULL 0
46 #endif
47
48
49 #ifdef _USE_SSE
50 #include "ltp_sse.h"
51 #elif defined (ARM4_ASM) || defined(ARM5E_ASM)
52 #include "ltp_arm4.h"
53 #elif defined (BFIN_ASM)
54 #include "ltp_bfin.h"
55 #endif
56
57 #ifndef OVERRIDE_INNER_PROD
58 spx_word32_t inner_prod(const spx_word16_t *x, const spx_word16_t *y, int len)
59 {
60    spx_word32_t sum=0;
61    len >>= 2;
62    while(len--)
63    {
64       spx_word32_t part=0;
65       part = MAC16_16(part,*x++,*y++);
66       part = MAC16_16(part,*x++,*y++);
67       part = MAC16_16(part,*x++,*y++);
68       part = MAC16_16(part,*x++,*y++);
69       /* HINT: If you had a 40-bit accumulator, you could shift only at the end */
70       sum = ADD32(sum,SHR32(part,6));
71    }
72    return sum;
73 }
74 #endif
75
76 #ifndef OVERRIDE_PITCH_XCORR
77 #if 0 /* HINT: Enable this for machines with enough registers (i.e. not x86) */
78 void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
79 {
80    int i,j;
81    for (i=0;i<nb_pitch;i+=4)
82    {
83       /* Compute correlation*/
84       /*corr[nb_pitch-1-i]=inner_prod(x, _y+i, len);*/
85       spx_word32_t sum1=0;
86       spx_word32_t sum2=0;
87       spx_word32_t sum3=0;
88       spx_word32_t sum4=0;
89       const spx_word16_t *y = _y+i;
90       const spx_word16_t *x = _x;
91       spx_word16_t y0, y1, y2, y3;
92       /*y0=y[0];y1=y[1];y2=y[2];y3=y[3];*/
93       y0=*y++;
94       y1=*y++;
95       y2=*y++;
96       y3=*y++;
97       for (j=0;j<len;j+=4)
98       {
99          spx_word32_t part1;
100          spx_word32_t part2;
101          spx_word32_t part3;
102          spx_word32_t part4;
103          part1 = MULT16_16(*x,y0);
104          part2 = MULT16_16(*x,y1);
105          part3 = MULT16_16(*x,y2);
106          part4 = MULT16_16(*x,y3);
107          x++;
108          y0=*y++;
109          part1 = MAC16_16(part1,*x,y1);
110          part2 = MAC16_16(part2,*x,y2);
111          part3 = MAC16_16(part3,*x,y3);
112          part4 = MAC16_16(part4,*x,y0);
113          x++;
114          y1=*y++;
115          part1 = MAC16_16(part1,*x,y2);
116          part2 = MAC16_16(part2,*x,y3);
117          part3 = MAC16_16(part3,*x,y0);
118          part4 = MAC16_16(part4,*x,y1);
119          x++;
120          y2=*y++;
121          part1 = MAC16_16(part1,*x,y3);
122          part2 = MAC16_16(part2,*x,y0);
123          part3 = MAC16_16(part3,*x,y1);
124          part4 = MAC16_16(part4,*x,y2);
125          x++;
126          y3=*y++;
127          
128          sum1 = ADD32(sum1,SHR32(part1,6));
129          sum2 = ADD32(sum2,SHR32(part2,6));
130          sum3 = ADD32(sum3,SHR32(part3,6));
131          sum4 = ADD32(sum4,SHR32(part4,6));
132       }
133       corr[nb_pitch-1-i]=sum1;
134       corr[nb_pitch-2-i]=sum2;
135       corr[nb_pitch-3-i]=sum3;
136       corr[nb_pitch-4-i]=sum4;
137    }
138
139 }
140 #else
141 void pitch_xcorr(const spx_word16_t *_x, const spx_word16_t *_y, spx_word32_t *corr, int len, int nb_pitch, char *stack)
142 {
143    int i;
144    for (i=0;i<nb_pitch;i++)
145    {
146       /* Compute correlation*/
147       corr[nb_pitch-1-i]=inner_prod(_x, _y+i, len);
148    }
149
150 }
151 #endif
152 #endif
153
154 #ifndef OVERRIDE_COMPUTE_PITCH_ERROR
155 static inline spx_word32_t compute_pitch_error(spx_word16_t *C, spx_word16_t *g, spx_word16_t pitch_control)
156 {
157    spx_word32_t sum = 0;
158    sum = ADD32(sum,MULT16_16(MULT16_16_16(g[0],pitch_control),C[0]));
159    sum = ADD32(sum,MULT16_16(MULT16_16_16(g[1],pitch_control),C[1]));
160    sum = ADD32(sum,MULT16_16(MULT16_16_16(g[2],pitch_control),C[2]));
161    sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[1]),C[3]));
162    sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[1]),C[4]));
163    sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[0]),C[5]));
164    sum = SUB32(sum,MULT16_16(MULT16_16_16(g[0],g[0]),C[6]));
165    sum = SUB32(sum,MULT16_16(MULT16_16_16(g[1],g[1]),C[7]));
166    sum = SUB32(sum,MULT16_16(MULT16_16_16(g[2],g[2]),C[8]));
167    return sum;
168 }
169 #endif
170
171 #ifndef OVERRIDE_OPEN_LOOP_NBEST_PITCH
172 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)
173 {
174    int i,j,k;
175    VARDECL(spx_word32_t *best_score);
176    VARDECL(spx_word32_t *best_ener);
177    spx_word32_t e0;
178    VARDECL(spx_word32_t *corr);
179    VARDECL(spx_word32_t *energy);
180
181    ALLOC(best_score, N, spx_word32_t);
182    ALLOC(best_ener, N, spx_word32_t);
183    ALLOC(corr, end-start+1, spx_word32_t);
184    ALLOC(energy, end-start+2, spx_word32_t);
185
186    for (i=0;i<N;i++)
187    {
188         best_score[i]=-1;
189         best_ener[i]=0;
190         pitch[i]=start;
191    }
192
193    energy[0]=inner_prod(sw-start, sw-start, len);
194    e0=inner_prod(sw, sw, len);
195    for (i=start;i<end;i++)
196    {
197       /* Update energy for next pitch*/
198       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));
199       if (energy[i-start+1] < 0)
200          energy[i-start+1] = 0;
201    }
202
203    pitch_xcorr(sw, sw-end, corr, len, end-start+1, stack);
204
205    /* FIXME: Fixed-point and floating-point code should be merged */
206 #ifdef FIXED_POINT
207    {
208       VARDECL(spx_word16_t *corr16);
209       VARDECL(spx_word16_t *ener16);
210       ALLOC(corr16, end-start+1, spx_word16_t);
211       ALLOC(ener16, end-start+1, spx_word16_t);
212       /* Normalize to 180 so we can square it and it still fits in 16 bits */
213       normalize16(corr, corr16, 180, end-start+1);
214       normalize16(energy, ener16, 180, end-start+1);
215
216       for (i=start;i<=end;i++)
217       {
218          spx_word16_t tmp = MULT16_16_16(corr16[i-start],corr16[i-start]);
219          /* Instead of dividing the tmp by the energy, we multiply on the other side */
220          if (MULT16_16(tmp,best_ener[N-1])>MULT16_16(best_score[N-1],ADD16(1,ener16[i-start])))
221          {
222             /* We can safely put it last and then check */
223             best_score[N-1]=tmp;
224             best_ener[N-1]=ener16[i-start]+1;
225             pitch[N-1]=i;
226             /* Check if it comes in front of others */
227             for (j=0;j<N-1;j++)
228             {
229                if (MULT16_16(tmp,best_ener[j])>MULT16_16(best_score[j],ADD16(1,ener16[i-start])))
230                {
231                   for (k=N-1;k>j;k--)
232                   {
233                      best_score[k]=best_score[k-1];
234                      best_ener[k]=best_ener[k-1];
235                      pitch[k]=pitch[k-1];
236                   }
237                   best_score[j]=tmp;
238                   best_ener[j]=ener16[i-start]+1;
239                   pitch[j]=i;
240                   break;
241                }
242             }
243          }
244       }
245    }
246 #else
247    for (i=start;i<=end;i++)
248    {
249       float tmp = corr[i-start]*corr[i-start];
250       if (tmp*best_ener[N-1]>best_score[N-1]*(1+energy[i-start]))
251       {
252          for (j=0;j<N;j++)
253          {
254             if (tmp*best_ener[j]>best_score[j]*(1+energy[i-start]))
255             {
256                for (k=N-1;k>j;k--)
257                {
258                   best_score[k]=best_score[k-1];
259                   best_ener[k]=best_ener[k-1];
260                   pitch[k]=pitch[k-1];
261                }
262                best_score[j]=tmp;
263                best_ener[j]=energy[i-start]+1;
264                pitch[j]=i;
265                break;
266             }
267          }
268       }
269    }
270 #endif
271
272    /* Compute open-loop gain */
273    if (gain)
274    {
275        for (j=0;j<N;j++)
276        {
277           spx_word16_t g;
278           i=pitch[j];
279           g = DIV32(corr[i-start], 10+SHR32(MULT16_16(spx_sqrt(e0),spx_sqrt(energy[i-start])),6));
280           /* FIXME: g = max(g,corr/energy) */
281                    if (g<0)
282                    g = 0;
283              gain[j]=g;
284        }
285    }
286 }
287 #endif
288
289 #ifndef OVERRIDE_PITCH_GAIN_SEARCH_3TAP_VQ
290 static int pitch_gain_search_3tap_vq(
291   const signed char *gain_cdbk,
292   int                gain_cdbk_size,
293   spx_word16_t      *C16,
294   spx_word16_t       max_gain
295 )
296 {
297   const signed char *ptr=gain_cdbk;
298   int                best_cdbk=0;
299   spx_word32_t       best_sum=-VERY_LARGE32;
300   spx_word32_t       sum=0;
301   spx_word16_t       g[3];
302   spx_word16_t       pitch_control=64;
303   spx_word16_t       gain_sum;
304   int                i;
305
306   for (i=0;i<gain_cdbk_size;i++) {
307          
308     ptr = gain_cdbk+4*i;
309     g[0]=ADD16((spx_word16_t)ptr[0],32);
310     g[1]=ADD16((spx_word16_t)ptr[1],32);
311     g[2]=ADD16((spx_word16_t)ptr[2],32);
312     gain_sum = (spx_word16_t)ptr[3];
313          
314     sum = compute_pitch_error(C16, g, pitch_control);
315          
316     if (sum>best_sum && gain_sum<=max_gain) {
317       best_sum=sum;
318       best_cdbk=i;
319     }
320   }
321
322   return best_cdbk;
323 }
324 #endif
325
326 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
327 static spx_word32_t pitch_gain_search_3tap(
328 const spx_word16_t target[],       /* Target vector */
329 const spx_coef_t ak[],          /* LPCs for this subframe */
330 const spx_coef_t awk1[],        /* Weighted LPCs #1 for this subframe */
331 const spx_coef_t awk2[],        /* Weighted LPCs #2 for this subframe */
332 spx_sig_t exc[],                /* Excitation */
333 const signed char *gain_cdbk,
334 int gain_cdbk_size,
335 int   pitch,                    /* Pitch value */
336 int   p,                        /* Number of LPC coeffs */
337 int   nsf,                      /* Number of samples in subframe */
338 SpeexBits *bits,
339 char *stack,
340 const spx_word16_t *exc2,
341 const spx_word16_t *r,
342 spx_word16_t *new_target,
343 int  *cdbk_index,
344 int plc_tuning,
345 spx_word32_t cumul_gain
346 )
347 {
348    int i,j;
349    VARDECL(spx_word16_t *tmp1);
350    VARDECL(spx_word16_t *e);
351    spx_word16_t *x[3];
352    spx_word32_t corr[3];
353    spx_word32_t A[3][3];
354    spx_word16_t gain[3];
355    spx_word32_t err;
356    spx_word16_t max_gain=128;
357    int          best_cdbk=0;
358
359    ALLOC(tmp1, 3*nsf, spx_word16_t);
360    ALLOC(e, nsf, spx_word16_t);
361
362    if (cumul_gain > 262144)
363       max_gain = 31;
364    
365    x[0]=tmp1;
366    x[1]=tmp1+nsf;
367    x[2]=tmp1+2*nsf;
368    
369    {
370       VARDECL(spx_mem_t *mm);
371       int pp=pitch-1;
372       ALLOC(mm, p, spx_mem_t);
373       for (j=0;j<nsf;j++)
374       {
375          if (j-pp<0)
376             e[j]=exc2[j-pp];
377          else if (j-pp-pitch<0)
378             e[j]=exc2[j-pp-pitch];
379          else
380             e[j]=0;
381       }
382       for (j=0;j<p;j++)
383          mm[j] = 0;
384       iir_mem16(e, ak, e, nsf, p, mm, stack);
385       for (j=0;j<p;j++)
386          mm[j] = 0;
387       filter_mem16(e, awk1, awk2, e, nsf, p, mm, stack);
388       for (j=0;j<nsf;j++)
389          x[2][j] = e[j];
390    }
391    for (i=1;i>=0;i--)
392    {
393       spx_word16_t e0=exc2[-pitch-1+i];
394       x[i][0]=MULT16_16_Q14(r[0], e0);
395       for (j=0;j<nsf-1;j++)
396          x[i][j+1]=ADD32(x[i+1][j],MULT16_16_P14(r[j+1], e0));
397    }
398
399    for (i=0;i<3;i++)
400       corr[i]=inner_prod(x[i],target,nsf);
401    for (i=0;i<3;i++)
402       for (j=0;j<=i;j++)
403          A[i][j]=A[j][i]=inner_prod(x[i],x[j],nsf);
404
405    {
406       spx_word32_t C[9];
407 #ifdef FIXED_POINT
408       spx_word16_t C16[9];
409 #else
410       spx_word16_t *C16=C;
411 #endif      
412       C[0]=corr[2];
413       C[1]=corr[1];
414       C[2]=corr[0];
415       C[3]=A[1][2];
416       C[4]=A[0][1];
417       C[5]=A[0][2];      
418       C[6]=A[2][2];
419       C[7]=A[1][1];
420       C[8]=A[0][0];
421       
422       /*plc_tuning *= 2;*/
423       if (plc_tuning<2)
424          plc_tuning=2;
425       if (plc_tuning>30)
426          plc_tuning=30;
427 #ifdef FIXED_POINT
428       C[0] = SHL32(C[0],1);
429       C[1] = SHL32(C[1],1);
430       C[2] = SHL32(C[2],1);
431       C[3] = SHL32(C[3],1);
432       C[4] = SHL32(C[4],1);
433       C[5] = SHL32(C[5],1);
434       C[6] = MAC16_32_Q15(C[6],MULT16_16_16(plc_tuning,655),C[6]);
435       C[7] = MAC16_32_Q15(C[7],MULT16_16_16(plc_tuning,655),C[7]);
436       C[8] = MAC16_32_Q15(C[8],MULT16_16_16(plc_tuning,655),C[8]);
437       normalize16(C, C16, 32767, 9);
438 #else
439       C[6]*=.5*(1+.02*plc_tuning);
440       C[7]*=.5*(1+.02*plc_tuning);
441       C[8]*=.5*(1+.02*plc_tuning);
442 #endif
443
444       best_cdbk = pitch_gain_search_3tap_vq(gain_cdbk, gain_cdbk_size, C16, max_gain);
445
446 #ifdef FIXED_POINT
447       gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4]);
448       gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+1]);
449       gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[best_cdbk*4+2]);
450       /*printf ("%d %d %d %d\n",gain[0],gain[1],gain[2], best_cdbk);*/
451 #else
452       gain[0] = 0.015625*gain_cdbk[best_cdbk*4]  + .5;
453       gain[1] = 0.015625*gain_cdbk[best_cdbk*4+1]+ .5;
454       gain[2] = 0.015625*gain_cdbk[best_cdbk*4+2]+ .5;
455 #endif
456       *cdbk_index=best_cdbk;
457    }
458
459    for (i=0;i<nsf;i++)
460       exc[i]=0;
461    for (i=0;i<3;i++)
462    {
463       int j;
464       int tmp1, tmp3;
465       int pp=pitch+1-i;
466       tmp1=nsf;
467       if (tmp1>pp)
468          tmp1=pp;
469       for (j=0;j<tmp1;j++)
470          exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp]);
471       tmp3=nsf;
472       if (tmp3>pp+pitch)
473          tmp3=pp+pitch;
474       for (j=tmp1;j<tmp3;j++)
475          exc[j]=MAC16_16(exc[j],SHL16(gain[2-i],7),exc2[j-pp-pitch]);
476    }
477    for (i=0;i<nsf;i++)
478    {
479       spx_word32_t tmp = ADD32(ADD32(MULT16_16(gain[0],x[2][i]),MULT16_16(gain[1],x[1][i])),
480                             MULT16_16(gain[2],x[0][i]));
481       new_target[i] = SUB16(target[i], EXTRACT16(PSHR32(tmp,6)));
482    }
483    err = inner_prod(new_target, new_target, nsf);
484
485    return err;
486 }
487
488 /** Finds the best quantized 3-tap pitch predictor by analysis by synthesis */
489 int pitch_search_3tap(
490 spx_word16_t target[],                 /* Target vector */
491 spx_word16_t *sw,
492 spx_coef_t ak[],                     /* LPCs for this subframe */
493 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
494 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
495 spx_sig_t exc[],                    /* Excitation */
496 const void *par,
497 int   start,                    /* Smallest pitch value allowed */
498 int   end,                      /* Largest pitch value allowed */
499 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
500 int   p,                        /* Number of LPC coeffs */
501 int   nsf,                      /* Number of samples in subframe */
502 SpeexBits *bits,
503 char *stack,
504 spx_word16_t *exc2,
505 spx_word16_t *r,
506 int complexity,
507 int cdbk_offset,
508 int plc_tuning,
509 spx_word32_t *cumul_gain
510 )
511 {
512    int i,j;
513    int cdbk_index, pitch=0, best_gain_index=0;
514    VARDECL(spx_sig_t *best_exc);
515    VARDECL(spx_word16_t *new_target);
516    VARDECL(spx_word16_t *best_target);
517    int best_pitch=0;
518    spx_word32_t err, best_err=-1;
519    int N;
520    const ltp_params *params;
521    const signed char *gain_cdbk;
522    int   gain_cdbk_size;
523    
524    VARDECL(int *nbest);
525    
526    params = (const ltp_params*) par;
527    gain_cdbk_size = 1<<params->gain_bits;
528    gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
529    
530    N=complexity;
531    if (N>10)
532       N=10;
533    if (N<1)
534       N=1;
535
536    ALLOC(nbest, N, int);
537    params = (const ltp_params*) par;
538
539    if (end<start)
540    {
541       speex_bits_pack(bits, 0, params->pitch_bits);
542       speex_bits_pack(bits, 0, params->gain_bits);
543       for (i=0;i<nsf;i++)
544          exc[i]=0;
545       return start;
546    }
547    
548    if (N>end-start+1)
549       N=end-start+1;
550    if (end != start)
551       open_loop_nbest_pitch(sw, start, end, nsf, nbest, NULL, N, stack);
552    else
553       nbest[0] = start;
554    
555    ALLOC(best_exc, nsf, spx_sig_t);
556    ALLOC(new_target, nsf, spx_word16_t);
557    ALLOC(best_target, nsf, spx_word16_t);
558    
559    for (i=0;i<N;i++)
560    {
561       pitch=nbest[i];
562       for (j=0;j<nsf;j++)
563          exc[j]=0;
564       err=pitch_gain_search_3tap(target, ak, awk1, awk2, exc, gain_cdbk, gain_cdbk_size, pitch, p, nsf,
565                                  bits, stack, exc2, r, new_target, &cdbk_index, plc_tuning, *cumul_gain);
566       if (err<best_err || best_err<0)
567       {
568          for (j=0;j<nsf;j++)
569             best_exc[j]=exc[j];
570          for (j=0;j<nsf;j++)
571             best_target[j]=new_target[j];
572          best_err=err;
573          best_pitch=pitch;
574          best_gain_index=cdbk_index;
575       }
576    }
577    /*printf ("pitch: %d %d\n", best_pitch, best_gain_index);*/
578    speex_bits_pack(bits, best_pitch-start, params->pitch_bits);
579    speex_bits_pack(bits, best_gain_index, params->gain_bits);
580 #ifdef FIXED_POINT
581    *cumul_gain = MULT16_32_Q13(SHL16(params->gain_cdbk[4*best_gain_index+3],8), MAX32(1024,*cumul_gain));
582 #else
583    *cumul_gain = 0.03125*MAX32(1024,*cumul_gain)*params->gain_cdbk[4*best_gain_index+3];
584 #endif
585    /*printf ("%f\n", cumul_gain);*/
586    /*printf ("encode pitch: %d %d\n", best_pitch, best_gain_index);*/
587    for (i=0;i<nsf;i++)
588       exc[i]=best_exc[i];
589    for (i=0;i<nsf;i++)
590       target[i]=best_target[i];
591
592    return pitch;
593 }
594
595 void pitch_unquant_3tap(
596 spx_word16_t exc[],             /* Input excitation */
597 spx_word32_t exc_out[],         /* Output excitation */
598 int   start,                    /* Smallest pitch value allowed */
599 int   end,                      /* Largest pitch value allowed */
600 spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */
601 const void *par,
602 int   nsf,                      /* Number of samples in subframe */
603 int *pitch_val,
604 spx_word16_t *gain_val,
605 SpeexBits *bits,
606 char *stack,
607 int count_lost,
608 int subframe_offset,
609 spx_word16_t last_pitch_gain,
610 int cdbk_offset
611 )
612 {
613    int i;
614    int pitch;
615    int gain_index;
616    spx_word16_t gain[3];
617    const signed char *gain_cdbk;
618    int gain_cdbk_size;
619    const ltp_params *params;
620
621    params = (const ltp_params*) par;
622    gain_cdbk_size = 1<<params->gain_bits;
623    gain_cdbk = params->gain_cdbk + 4*gain_cdbk_size*cdbk_offset;
624
625    pitch = speex_bits_unpack_unsigned(bits, params->pitch_bits);
626    pitch += start;
627    gain_index = speex_bits_unpack_unsigned(bits, params->gain_bits);
628    /*printf ("decode pitch: %d %d\n", pitch, gain_index);*/
629 #ifdef FIXED_POINT
630    gain[0] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4]);
631    gain[1] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+1]);
632    gain[2] = ADD16(32,(spx_word16_t)gain_cdbk[gain_index*4+2]);
633 #else
634    gain[0] = 0.015625*gain_cdbk[gain_index*4]+.5;
635    gain[1] = 0.015625*gain_cdbk[gain_index*4+1]+.5;
636    gain[2] = 0.015625*gain_cdbk[gain_index*4+2]+.5;
637 #endif
638
639    if (count_lost && pitch > subframe_offset)
640    {
641       spx_word16_t gain_sum;
642       if (1) {
643 #ifdef FIXED_POINT
644          spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : SHR16(last_pitch_gain,1);
645          if (tmp>62)
646             tmp=62;
647 #else
648          spx_word16_t tmp = count_lost < 4 ? last_pitch_gain : 0.5 * last_pitch_gain;
649          if (tmp>.95)
650             tmp=.95;
651 #endif
652          gain_sum = gain_3tap_to_1tap(gain);
653
654          if (gain_sum > tmp)
655          {
656             spx_word16_t fact = DIV32_16(SHL32(EXTEND32(tmp),14),gain_sum);
657             for (i=0;i<3;i++)
658                gain[i]=MULT16_16_Q14(fact,gain[i]);
659          }
660
661       }
662
663    }
664
665    *pitch_val = pitch;
666    gain_val[0]=gain[0];
667    gain_val[1]=gain[1];
668    gain_val[2]=gain[2];
669    gain[0] = SHL16(gain[0],7);
670    gain[1] = SHL16(gain[1],7);
671    gain[2] = SHL16(gain[2],7);
672    for (i=0;i<nsf;i++)
673       exc_out[i]=0;
674    for (i=0;i<3;i++)
675    {
676       int j;
677       int tmp1, tmp3;
678       int pp=pitch+1-i;
679       tmp1=nsf;
680       if (tmp1>pp)
681          tmp1=pp;
682       for (j=0;j<tmp1;j++)
683          exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp]);
684       tmp3=nsf;
685       if (tmp3>pp+pitch)
686          tmp3=pp+pitch;
687       for (j=tmp1;j<tmp3;j++)
688          exc_out[j]=MAC16_16(exc_out[j],gain[2-i],exc[j-pp-pitch]);
689    }
690    /*for (i=0;i<nsf;i++)
691    exc[i]=PSHR32(exc32[i],13);*/
692 }
693
694
695 /** Forced pitch delay and gain */
696 int forced_pitch_quant(
697 spx_word16_t target[],                 /* Target vector */
698 spx_word16_t *sw,
699 spx_coef_t ak[],                     /* LPCs for this subframe */
700 spx_coef_t awk1[],                   /* Weighted LPCs #1 for this subframe */
701 spx_coef_t awk2[],                   /* Weighted LPCs #2 for this subframe */
702 spx_sig_t exc[],                    /* Excitation */
703 const void *par,
704 int   start,                    /* Smallest pitch value allowed */
705 int   end,                      /* Largest pitch value allowed */
706 spx_word16_t pitch_coef,               /* Voicing (pitch) coefficient */
707 int   p,                        /* Number of LPC coeffs */
708 int   nsf,                      /* Number of samples in subframe */
709 SpeexBits *bits,
710 char *stack,
711 spx_word16_t *exc2,
712 spx_word16_t *r,
713 int complexity,
714 int cdbk_offset,
715 int plc_tuning,
716 spx_word32_t *cumul_gain
717 )
718 {
719    int i;
720    VARDECL(spx_sig_t *res);
721    ALLOC(res, nsf, spx_sig_t);
722 #ifdef FIXED_POINT
723    if (pitch_coef>63)
724       pitch_coef=63;
725 #else
726    if (pitch_coef>.99)
727       pitch_coef=.99;
728 #endif
729    for (i=0;i<nsf&&i<start;i++)
730    {
731       exc[i]=MULT16_16(SHL16(pitch_coef, 7),exc2[i-start]);
732    }
733    for (;i<nsf;i++)
734    {
735       exc[i]=MULT16_32_Q15(SHL16(pitch_coef, 9),exc[i-start]);
736    }
737    syn_percep_zero(exc, ak, awk1, awk2, res, nsf, p, stack);
738    for (i=0;i<nsf;i++)
739       target[i]=EXTRACT16(SATURATE(SUB32(EXTEND32(target[i]),PSHR32(res[i],SIG_SHIFT-1)),32700));
740    return start;
741 }
742
743 /** Unquantize forced pitch delay and gain */
744 void forced_pitch_unquant(
745 spx_word16_t exc[],             /* Input excitation */
746 spx_word32_t exc_out[],         /* Output excitation */
747 int   start,                    /* Smallest pitch value allowed */
748 int   end,                      /* Largest pitch value allowed */
749 spx_word16_t pitch_coef,        /* Voicing (pitch) coefficient */
750 const void *par,
751 int   nsf,                      /* Number of samples in subframe */
752 int *pitch_val,
753 spx_word16_t *gain_val,
754 SpeexBits *bits,
755 char *stack,
756 int count_lost,
757 int subframe_offset,
758 spx_word16_t last_pitch_gain,
759 int cdbk_offset
760 )
761 {
762    int i;
763 #ifdef FIXED_POINT
764    if (pitch_coef>63)
765       pitch_coef=63;
766 #else
767    if (pitch_coef>.99)
768       pitch_coef=.99;
769 #endif
770    for (i=0;i<nsf;i++)
771    {
772       exc_out[i]=MULT16_16(exc[i-start],SHL16(pitch_coef,7));
773       exc[i] = PSHR(exc_out[i],13);
774    }
775    *pitch_val = start;
776    gain_val[0]=gain_val[2]=0;
777    gain_val[1] = pitch_coef;
778 }