math_adv40.c

EVRC是用于高通公司的语音编码

#ifdef WMOPS_FX
#include "typedef_fx.h"
#include "main_fx.h"
#include "const_fx.h"
#include "lib_wmp_fx.h"

#endif

/*----------------------------------------------------------------------------*/
/*-------------------------------- FUNCTIONS ---------------------------------*/
/*----------------------------------------------------------------------------*/


/***************************************************************************
 *
 *   FUNCTION NAME: fn1_sqroot
 *
 *   PURPOSE:
 *     The purpose of this function is to implement an approximation
 *     of 1/sqroot(x) function
 *
 *
 *   INPUTS:
 *
 *     Input
 *                     normalized input (input range constrained to be < 1.0)
 *                     (Q15)
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     Output
 *                     = 1/sqroot( L_Input) (Q14)
 *
 *   DESCRIPTION:
 *
 *     The following recursive algorithm is used for the approximation
 *    
 *     x = 1.0;
 *     for (i = 0; i < M; i++)
 *	  x = x*(1.5 - v*x*x/2);
 *
 *     where v i the normalized input value
 *
 *************************************************************************/

Word16  fn1_sqroot (Word16 Input)
{

/*_________________________________________________________________________
 |                                                                         |
 |                           Local Static Variables                        |
 |_________________________________________________________________________|
*/

/*_________________________________________________________________________
 |                                                                         |
 |                            Automatic Variables                          |
 |_________________________________________________________________________|
*/
        Word16 i;
	Word16 Output;
	Word16 reg16_1, reg16_2;
	Word32 reg32;
	Word40 acc40;
/*_________________________________________________________________________
 |                                                                         |
 |                              Executable Code                            |
 |_________________________________________________________________________|
*/

	reg16_1    = 0x4000;                     //(1.0 in Q14)
	for (i = 0; i < 5; i++)
	  {
#ifdef WMOPS_FX
	    move16();
#endif
	    reg16_2 = reg16_1;
	    reg32   = L_mult(reg16_1, reg16_1);  // x^2 Q29 32bits
	    reg32   = L_shl (reg32, 1);          // x^2 Q30 32bits
	    reg16_1 = round32(reg32);             // x^2 Q14 16bits
	      
	    acc40   = L_mac40(0, reg16_1, Input); // v/2*x^2 Q31 40 bits 
	    acc40   = L_shr40(acc40, 1);          // v/2*x^2 Q30 40 bits 
	    reg32   = (Word32)acc40;              // no need saturation
	    reg16_1 = round32(reg32);              // y = v/2*x^2 Q14 16bits

	    reg16_1 = sub(0x6000, reg16_1);       // z = 1.5 -y Q14 16 bits

	    reg32   = L_mult(reg16_2, reg16_1);   // x*z Q29 32bits
	    reg32   = L_shl (reg32, 1);           // x*z Q30 32bits
	    reg16_1 = round32(reg32);              // x*z Q14 16bits
	  }
	Output = reg16_1;

	/* return result */
	/* ------------- */

	return (Output);
}


/***************************************************************************
 *
 *   FUNCTION NAME: sqrt_ratio40
 *
 *   PURPOSE:
 *
 *     The purpose of this function is to perform a single precision square
 *     root function on a ratio of Word40
 *
 *   INPUTS:
 *
 *     L_num
 *                     input numerator 
 *     L_den
 *                     input denominator
 *     shift_result   
 *                     input additional shift for the result
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     reg16_2
 *                     output to square root function 16 bits
 *
 *   DESCRIPTION:
 *
 *      Input assumed to be NOT normalized
 *
 *
 *************************************************************************/

Word16 sqrt_ratio40 (Word40 L_num40, Word40 L_den40, Word16 shift)

{
  /*-------------------------------------------------------------------------*/

  Word16 exp_num, exp_den, exp_ratio;
  Word32 num, den;

  Word16 reg16, reg16_2;
  Word32 reg32;

  /*-------------------------------------------------------------------------*/
  /*                              Normalize the Energies                     */
  /*-------------------------------------------------------------------------*/
  
  exp_num = norm32_l40(L_num40);
  num = (Word32) L_shl40(L_num40, exp_num);
  
  exp_den = norm32_l40(L_den40);
  den = (Word32) L_shl40(L_den40, exp_den);
  
  /*-------------------------------------------------------------------------*/
  /*                          Calculate the ratio num / den                  */
  /*-------------------------------------------------------------------------*/
  
  if (L_sub(num, den) > 0L)
    {
      num = L_shr(num, 1);
      exp_num = sub(exp_num, 1);
    }
  
  reg32 = L_divide(num, den);     //Q31
  
  /*-------------------------------------------------------------------------*/
  /*              Normalize num / den to calculate sqroot(num / den)         */
  /*-------------------------------------------------------------------------*/
  
  exp_ratio = norm_l(reg32);
  reg32 = L_shl (reg32, exp_ratio);
  
#ifdef WMOPS_FX
  test();
#endif
  
  if (reg32 == 0)
    reg16_2 = 0;
  else
    reg16_2 = sqroot (reg32);
  
  /*-------------------------------------------------------------------------*/
  /*                    Calculate the sqroot for the exponent                */
  /*-------------------------------------------------------------------------*/
  /*-------------------------------------------------------------------------*/
  /*                     Test if the exponent is even or odd                 */
  /*-------------------------------------------------------------------------*/
  
#ifdef WMOPS_FX
  test();
  logic16();
#endif
  reg16 = add(exp_ratio, exp_num);
  reg16 = sub(reg16, exp_den);
  
  if ((reg16 & 0x0001) != 0)
    reg16_2 = mult_r(reg16_2, 0x5A82);     // 1/sqrt(2) in Q15
  
  /*-------------------------------------------------------------------------*/
  /*                      Calculate the sqroot for the exponent              */
  /*-------------------------------------------------------------------------*/
	  
  reg16 = shr(reg16, 1);
  
  /*-------------------------------------------------------------------------*/
  /*                          reg16 = sqroot( L_num/L_den)                   */
  /*-------------------------------------------------------------------------*/

  reg16   = add(reg16, shift);
  reg16_2 = shr(reg16_2, reg16);    // Q(15 - shift)

  /*-------------------------------------------------------------------------*/

  return (reg16_2);

  /*-------------------------------------------------------------------------*/
}

/*---------------------------------------------------------------------------*/

/*============================================================================*/
/*----------------------------------- END ------------------------------------*/
/*============================================================================*/