1 /*
2  *===================================================================
3  *  3GPP AMR Wideband Floating-point Speech Codec
4  *===================================================================
5  */
6 #include <memory.h>
7 #include "typedef.h"
8 #include "dec_util.h"
9 
10 #define L_SUBFR         64       /* Subframe size */
11 #define L_LTPHIST       5
12 #define ONE_PER_3       10923
13 #define ONE_PER_LTPHIST 6554
14 #define UP_SAMP         4
15 #define L_INTERPOL2     16
16 
17 extern const Word16 D_ROM_inter4_2[];
18 extern const Word16 D_ROM_pdown_unusable[];
19 extern const Word16 D_ROM_pdown_usable[];
20 extern const Word16 D_ROM_cdown_unusable[];
21 extern const Word16 D_ROM_cdown_usable[];
22 extern const Word16 D_ROM_qua_gain6b[];
23 extern const Word16 D_ROM_qua_gain7b[];
24 
25 /*
26  * D_GAIN_init
27  *
28  * Parameters:
29  *    mem         O: static memory
30  *
31  * Function:
32  *    Initialisation of 2nd order quantiser energy predictor.
33  *
34  * Returns:
35  *    void
36  */
D_GAIN_init(Word16 * mem)37 void D_GAIN_init(Word16 *mem)
38 {
39 
40    /* 4nd order quantizer energy predictor (init to -14.0 in Q10) */
41    mem[0] = -14336;   /* past_qua_en[0] */
42    mem[1] = -14336;   /* past_qua_en[1] */
43    mem[2] = -14336;   /* past_qua_en[2] */
44    mem[3] = -14336;   /* past_qua_en[3] */
45    /*
46     * mem[4] = 0;       past_gain_pit
47     * mem[5] = 0;       past_gain_code
48     * mem[6] = 0;       prev_gc
49     * mem[7 - 11] = 0;  pbuf[i]
50     * mem[12 - 16] = 0; gbuf[i]
51     * mem[17 - 21] = 0; pbuf2[i]
52     */
53    memset(&mem[4], 0, 18 * sizeof(Word16));
54 
55    mem[22] = 21845;   /* seed */
56    return;
57 }
58 
59 
60 /*
61  * D_GAIN_median
62  *
63  * Parameters:
64  *    buf            I: previous gains
65  *
66  * Function:
67  *    Median of gains
68  *
69  * Returns:
70  *    median of 5 previous gains
71  */
D_GAIN_median(Word16 x[])72 static Word16 D_GAIN_median(Word16 x[])
73 {
74    Word16 x1, x2, x3, x4, x5;
75    Word16 tmp;
76    x1 = x[ - 2];
77    x2 = x[ - 1];
78    x3 = x[0];
79    x4 = x[1];
80    x5 = x[2];
81 
82    if(x2 < x1)
83    {
84       tmp = x1;
85       x1 = x2;
86       x2 = tmp;
87    }
88 
89    if(x3 < x1)
90    {
91       tmp = x1;
92       x1 = x3;
93       x3 = tmp;
94    }
95 
96    if(x4 < x1)
97    {
98       tmp = x1;
99       x1 = x4;
100       x4 = tmp;
101    }
102 
103    if(x5 < x1)
104    {
105       x5 = x1;
106    }
107 
108    if(x3 < x2)
109    {
110       tmp = x2;
111       x2 = x3;
112       x3 = tmp;
113    }
114 
115    if(x4 < x2)
116    {
117       tmp = x2;
118       x2 = x4;
119       x4 = tmp;
120    }
121 
122    if(x5 < x2)
123    {
124       x5 = x2;
125    }
126 
127    if(x4 < x3)
128    {
129       x3 = x4;
130    }
131 
132    if(x5 < x3)
133    {
134       x3 = x5;
135    }
136 
137    return(x3);
138 }
139 
140 
141 /*
142  * D_GAIN_decode
143  *
144  * Parameters:
145  *    index             I: Quantization index
146  *    nbits             I: number of bits (6 or 7)
147  *    code              I: Innovative code vector
148  *    L_subfr           I: Subframe size
149  *    gain_pit          O: (Q14) Quantized pitch gain
150  *    gain_code         O: (Q16) Quantized codebook gain
151  *    bfi               I: Bad frame indicator
152  *    prev_bfi          I: Previous BF indicator
153  *    state             I: State of BFH
154  *    unusable_frame    I: UF indicator
155  *    vad_hist          I: number of non-speech frames
156  *    mem             I/O: static memory (4 words)
157  *
158  *
159  * Function:
160  *    Decoding of pitch and codebook gains
161  *
162  * Returns:
163  *    void
164  */
D_GAIN_decode(Word16 index,Word16 nbits,Word16 code[],Word16 * gain_pit,Word32 * gain_cod,Word16 bfi,Word16 prev_bfi,Word16 state,Word16 unusable_frame,Word16 vad_hist,Word16 * mem)165 void D_GAIN_decode(Word16 index, Word16 nbits, Word16 code[], Word16 *gain_pit,
166                    Word32 *gain_cod, Word16 bfi, Word16 prev_bfi,
167                    Word16 state, Word16 unusable_frame, Word16 vad_hist,
168                    Word16 *mem)
169 {
170 
171    Word32 gcode0, qua_ener, L_tmp;
172    const Word16 * p;
173    Word16 *past_gain_pit, *past_gain_code, *past_qua_en, *prev_gc;
174    Word16 *gbuf, *pbuf, *pbuf2;
175    Word16 i, tmp, exp, frac, exp_gcode0, gcode_inov;
176    Word16 g_code;
177 
178    past_qua_en = mem;
179    past_gain_pit = mem + 4;
180    past_gain_code = mem + 5;
181    prev_gc = mem + 6;
182    pbuf = mem + 7;
183    gbuf = mem + 12;
184    pbuf2 = mem + 17;
185 
186    /*
187     * Find energy of code and compute:
188     *
189     *    L_tmp = 1.0 / sqrt(energy of code/ L_subfr)
190     */
191    L_tmp = D_UTIL_dot_product12(code, code, L_SUBFR, &exp);
192    exp = (Word16)(exp - (18 + 6));   /* exp: -18 (code in Q9), -6 (/L_subfr) */
193    D_UTIL_normalised_inverse_sqrt(&L_tmp, &exp);
194 
195    if(exp > 3)
196    {
197       L_tmp <<= (exp - 3);
198    }
199    else
200    {
201       L_tmp >>= (3 - exp);
202    }
203 
204    gcode_inov = (Word16)(L_tmp >>16);   /* g_code_inov in Q12 */
205 
206    /*
207     * Case of erasure.
208     */
209    if(bfi != 0)
210    {
211       tmp = D_GAIN_median(&pbuf[2]);
212       *past_gain_pit = tmp;
213 
214       if(*past_gain_pit > 15565)
215       {
216          *past_gain_pit = 15565;   /* 0.95 in Q14 */
217       }
218 
219       if(unusable_frame != 0)
220       {
221          *gain_pit =
222             (Word16)((D_ROM_pdown_unusable[state] * *past_gain_pit) >> 15);
223       }
224       else
225       {
226          *gain_pit =
227             (Word16)((D_ROM_pdown_usable[state] * *past_gain_pit) >> 15);
228       }
229 
230       tmp = D_GAIN_median(&gbuf[2]);
231 
232       if(vad_hist > 2)
233       {
234          *past_gain_code = tmp;
235       }
236       else
237       {
238          if(unusable_frame != 0)
239          {
240             *past_gain_code =
241                (Word16)((D_ROM_cdown_unusable[state] * tmp) >> 15);
242          }
243          else
244          {
245             *past_gain_code =
246                (Word16)((D_ROM_cdown_usable[state] * tmp) >> 15);
247          }
248       }
249 
250       /* update table of past quantized energies */
251       L_tmp = past_qua_en[0] + past_qua_en[1]+ past_qua_en[2] + past_qua_en[3];
252       qua_ener = L_tmp >> 2;
253       qua_ener = qua_ener - 3072;   /* -3 in Q10 */
254 
255       if(qua_ener < - 14336)
256       {
257          qua_ener = -14336;   /* -14 in Q10 */
258       }
259 
260       past_qua_en[3] = past_qua_en[2];
261       past_qua_en[2] = past_qua_en[1];
262       past_qua_en[1] = past_qua_en[0];
263       past_qua_en[0] = (Word16)qua_ener;
264 
265       for(i = 1; i < 5; i++)
266       {
267          gbuf[i - 1] = gbuf[i];
268       }
269       gbuf[4] = *past_gain_code;
270 
271       for(i = 1; i < 5; i++)
272       {
273          pbuf[i - 1] = pbuf[i];
274       }
275       pbuf[4] = *past_gain_pit;
276 
277       /* adjust gain according to energy of code */
278       /* past_gain_code(Q3) * gcode_inov(Q12) => Q16 */
279       *gain_cod = (*past_gain_code * gcode_inov) << 1;
280 
281       return;
282    }
283 
284    /*
285     * Compute gcode0.
286     *  = Sum(i=0,1) pred[i]*past_qua_en[i] + mean_ener - ener_code
287     */
288 
289    /* MEAN_ENER in Q24 = 0x1e000000 */
290    /* MA prediction coeff = {0.5, 0.4, 0.3, 0.2} in Q13 */
291    L_tmp = 0xF000000 + (4096 * past_qua_en[0]); /* Q13*Q10 -> Q24 */
292    L_tmp = L_tmp + (3277 * past_qua_en[1]);     /* Q13*Q10 -> Q24 */
293    L_tmp = L_tmp + (2458 * past_qua_en[2]);     /* Q13*Q10 -> Q24 */
294    L_tmp = L_tmp + (1638 * past_qua_en[3]);     /* Q13*Q10 -> Q24 */
295    gcode0 = L_tmp >> 15;               /* From Q24 to Q8 */
296 
297    /*
298     * gcode0 = pow(10.0, gcode0/20)
299     *        = pow(2, 3.321928*gcode0/20)
300     *        = pow(2, 0.166096*gcode0)
301     */
302    L_tmp = (gcode0 * 5443) >> 7;
303    /* *0.166096 in Q15 -> Q24, From Q24 to Q16 */
304    D_UTIL_l_extract(L_tmp, &exp_gcode0, &frac);
305    /* Extract exponant of gcode0  */
306    gcode0 = D_UTIL_pow2(14, frac); /* Put 14 as exponant so that */
307 
308    /*
309     * output of Pow2() will be:
310     * 16384 < Pow2() <= 32767
311     */
312    exp_gcode0 = (Word16)(exp_gcode0 - 14);
313 
314    /* Read the quantized gains */
315    if(nbits == 6)
316    {
317       p = &D_ROM_qua_gain6b[(index << 1)];
318    }
319    else
320    {
321       p = &D_ROM_qua_gain7b[(index << 1)];
322    }
323 
324    *gain_pit = *p++; /* selected pitch gain in Q14 */
325    g_code = *p++;    /* selected code gain in Q11  */
326    L_tmp = g_code * gcode0;
327    exp_gcode0 += 5;
328 
329    if(exp_gcode0 >= 0)
330    {
331       *gain_cod = L_tmp << exp_gcode0;    /* gain of code in Q16 */
332    }
333    else
334    {
335       *gain_cod = L_tmp >> -exp_gcode0;   /* gain of code in Q16 */
336    }
337 
338    if(prev_bfi == 1)
339    {
340       L_tmp = (*prev_gc * 5120) << 1;  /* prev_gc(Q3) * 1.25(Q12) = Q16 */
341 
342       /* if((*gain_cod > ((*prev_gc) * 1.25)) && (*gain_cod > 100.0)) */
343       if((*gain_cod > L_tmp) & (*gain_cod > 6553600))
344       {
345          *gain_cod = L_tmp;
346       }
347    }
348 
349    /* keep past gain code in Q3 for frame erasure (can saturate) */
350    L_tmp = (*gain_cod + 0x1000) >> 13;
351 
352    if(L_tmp < 32768)
353    {
354       *past_gain_code = (Word16)L_tmp;
355    }
356    else
357    {
358       *past_gain_code = 32767;
359    }
360 
361    *past_gain_pit = *gain_pit;
362    *prev_gc = *past_gain_code;
363 
364    for(i = 1; i < 5; i++)
365    {
366       gbuf[i - 1] = gbuf[i];
367    }
368    gbuf[4] = *past_gain_code;
369 
370    for(i = 1; i < 5; i++)
371    {
372       pbuf[i - 1] = pbuf[i];
373    }
374    pbuf[4] = *past_gain_pit;
375 
376    for(i = 1; i < 5; i++)
377    {
378       pbuf2[i - 1] = pbuf2[i];
379    }
380    pbuf2[4] = *past_gain_pit;
381 
382    /* adjust gain according to energy of code */
383    D_UTIL_l_extract(*gain_cod, &exp, &frac);
384    L_tmp = D_UTIL_mpy_32_16(exp, frac, gcode_inov);
385 
386    if(L_tmp < 0xFFFFFFF)
387    {
388       *gain_cod = (L_tmp << 3);   /* gcode_inov in Q12 */
389    }
390    else
391    {
392       *gain_cod = 0x7FFFFFFF;
393    }
394 
395    /*
396     * qua_ener = 20*log10(g_code)
397     *          = 6.0206*log2(g_code)
398     *          = 6.0206*(log2(g_codeQ11) - 11)
399     */
400    L_tmp = (Word32)(g_code);
401    D_UTIL_log2(L_tmp, &exp, &frac);
402    exp = (Word16)(exp - 11);
403    L_tmp = D_UTIL_mpy_32_16(exp, frac, 24660);   /* x 6.0206 in Q12 */
404    qua_ener = L_tmp >>3;   /* result in Q10 */
405 
406    /* update table of past quantized energies */
407    past_qua_en[3] = past_qua_en[2];
408    past_qua_en[2] = past_qua_en[1];
409    past_qua_en[1] = past_qua_en[0];
410    past_qua_en[0] = (Word16)qua_ener;
411 
412    return;
413 }
414 
415 
416 /*
417  * D_GAIN_adaptive_control
418  *
419  * Parameters:
420  *    sig_in            I: postfilter input signal
421  *    sig_out         I/O: postfilter output signal
422  *    l_trm             I: subframe size
423  *
424  * Function:
425  *    Adaptive gain control is used to compensate for
426  *    the gain difference between the non-emphasized excitation and
427  *    emphasized excitation.
428  *
429  * Returns:
430  *    void
431  */
D_GAIN_adaptive_control(Word16 * sig_in,Word16 * sig_out,Word16 l_trm)432 void D_GAIN_adaptive_control(Word16 *sig_in, Word16 *sig_out, Word16 l_trm)
433 {
434    Word32 s, temp, i, exp;
435    Word32 gain_in, gain_out, g0;
436 
437    /* calculate gain_out with exponent */
438    temp = sig_out[0] >> 2;
439    s = temp * temp;
440 
441    for(i = 1; i < l_trm; i++)
442    {
443       temp = sig_out[i] >> 2;
444       s += temp * temp;
445    }
446 
447    s <<= 1;
448 
449    if(s == 0)
450    {
451       return;
452    }
453    exp = (D_UTIL_norm_l(s) - 1);
454 
455    if(exp >= 0)
456    {
457       gain_out = ((s << exp) + 0x8000) >> 16;
458    }
459    else
460    {
461       gain_out = ((s >> -exp) + 0x8000) >> 16;
462    }
463 
464    /* calculate gain_in with exponent */
465    temp = sig_in[0] >> 2;
466    s = temp * temp;
467 
468    for(i = 1; i < l_trm; i++)
469    {
470       temp = sig_in[i] >> 2;
471       s += temp * temp;
472    }
473 
474    s <<= 1;
475 
476    if(s == 0)
477    {
478       g0 = 0;
479    }
480    else
481    {
482       i = D_UTIL_norm_l(s);
483       s = ((s << i) + 0x8000) >> 16;
484 
485       if((s < 32768) & (s > 0))
486       {
487          gain_in = s;
488       }
489       else
490       {
491          gain_in = 32767;
492       }
493       exp = exp - i;
494 
495       /*
496        * g0 = sqrt(gain_in/gain_out)
497        */
498       s = (gain_out << 15) / gain_in;
499       s = s << (7 - exp);   /* s = gain_out / gain_in */
500       s = D_UTIL_inverse_sqrt(s);
501       g0 = ((s << 9) + 0x8000) >> 16;
502    }
503 
504    /* sig_out(n) = gain(n) sig_out(n) */
505    for(i = 0; i < l_trm; i++)
506    {
507       s = (sig_out[i] * g0) >> 13;
508       sig_out[i] = D_UTIL_saturate(s);
509    }
510 
511    return;
512 }
513 
514 
515 /*
516  * D_GAIN_insert_lag
517  *
518  * Parameters:
519  *    array        I/O: pitch lag history
520  *    n              I: history size
521  *    x              I: lag value
522  *
523  * Function:
524  *    Insert lag into correct location
525  *
526  * Returns:
527  *    void
528  */
D_GAIN_insert_lag(Word16 array[],Word32 n,Word16 x)529 static void D_GAIN_insert_lag(Word16 array[], Word32 n, Word16 x)
530 {
531    Word32 i;
532 
533    for(i = n - 1; i >= 0; i--)
534    {
535       if(x < array[i])
536       {
537          array[i + 1] = array[i];
538       }
539       else
540       {
541          break;
542       }
543    }
544 
545    array[i + 1] = x;
546 }
547 
548 
549 /*
550  * D_GAIN_sort_lag
551  *
552  * Parameters:
553  *    array        I/O: pitch lag history
554  *    n              I: history size
555  *
556  * Function:
557  *    Sorting of the lag history
558  *
559  * Returns:
560  *    void
561  */
D_GAIN_sort_lag(Word16 array[],Word16 n)562 static void D_GAIN_sort_lag(Word16 array[], Word16 n)
563 {
564    Word32 i;
565 
566    for(i = 0; i < n; i++)
567    {
568       D_GAIN_insert_lag(array, i, array[i]);
569    }
570 }
571 
572 
573 /*
574  * D_GAIN_lag_concealment_init
575  *
576  * Parameters:
577  *    lag_hist       O: pitch lag history
578  *
579  * Function:
580  *    Initialise lag history to 64
581  *
582  * Returns:
583  *    void
584  */
D_GAIN_lag_concealment_init(Word16 lag_hist[])585 void D_GAIN_lag_concealment_init(Word16 lag_hist[])
586 {
587    Word32 i;
588 
589    for(i = 0; i < L_LTPHIST; i++)
590    {
591       lag_hist[i] = 64;
592    }
593 }
594 
595 
596 /*
597  * D_GAIN_lag_concealment
598  *
599  * Parameters:
600  *    gain_hist         I: gain history
601  *    lag_hist          I: pitch lag history
602  *    T0                O: current lag
603  *    old_T0            I: previous lag
604  *    seed            I/O: seed for random
605  *    unusable_frame    I: lost frame
606  *
607  * Function:
608  *    Concealment of LTP lags during bad frames
609  *
610  * Returns:
611  *    void
612  */
D_GAIN_lag_concealment(Word16 gain_hist[],Word16 lag_hist[],Word32 * T0,Word16 * old_T0,Word16 * seed,Word16 unusable_frame)613 void D_GAIN_lag_concealment(Word16 gain_hist[], Word16 lag_hist[],
614                             Word32 *T0, Word16 *old_T0, Word16 *seed,
615                             Word16 unusable_frame)
616 {
617    Word32 i, lagDif, tmp, tmp2, D2, meanLag = 0;
618    Word16 lag_hist2[L_LTPHIST] = {0};
619    Word16 maxLag, minLag, lastLag;
620    Word16 minGain, lastGain, secLastGain;
621    Word16 D;
622 
623    /*
624     * Is lag index such that it can be aplied directly
625     * or does it has to be subtituted
626     */
627    lastGain = gain_hist[4];
628    secLastGain = gain_hist[3];
629    lastLag = lag_hist[0];
630 
631    /* SMALLEST history lag */
632    minLag = lag_hist[0];
633 
634    for(i = 1; i < L_LTPHIST; i++)
635    {
636       if(lag_hist[i] < minLag)
637       {
638          minLag = lag_hist[i];
639       }
640    }
641 
642    /* BIGGEST history lag */
643    maxLag = lag_hist[0];
644 
645    for(i = 1; i < L_LTPHIST; i++)
646    {
647       if(lag_hist[i] > maxLag)
648       {
649          maxLag = lag_hist[i];
650       }
651    }
652 
653    /* SMALLEST history gain */
654    minGain = gain_hist[0];
655 
656    for(i = 1; i < L_LTPHIST; i++)
657    {
658       if(gain_hist[i] < minGain)
659       {
660          minGain = gain_hist[i];
661       }
662    }
663 
664    /* Difference between MAX and MIN lag */
665    lagDif = maxLag - minLag;
666 
667    if(unusable_frame != 0)
668    {
669       /*
670        * LTP-lag for RX_SPEECH_LOST
671        * Recognition of the LTP-history
672        */
673       if((minGain > 8192) & (lagDif < 10))
674       {
675          *T0 = *old_T0;
676       }
677       else if((lastGain > 8192) && (secLastGain > 8192))
678       {
679          *T0 = lag_hist[0];
680       }
681       else
682       {
683          /*
684           * SORT
685           * The sorting of the lag history
686           */
687          for(i = 0; i < L_LTPHIST; i++)
688          {
689             lag_hist2[i] = lag_hist[i];
690          }
691          D_GAIN_sort_lag(lag_hist2, 5);
692 
693          /*
694           * Lag is weighted towards bigger lags
695           * and random variation is added
696           */
697          lagDif = (lag_hist2[4] - lag_hist2[2]);
698 
699          if(lagDif > 40)
700          {
701             lagDif = 40;
702          }
703 
704          D = D_UTIL_random(seed);   /* D={-1, ...,1} */
705 
706          /* D2={-lagDif/2..lagDif/2} */
707          tmp = lagDif >> 1;
708          D2 = (tmp * D) >> 15;
709          tmp = (lag_hist2[2] + lag_hist2[3]) + lag_hist2[4];
710          *T0 = ((tmp * ONE_PER_3) >> 15) + D2;
711       }
712 
713       /* New lag is not allowed to be bigger or smaller than last lag values */
714       if(*T0 > maxLag)
715       {
716          *T0 = maxLag;
717       }
718 
719       if(*T0 < minLag)
720       {
721          *T0 = minLag;
722       }
723    }
724    else
725    {
726       /*
727        * LTP-lag for RX_BAD_FRAME
728        * MEAN lag
729        */
730       meanLag = 0;
731 
732       for(i = 0; i < L_LTPHIST; i++)
733       {
734          meanLag = meanLag + lag_hist[i];
735       }
736 
737       meanLag = (meanLag * ONE_PER_LTPHIST) >> 15;
738       tmp = *T0 - maxLag;
739       tmp2 = *T0 - lastLag;
740 
741       if((lagDif < 10) & (*T0 > (minLag - 5)) & (tmp < 5))
742       {
743          *T0 = *T0;
744       }
745       else if((lastGain > 8192) & (secLastGain > 8192) & ((tmp2 > - 10)
746          & (tmp2 < 10)))
747       {
748          *T0 = *T0;
749       }
750       else if((minGain < 6554) & (lastGain == minGain) & ((*T0 > minLag)
751          & (*T0 < maxLag)))
752       {
753          *T0 = *T0;
754       }
755       else if((lagDif < 70) & (*T0 > minLag) & (*T0 < maxLag))
756       {
757          *T0 = *T0;
758       }
759       else if((*T0 > meanLag) & (*T0 < maxLag))
760       {
761          *T0 = *T0;
762       }
763       else
764       {
765          if((minGain > 8192) & (lagDif < 10))
766          {
767             *T0 = lag_hist[0];
768          }
769          else if((lastGain > 8192) & (secLastGain > 8192))
770          {
771             *T0 = lag_hist[0];
772          }
773          else
774          {
775             /*
776              * SORT
777              * The sorting of the lag history
778              */
779             for(i = 0; i < L_LTPHIST; i++)
780             {
781                lag_hist2[i] = lag_hist[i];
782             }
783 
784             D_GAIN_sort_lag(lag_hist2, 5);
785 
786             /*
787              * Lag is weighted towards bigger lags
788              * and random variation is added
789              */
790             lagDif = lag_hist2[4] - lag_hist2[2];
791 
792             if(lagDif > 40)
793             {
794                lagDif = 40;
795             }
796 
797             D = D_UTIL_random(seed);   /* D={-1,.., 1} */
798 
799             /* D2={-lagDif/2..lagDif/2} */
800             tmp = lagDif >> 1;
801             D2 = (tmp * D) >> 15;
802             tmp = (lag_hist2[2] + lag_hist2[3]) + lag_hist2[4];
803             *T0 = ((tmp * ONE_PER_3) >> 15) + D2;
804          }
805 
806          /*
807           * New lag is not allowed to be bigger or
808           * smaller than last lag values
809           */
810          if(*T0 > maxLag)
811          {
812             *T0 = maxLag;
813          }
814 
815          if(*T0 < minLag)
816          {
817             *T0 = minLag;
818          }
819       }
820    }
821 }
822 
823 
824 /*
825  * D_GAIN_adaptive_codebook_excitation
826  *
827  * Parameters:
828  *    exc          I/O: excitation buffer
829  *    T0             I: integer pitch lag
830  *    frac           I: fraction of lag
831  *
832  * Function:
833  *    Compute the result of Word32 term prediction with fractional
834  *    interpolation of resolution 1/4.
835  *
836  * Returns:
837  *    interpolated signal (adaptive codebook excitation)
838  */
D_GAIN_adaptive_codebook_excitation(Word16 exc[],Word32 T0,Word32 frac)839 void D_GAIN_adaptive_codebook_excitation(Word16 exc[], Word32 T0, Word32 frac)
840 {
841    Word32 i, j, k, sum;
842    Word16 *x;
843 
844    x = &exc[ - T0];
845    frac = -(frac);
846 
847    if(frac < 0)
848    {
849       frac = (frac + UP_SAMP);
850       x--;
851    }
852    x = x - L_INTERPOL2 + 1;
853 
854    for(j = 0; j < L_SUBFR + 1; j++)
855    {
856       sum = 0L;
857 
858       for(i = 0, k = ((UP_SAMP - 1) - frac); i < 2 * L_INTERPOL2; i++,
859          k += UP_SAMP)
860       {
861          sum += x[i] * D_ROM_inter4_2[k];
862       }
863       sum = (sum + 0x2000) >> 14;
864 
865       exc[j] = D_UTIL_saturate(sum);
866 
867       x++;
868    }
869    return;
870 }
871 
872 
873 /*
874  * D_GAIN_pitch_sharpening
875  *
876  * Parameters:
877  *    x            I/O: impulse response (or algebraic code)
878  *    pit_lag        I: pitch lag
879  *    sharp          I: (Q15) pitch sharpening factor
880  *
881  * Function:
882  *    Performs Pitch sharpening routine for one subframe.
883  *
884  * Returns:
885  *    void
886  */
D_GAIN_pitch_sharpening(Word16 * x,Word32 pit_lag,Word16 sharp)887 void D_GAIN_pitch_sharpening(Word16 *x, Word32 pit_lag, Word16 sharp)
888 {
889    Word32 i;
890    Word32 tmp;
891 
892    for(i = pit_lag; i < L_SUBFR; i++)
893    {
894       tmp = x[i] << 15;
895       tmp += x[i - pit_lag] * sharp;
896       x[i] = (Word16)((tmp + 0x4000) >> 15);
897    }
898    return;
899 }
900 
901 
902 /*
903  * D_GAIN_find_voice_factor
904  *
905  * Parameters:
906  *    exc            I: pitch excitation
907  *    Q_exc          I: exc format
908  *    gain_pit       I: (Q14) gain of pitch
909  *    code           I: (Q9) fixed codebook excitation
910  *    gain_code      I: (Q0) gain of code
911  *    L_subfr        I: subframe length
912  *
913  * Function:
914  *    Find the voicing factor.
915  *
916  * Returns:
917  *    (Q15) 1=voice to -1=unvoiced
918  */
D_GAIN_find_voice_factor(Word16 exc[],Word16 Q_exc,Word16 gain_pit,Word16 code[],Word16 gain_code,Word16 L_subfr)919 Word16 D_GAIN_find_voice_factor(Word16 exc[], Word16 Q_exc,
920                                 Word16 gain_pit, Word16 code[],
921                                 Word16 gain_code, Word16 L_subfr)
922 {
923 
924    Word32 tmp, ener1, ener2, i;
925    Word16 exp, exp1, exp2;
926 
927    ener1 = (D_UTIL_dot_product12(exc, exc, L_subfr, &exp1)) >> 16;
928    exp1 = (Word16)(exp1 - (Q_exc + Q_exc));
929    tmp = (gain_pit * gain_pit) << 1;
930    exp = D_UTIL_norm_l(tmp);
931    tmp = (tmp << exp) >> 16;
932    ener1 = (ener1 * tmp) >> 15;
933    exp1 = (Word16)((exp1 - exp) - 10);   /* 10 -> gain_pit Q14 to Q9 */
934    ener2 = D_UTIL_dot_product12(code, code, L_subfr, &exp2) >> 16;
935    exp = D_UTIL_norm_s(gain_code);
936    tmp = gain_code << exp;
937    tmp = (tmp * tmp) >> 15;
938    ener2 = (ener2 * tmp) >> 15;
939    exp2 = (Word16)(exp2 - (exp << 1));
940    i = exp1 - exp2;
941 
942    if(i >= 0)
943    {
944       ener1 = ener1 >> 1;
945       ener2 = ener2 >> (i + 1);
946    }
947    else if(i > (-16))
948    {
949       ener1 = ener1 >> (1 - i);
950       ener2 = ener2 >> 1;
951    }
952    else
953    {
954       ener1 = 0;
955       ener2 = ener2 >> 1;
956    }
957 
958    tmp = ener1 - ener2;
959    ener1 = (ener1 + ener2) + 1;
960    tmp = (tmp << 15) / ener1;
961 
962    return((Word16)tmp);
963 }
964