math_lib.c

2400 bps MELP语音编解码定点算法

  
  static Shortword tens_table[9]=
  {
    26844, 16777, 20972, 26214, 1, 10, 100, 1000, 10000
  };
  
  static Shortword Q_table[4]=
  {
    28, 24, 21, 18
  };
  
  
  Shortword y, interp_factor, interp_component;
  Shortword index1, index2;
  Shortword ten_multiple;
  Longword L_y;
  Shortword temp1, temp2;
  
  
  ten_multiple = shr(x,12);    data_move();
  if (ten_multiple < -4)
    return((Shortword)0);
  else if (ten_multiple > 4) {
    saturation = saturation + 1;
    return((Shortword)SW_MAX);
  }
  
  index1 = shr((Shortword)(x&(Shortword)0x0ff0), 4);    logic();  data_move();
  index2 = add(index1,1);    data_move();
  
    
  /* interpolation */
  /* shift by 11 to make it a number between 0 & 1 in Q15 */
  interp_factor = shl((Shortword)(x&(Shortword)0x000f),11);    logic();   data_move();
  temp1 = sub(table[index2],table[index1]);
  interp_component = mult(temp1,interp_factor);    data_move();
  
  /* L_y in Q12 if x >= 0 and in Q_table[temp2]+12 if x < 0 */
  temp1 = add(table[index1], interp_component);
  temp2 = add(ten_multiple,4);
  L_y = L_mult(tens_table[temp2],temp1);     L_data_move();

  /* increment complexity for if statement */
  compare_zero();
  if (ten_multiple >= 0)
    {
      temp1 = sub(12,Q);
      L_y = L_shr(L_y, temp1);
      y = extract_l(L_y);
      if (extract_h(L_y)) {
	y=SW_MAX;    data_move();
	saturation = saturation + 1;
      }
    }
  else
    {
      temp1 = add(Q_table[temp2],12);
      temp1 = sub(temp1,Q);
      y = extract_l(L_shr(L_y, temp1));
    }
  
  return(y);
} /* pow10_fxp */



/***************************************************************************
 *
 *   FUNCTION NAME: sqrt_fxp
 *
 *   PURPOSE:
 *
 *     Compute the square root of x.
 *
 *
 *   INPUTS:
 *
 *     x
 *                     16 bit short signed integer (Shortword).
 *     Q
 *                     16 bit short signed integer (Shortword) represents
 *                     Q value of x.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     temp
 *                     16 bit short signed integer (Shortword) in same Q.
 *
 *************************************************************************/
Shortword sqrt_fxp(Shortword x, Shortword Q)
{
  Shortword temp;

  /* increment complexity for if statement */
  compare_zero();
  if (!x)
    return((Shortword)0);

  temp = shr(log10_fxp(x,Q),1);
  temp = pow10_fxp(temp,Q);

  return (temp);
}


/***************************************************************************
 *
 *   FUNCTION NAME: L_sqrt_fxp
 *
 *   PURPOSE:
 *
 *     Compute the square root of x.
 *
 *
 *   INPUTS:
 *
 *     x
 *                     32 bit long signed integer (Longword).
 *     Q
 *                     16 bit short signed integer (Shortword) represents
 *                     Q value of x.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     temp
 *                     16 bit short signed integer (Shortword) in same Q.
 *
 *************************************************************************/
Shortword L_sqrt_fxp(Longword x, Shortword Q)
{
  Shortword temp;

  /* increment complexity for if statement */
  compare_zero();
  if (!x)
    return((Shortword)0);

  temp = L_log10_fxp(x,Q);
  /* temp in Q11, pow10 treat it as Q12, => no need to shr by 1. */
  temp = pow10_fxp(temp,Q);

  return (temp);
}


/***************************************************************************
 *
 *   FUNCTION NAME: L_pow_fxp
 *
 *   PURPOSE:
 *
 *     Compute the value of x raised to the power 'power'.
 *
 *
 *   INPUTS:
 *
 *     x
 *                     32 bit long signed integer (Longword).
 *     power
 *                     16 bit short signed integer (Shortword) in Q15.
 *     Q_in
 *                     16 bit short signed integer (Shortword) represents
 *                     Q value of x.
 *     Q_out
 *                     16 bit short signed integer (Shortword) represents
 *                     required Q value of returned result.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     temp
 *                     16 bit short signed integer (Shortword).
 *
 *************************************************************************/
Shortword L_pow_fxp(Longword x,Shortword power,Shortword Q_in,Shortword Q_out)
{
  Shortword temp;

  /* increment complexity for if statement */
  compare_zero();
  if (!x)
    return((Shortword)0);

  temp = L_log10_fxp(x,Q_in);   /* temp in Q11 */
  temp = mult(power,shl(temp,1));    data_move();   /* temp in Q12 */

  temp = pow10_fxp(temp,Q_out);

  return (temp);
}


/***************************************************************************
 *
 *   FUNCTION NAME: sin_fxp
 *
 *   PURPOSE:
 *
 *     Compute the sine of x whose value is expressed in radians/PI.
 *
 *
 *   INPUTS:
 *
 *     x
 *                     16 bit short signed integer (Shortword) in Q15.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     ty
 *                     16 bit short signed integer (Shortword) in Q15.
 *
 *************************************************************************/

#define PI_Q13     24576    /* M_PI*(1<<13) */

Shortword sin_fxp(Shortword x)
{
    static Shortword table[129] =
    { 
            0,   402,   804,  1206,  1608,  2009,  2411,  2811,  3212,
	 3612,  4011,  4410,  4808,  5205,  5602,  5998,  6393,  6787,
	 7180,  7571,  7962,  8351,  8740,  9127,  9512,  9896, 10279,
	10660, 11039, 11417, 11793, 12167, 12540, 12910, 13279, 13646,
	14010, 14373, 14733, 15091, 15447, 15800, 16151, 16500, 16846,
	17190, 17531, 17869, 18205, 18538, 18868, 19195, 19520, 19841,
	20160, 20475, 20788, 21097, 21403, 21706, 22006, 22302, 22595,
	22884, 23170, 23453, 23732, 24008, 24279, 24548, 24812, 25073,
	25330, 25583, 25833, 26078, 26320, 26557, 26791, 27020, 27246,
	27467, 27684, 27897, 28106, 28311, 28511, 28707, 28899, 29086,
	29269, 29448, 29622, 29792, 29957, 30118, 30274, 30425, 30572,
	30715, 30853, 30986, 31114, 31238, 31357, 31471, 31581, 31686,
	31786, 31881, 31972, 32058, 32138, 32214, 32286, 32352, 32413,
	32470, 32522, 32568, 32610, 32647, 32679, 32706, 32729, 32746,
	32758, 32766, 32767
    };
    
    Shortword tx, ty;
    Shortword sign;
    Shortword index1,index2;
    Shortword m;
    Shortword temp;

    sign = 0;
    if (x < 0) {
        tx = negate(x);     data_move();
	sign = -1;     data_move();
    }
    else {
      tx = x;     data_move();
    }

    /* if angle > pi/2, sin(angle) = sin(pi-angle) */
    if (tx > X05_Q15)
    {
        tx = sub(ONE_Q15,tx);     data_move();
    }
    /* convert input to be within range 0-128 */
    index1 = shr(tx,7);     data_move();
    index2 = add(index1,1);     data_move();
    
    /* increment complexity for if statement */
    compare_nonzero();
    if (index1 == 128) {
      if (sign != 0)
        return(negate(table[index1]));
      else
	return(table[index1]);
    }
    
    m = sub(tx,shl(index1,7));
    /* convert decimal part to Q15 */
    m = shl(m,8);     data_move();

    /* interpolate */
    temp = sub(table[index2],table[index1]);
    temp = mult(m,temp);
    ty = add(table[index1],temp);     data_move();

    /* increment complexity for if statement */
    compare_zero();
    if (sign != 0)
        return(negate(ty));
    else
        return(ty);
} /* sin_fxp */


/***************************************************************************
 *
 *   FUNCTION NAME: cos_fxp
 *
 *   PURPOSE:
 *
 *     Compute the cosine of x whose value is expressed in radians/PI.
 *
 *
 *   INPUTS:
 *
 *     x
 *                     16 bit short signed integer (Shortword) in Q15.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     ty
 *                     16 bit short signed integer (Shortword) in Q15.
 *
 *************************************************************************/
Shortword cos_fxp(Shortword x)
{
    static Shortword table[129] =
    { 
        32767, 32766, 32758, 32746, 32729, 32706, 32679, 32647, 32610,
        32568, 32522, 32470, 32413, 32352, 32286, 32214, 32138, 32058,
        31972, 31881, 31786, 31686, 31581, 31471, 31357, 31238, 31114,
        30986, 30853, 30715, 30572, 30425, 30274, 30118, 29957, 29792,
        29622, 29448, 29269, 29086, 28899, 28707, 28511, 28311, 28106,
        27897, 27684, 27467, 27246, 27020, 26791, 26557, 26320, 26078,
        25833, 25583, 25330, 25073, 24812, 24548, 24279, 24008, 23732,
        23453, 23170, 22884, 22595, 22302, 22006, 21706, 21403, 21097,
        20788, 20475, 20160, 19841, 19520, 19195, 18868, 18538, 18205,
        17869, 17531, 17190, 16846, 16500, 16151, 15800, 15447, 15091,
        14733, 14373, 14010, 13646, 13279, 12910, 12540, 12167, 11793,
        11417, 11039, 10660, 10279,  9896,  9512,  9127,  8740,  8351,
        7962,   7571,  7180,  6787,  6393,  5998,  5602,  5205,  4808,
        4410,   4011,  3612,  3212,  2811,  2411,  2009,  1608,  1206,
        804,     402,     0
    };
    
    Shortword tx, ty;
    Shortword sign;
    Shortword index1,index2;
    Shortword m;
    Shortword temp;

    sign = 0;
    /* increment complexity for if statement */
    compare_zero();
    if (x < 0) {
      tx = negate(x);     data_move();
    }
    else {
        tx = x;     data_move();
    }

    /* if angle > pi/2, cos(angle) = -cos(pi-angle) */
    /* increment complexity for if statement */
    compare_nonzero();
    if (tx > X05_Q15)
    {
        tx = sub(ONE_Q15,tx);     data_move();
        sign = -1;     data_move();
    }
    /* convert input to be within range 0-128 */
    index1 = shr(tx,7);     data_move();
    index2 = add(index1,1);     data_move();
    
    /* increment complexity for if statement */
    compare_nonzero();
    if (index1 == 128)
        return((Shortword)0);
    
    m = sub(tx,shl(index1,7));
    /* convert decimal part to Q15 */
    m = shl(m,8);     data_move();

    temp = sub(table[index2],table[index1]);
    temp = mult(m,temp);
    ty = add(table[index1],temp);     data_move();

    /* increment complexity for if statement */
    compare_zero();
    if (sign != 0)
        return(negate(ty));
    else
        return(ty);
} /* cos_fxp */