dec_gain.c
上传用户:zhongxx05
上传日期:2007-06-06
资源大小:33641k
文件大小:21k
源码类别:

Symbian

开发平台:

C/C++

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