sp_sfrm.c

GSM中半速率语音编解码源码

    L_Temp = L_mult(ErrorTerm[1].man, snsRs11.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[1].sh = add(ErrorTerm[1].sh, swShift);
    ErrorTerm[1].man = round(L_shl(L_Temp, swShift));
    ErrorTerm[1].sh = add(ErrorTerm[1].sh, snsRs11.sh);
    if (sub(ErrorTerm[1].sh, siNormMin) < 0)
      siNormMin = ErrorTerm[1].sh;

    /* Compute error coefficient C, equation 5.24 */
    /* ------------------------------------------ */

    L_Temp = L_mult(ErrorTerm[2].man, snsRs00.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[2].sh = add(ErrorTerm[2].sh, swShift);
    ErrorTerm[2].man = round(L_shl(L_Temp, swShift));
    L_Temp = L_mult(ErrorTerm[2].man, snsRs11.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[2].sh = add(ErrorTerm[2].sh, swShift);
    ErrorTerm[2].man = round(L_shl(L_Temp, swShift));
    ErrorTerm[2].sh = add(ErrorTerm[2].sh, snsRs00.sh);
    ErrorTerm[2].sh = add(ErrorTerm[2].sh, snsRs11.sh);
    ErrorTerm[2].sh = add(ErrorTerm[2].sh, swWIShift);
    if (sub(ErrorTerm[2].sh, siNormMin) < 0)
      siNormMin = ErrorTerm[2].sh;

    /* Compute error coefficient D, equation 5.25 */
    /* ------------------------------------------ */

    L_Temp = L_mult(ErrorTerm[3].man, snsRs00.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[3].sh = add(ErrorTerm[3].sh, swShift);
    ErrorTerm[3].man = round(L_shl(L_Temp, swShift));
    L_Temp = L_mult(ErrorTerm[3].man, snsRs00.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[3].sh = add(ErrorTerm[3].sh, swShift);
    ErrorTerm[3].man = round(L_shl(L_Temp, swShift));
    ErrorTerm[3].sh = add(ErrorTerm[3].sh, snsRs00.sh);
    ErrorTerm[3].sh = add(ErrorTerm[3].sh, snsRs00.sh);
    ErrorTerm[3].sh = add(ErrorTerm[3].sh, swWIShift);
    if (sub(ErrorTerm[3].sh, siNormMin) < 0)
      siNormMin = ErrorTerm[3].sh;

    /* Compute error coefficient E, equation 5.26 */
    /* ------------------------------------------ */

    L_Temp = L_mult(ErrorTerm[4].man, snsRs11.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[4].sh = add(ErrorTerm[4].sh, swShift);
    ErrorTerm[4].man = round(L_shl(L_Temp, swShift));
    L_Temp = L_mult(ErrorTerm[4].man, snsRs11.man);
    swShift = norm_s(extract_h(L_Temp));
    ErrorTerm[4].sh = add(ErrorTerm[4].sh, swShift);
    ErrorTerm[4].man = round(L_shl(L_Temp, swShift));
    ErrorTerm[4].sh = add(ErrorTerm[4].sh, snsRs11.sh);
    ErrorTerm[4].sh = add(ErrorTerm[4].sh, snsRs11.sh);
    ErrorTerm[4].sh = add(ErrorTerm[4].sh, swWIShift);
    if (sub(ErrorTerm[4].sh, siNormMin) < 0)
      siNormMin = ErrorTerm[4].sh;

  }                                    /* Voicing level */



  /* Normalize all error coefficients to same shift count */
  /* ---------------------------------------------------- */

  for (i = 0; i < GSP0_VECTOR_SIZE; i++)
  {
    L_Temp = L_deposit_h(ErrorTerm[i].man);
    siNormShift = sub(ErrorTerm[i].sh, siNormMin);
    if (siNormShift > 0)
      L_Temp = L_shr(L_Temp, siNormShift);
    ErrorTerm[i].man = round(L_Temp);
  }


  /* Codebook search, find max of error equation 5.21 */
  /* ------------------------------------------------ */

  L_Temp2 = 0x80000000;

  for (i = 0; i < GSP0_NUM; i++)
  {

    L_Temp = L_mult(pppsrGsp0[swUVCode][i][0], ErrorTerm[0].man);
    L_Temp = L_mac(L_Temp, pppsrGsp0[swUVCode][i][1], ErrorTerm[1].man);
    L_Temp = L_mac(L_Temp, pppsrGsp0[swUVCode][i][2], ErrorTerm[2].man);
    L_Temp = L_mac(L_Temp, pppsrGsp0[swUVCode][i][3], ErrorTerm[3].man);
    L_Temp = L_mac(L_Temp, pppsrGsp0[swUVCode][i][4], ErrorTerm[4].man);
    if (L_sub(L_Temp2, L_Temp) < 0)
    {
      L_Temp2 = L_Temp;
      siCode = i;                      /* Save best code */
    }
  }
  return (siCode);
}

/***************************************************************************
 *
 *   FUNCTION NAME: gainTweak
 *
 *   PURPOSE:
 *
 *     Calculates gain bias factor, limits it, and
 *     applies it to A and B error coefficients.
 *
 *   INPUTS:
 *
 *     psErrorTerm[0:5] - array (6) of error coefficients in floating
 *                        point format
 *
 *   OUTPUTS:
 *
 *     psErrorTerm[0:5] - array of gain adjusted A and B error coefficients
 *
 *   RETURN VALUE:
 *
 *     None
 *
 *   IMPLEMENTATION:
 *
 *     The gain tweak is:
 *
 *                 Rpp*Rcc(0,0)*Rcc(1,1) - Rpp*Rcc(0,1)*Rcc(0,1)
 *sqrt(---------------------------------------------------------------------)
 *     Rcc(0,0)*Rpc(1)*Rpc(1)-2*Rcc(0,1)*Rpc(0)*Rpc(1)+Rcc(1,1)*Rpc(0)*Rpc(0)
 *
 *   REFERENCE:  Sub-clause 4.1.11.1 of GSM Recommendation 06.20
 *
 *   KEYWORDS: gain tweak, g_quant_vl
 *
 **************************************************************************/

void   gainTweak(struct NormSw *psErrorTerm)
{

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/

  Longword L_Temp;
  Shortword swTemp,
         swNum,
         swDenom,
         swGainTweak,
         swShift;
  struct NormSw terms[5];
  Shortword i,
         siNormShift,
         siNorm;

/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

  /* Calculate third order terms in the gain tweak factor, while
   * maintaining the largest exponent */
  /* ---------------------------------------------------- */

  /* Compute Rpp*Rcc(0,0)*Rcc(1,1) */
  /* ----------------------------- */

  L_Temp = L_mult(psErrorTerm[3].man, psErrorTerm[5].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[0].sh = add(psErrorTerm[3].sh, swShift);
  terms[0].man = round(L_shl(L_Temp, swShift));
  L_Temp = L_mult(terms[0].man, psErrorTerm[4].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[0].sh = add(terms[0].sh, swShift);
  terms[0].man = round(L_shl(L_Temp, swShift));
  terms[0].sh = add(terms[0].sh, psErrorTerm[4].sh);
  terms[0].sh = add(terms[0].sh, psErrorTerm[5].sh);
  /* Init. siNorm */
  siNorm = terms[0].sh;

  /* Compute Rpp*Rcc(0,1)*Rcc(0,1) */
  /* ----------------------------- */

  L_Temp = L_mult(psErrorTerm[2].man, psErrorTerm[2].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[1].sh = add(psErrorTerm[2].sh, swShift);
  terms[1].man = round(L_shl(L_Temp, swShift));
  L_Temp = L_mult(terms[1].man, psErrorTerm[5].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[1].sh = add(terms[1].sh, swShift);
  terms[1].man = round(L_shl(L_Temp, swShift));
  terms[1].sh = add(terms[1].sh, psErrorTerm[2].sh);
  terms[1].sh = add(terms[1].sh, psErrorTerm[5].sh);
  if (sub(terms[1].sh, siNorm) < 0)
    siNorm = terms[1].sh;

  /* Compute Rcc(0,0)*Rpc(1)*Rpc(1) */
  /* ------------------------------ */

  L_Temp = L_mult(psErrorTerm[1].man, psErrorTerm[1].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[2].sh = add(psErrorTerm[1].sh, swShift);
  terms[2].man = round(L_shl(L_Temp, swShift));
  L_Temp = L_mult(terms[2].man, psErrorTerm[3].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[2].sh = add(terms[2].sh, swShift);
  terms[2].man = round(L_shl(L_Temp, swShift));
  terms[2].sh = add(terms[2].sh, psErrorTerm[1].sh);
  terms[2].sh = add(terms[2].sh, psErrorTerm[3].sh);
  if (sub(terms[2].sh, siNorm) < 0)
    siNorm = terms[2].sh;

  /* Compute 2*Rcc(0,1)*Rpc(0)*Rpc(1) */
  /* -------------------------------- */

  L_Temp = L_mult(psErrorTerm[0].man, psErrorTerm[1].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[3].sh = add(psErrorTerm[0].sh, swShift);
  terms[3].man = round(L_shl(L_Temp, swShift));
  L_Temp = L_mult(terms[3].man, psErrorTerm[2].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[3].sh = add(terms[3].sh, swShift);
  terms[3].man = round(L_shl(L_Temp, swShift));
  terms[3].sh = add(terms[3].sh, psErrorTerm[1].sh);
  terms[3].sh = add(terms[3].sh, psErrorTerm[2].sh);
  terms[3].sh = sub(terms[3].sh, 1);   /* Multiply by 2 */
  if (sub(terms[3].sh, siNorm) < 0)
    siNorm = terms[3].sh;

  /* Compute Rcc(1,1)*Rpc(0)*Rpc(0) */
  /* ------------------------------ */

  L_Temp = L_mult(psErrorTerm[0].man, psErrorTerm[4].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[4].sh = add(psErrorTerm[0].sh, swShift);
  terms[4].man = round(L_shl(L_Temp, swShift));
  L_Temp = L_mult(terms[4].man, psErrorTerm[0].man);
  swShift = norm_s(extract_h(L_Temp));
  terms[4].sh = add(terms[4].sh, swShift);
  terms[4].man = round(L_shl(L_Temp, swShift));
  terms[4].sh = add(terms[4].sh, psErrorTerm[0].sh);
  terms[4].sh = add(terms[4].sh, psErrorTerm[4].sh);
  if (sub(terms[4].sh, siNorm) < 0)
    siNorm = terms[4].sh;

  /* Normalize all terms to same shift count */
  /* --------------------------------------- */

  for (i = 0; i < 5; i++)
  {
    L_Temp = L_deposit_h(terms[i].man);
    siNormShift = sub(terms[i].sh, siNorm);
    if (siNormShift > 0)
    {
      L_Temp = L_shr(L_Temp, siNormShift);
    }
    terms[i].man = round(L_Temp);
  }

  /* Calculate numerator */
  /* ------------------- */

  /* Rpp*Rcc(0,0)*Rcc(1,1) - Rpp*Rcc(0,1)*Rcc(0,1) */
  /* --------------------------------------------- */

  swNum = sub(terms[0].man, terms[1].man);

  /* Skip gain tweak if numerator =< 0 */
  /* --------------------------------- */

  if (swNum <= 0)
    return;

  /* Calculate denominator */
  /* --------------------- */

  /* Rcc(0,0)*Rpc(1)*Rpc(1)-2*Rcc(0,1)*Rpc(0)*Rpc(1)+Rcc(1,1)*Rpc(0)*Rpc(0) */
  /*----------------------------------------------------------------------*/

  swDenom = sub(terms[2].man, terms[3].man);
  swDenom = add(swDenom, terms[4].man);

  /* Skip gain tweak if denominator =< 0 */
  /* ----------------------------------- */

  if (swDenom <= 0)
    return;

  /* Compare numerator to denominator, skip if tweak =< 1 */
  /* ---------------------------------------------------- */

  swTemp = sub(swNum, swDenom);
  if (swTemp <= 0)
    return;

  /* Normalize and do divide */
  /* ----------------------- */

  swShift = norm_s(swNum);
  siNormShift = sub(swShift, 1);       /* Multiply by 2 */
  swNum = shl(swNum, swShift);
  swNum = shr(swNum, 1);
  swShift = norm_s(swDenom);
  siNormShift = sub(siNormShift, swShift);
  swDenom = shl(swDenom, swShift);
  swTemp = divide_s(swNum, swDenom);
  swShift = norm_s(swTemp);
  siNormShift = add(siNormShift, swShift);
  L_Temp = L_shl(L_deposit_h(swTemp), swShift);

  /* Calculate square root */
  /* --------------------- */

  swTemp = sqroot(L_Temp);

  /* If odd no. of shifts compensate by sqrt(0.5) */
  /* -------------------------------------------- */

  if (siNormShift & 1)
  {
    L_Temp = L_mult(0x5a82, swTemp);
    siNormShift = sub(siNormShift, 1);
  }
  else
    L_Temp = L_deposit_h(swTemp);
  siNormShift = shr(siNormShift, 1);
  swShift = norm_s(extract_h(L_Temp));
  siNormShift = add(siNormShift, swShift);
  swGainTweak = round(L_shl(L_Temp, swShift));

  /* If exponent > -1, skip gain tweak */
  /* --------------------------------- */

  if (add(1, siNormShift) > 0)
    return;

  /* If exponent < -1, limit gain tweak to GTWEAKMAX */
  /* ----------------------------------------------- */

  if (add(1, siNormShift) < 0)
    swGainTweak = GTWEAKMAX;
  else
  {

    /* If exponent = -1, compare to GTWEAKMAX */
    /* -------------------------------------- */

    if (sub(GTWEAKMAX, swGainTweak) < 0)
      swGainTweak = GTWEAKMAX;
  }

  /* Multiply gain tweak factor on A and B error terms */
  /* ------------------------------------------------- */

  L_Temp = L_mult(swGainTweak, psErrorTerm[0].man);
  swShift = norm_s(extract_h(L_Temp));
  psErrorTerm[0].sh = add(psErrorTerm[0].sh, swShift);
  psErrorTerm[0].sh = sub(psErrorTerm[0].sh, 1);
  psErrorTerm[0].man = round(L_shl(L_Temp, swShift));

  L_Temp = L_mult(swGainTweak, psErrorTerm[1].man);
  swShift = norm_s(extract_h(L_Temp));
  psErrorTerm[1].sh = add(psErrorTerm[1].sh, swShift);
  psErrorTerm[1].sh = sub(psErrorTerm[1].sh, 1);
  psErrorTerm[1].man = round(L_shl(L_Temp, swShift));

}

/***************************************************************************
 *
 *   FUNCTION NAME: hnwFilt
 *
 *   PURPOSE:
 *     Performs the filtering operation for harmonic noise weighting.
 *
 *   INPUTS:
 *         pswInSample[0:39] - array of input speech signal,