basicop2.c

AMR-NB 的编码实现,纯C, VC下建立工程即可用.

/*********************************************************************************
  File: basicop2.c
  Purpose: implemention of  fix-point compution

**********************************************************************************/
 
#include "typedef.h"
#include "basic_op.h"


static const Word16 sqrt_exp_table[49] =
{
 16384,
 16888,
 17378,
 17854,
 18318,
 18770,
 19212,
 19644,
 20066,
 20480,
 20886,
 21283,
 21674,
 22058,
 22435,
 22806,
 23170,
 23530,
 23884,
 24232,
 24576,
 24915,
 25249,
 25580,
 25905,
 26227,
 26545,
 26859,
 27170,
 27477,
 27780,
 28081,
 28378,
 28672,
 28963,
 29251,
 29537,
 29819,
 30099,
 30377,
 30652,
 30924,
 31194,
 31462,
 31727,
 31991,
 32252,
 32511,
 32767
};

static const Word16 pow2_table[33] =
{
    16384, 16743, 17109, 17484, 17867, 18258, 18658, 19066, 19484, 19911,
    20347, 20792, 21247, 21713, 22188, 22674, 23170, 23678, 24196, 24726,
    25268, 25821, 26386, 26964, 27554, 28158, 28774, 29405, 30048, 30706,
    31379, 32066, 32767
};


static const Word16 log2_table[33] =
{
    0, 1455, 2866, 4236, 5568, 6863, 8124, 9352, 10549, 11716,
    12855, 13967, 15054, 16117, 17156, 18172, 19167, 20142, 21097, 22033,
    22951, 23852, 24735, 25603, 26455, 27291, 28113, 28922, 29716, 30497,
    31266, 32023, 32767
};

static const Word16 table[49] =
{

    32767, 31790, 30894, 30070, 29309, 28602, 27945, 27330, 26755, 26214,
    25705, 25225, 24770, 24339, 23930, 23541, 23170, 22817, 22479, 22155,
    21845, 21548, 21263, 20988, 20724, 20470, 20225, 19988, 19760, 19539,
    19326, 19119, 18919, 18725, 18536, 18354, 18176, 18004, 17837, 17674,
    17515, 17361, 17211, 17064, 16921, 16782, 16646, 16514, 16384
};



Word16 shr_r (Word16 var1, Word16 var2)
{
    Word16 var_out;

    if (var2 > 15)
    {
        var_out = 0;
    }
    else
    {
       
        var_out = var2>0 ? var1>>var2 : ( var1 > MAX_16>>-var2? MAX_16 : ( var1 < MIN_16 >> -var2 ? MIN_16: var1 << -var2 ));

        if (var2 > 0)
        {
            if ((var1 & ((Word16) 1 << (var2 - 1))) != 0)
            {
                var_out++;
            }
        }
    }

    return (var_out);
}

Word32 L_shr_r (Word32 L_var1, Word16 var2)
{
    Word32 L_var_out;

    if (var2 > 31)
    {
        L_var_out = 0;
    }
    else
    {

	 L_var_out = var2 >0 ? L_var1>>var2 : ( L_var1 >= MAX_32>> -var2 ? MAX_32 : (L_var1 <= (MIN_32>> -var2) ? MIN_32 : L_var1 << -var2 ) );

        if (var2 > 0)
        {
            if ((L_var1 & ((Word32) 1 << (var2 - 1))) != 0)
            {
                L_var_out++;
            }
        }
    }

    return (L_var_out);
}


Word16 norm_s (Word16 var1)
{

    Word16 var_out;

    if (var1 == 0)
    {
        var_out = 0;
    }
    else
    {
        if (var1 == (Word16) 0xffff)
        {
            var_out = 15;
        }
        else
        {
            if (var1 < 0)
            {
                var1 = ~var1;
            }
            for (var_out = 0; var1 < 0x4000; var_out++)
            {
                var1 <<= 1;
            }
        }
    }
    return (var_out);


}
Word16 div_s (Word16 var1, Word16 var2)
{


    Word16 var_out = 0;
    Word16 iteration;
    Word32 L_num;
    Word32 L_denom;

    if ((var1 > var2) || (var1 < 0) || (var2 < 0))
    {
       printf ("Division Error var1=%d  var2=%d\n", var1, var2);
      //      abort(); 
    }
    if (var2 == 0)
    {
        printf ("Division by 0, Fatal error \n");
        //    abort(); 
    }
    if (var1 == 0)
    {
        var_out = 0;
    }
    else
    {
        if (var1 == var2)
        {
            var_out = MAX_16;
        }
        else
        {
            L_num = (Word32) var1;
            L_denom = (Word32)var2;

            for (iteration = 0; iteration < 15; iteration++)
            {
                var_out <<= 1;
                L_num <<= 1;

	           var_out = L_num >= L_denom ? var_out+1 : var_out;
		    L_num  = L_num >= L_denom ?L_num -L_denom : L_num;
			
            }
        }
    }

    return (var_out);


}


Word16 norm_l (Word32 L_var1)
{
    Word16 var_out;

    if (L_var1 == 0)
    {
        var_out = 0;
    }
    else
    {
        if (L_var1 == (Word32) 0xffffffffL)
        {
            var_out = 31;
        }
        else
        {
            if (L_var1 < 0)
            {
                L_var1 = ~L_var1;
            }
            for (var_out = 0; L_var1 < (Word32) 0x40000000L; var_out++)
            {
                L_var1 <<= 1;
            }
        }
    }


    return (var_out);
}
Word32 Div_32 (Word32 L_num, Word16 denom_hi, Word16 denom_lo)
{
	Word16 approx, hi, lo, n_hi, n_lo;
	Word32 L_32;

	/* First approximation: 1 / L_denom = 1/denom_hi */

	approx = div_s ((Word16) 0x3fff, denom_hi);

	/* 1/L_denom = approx * (2.0 - L_denom * approx) */
	L_32 = (denom_hi*approx<<1 )+ ((denom_lo*approx>>15)<<1);


	L_32 = ((Word32) 0x7fffffffL - L_32);
	hi = (Word16)(L_32 >> 16);
	lo = (L_32 -(hi<<16))>>1;

	L_32 =(hi*approx<<1)+((lo*approx>>15)<<1);

	/* L_num * (1/L_denom) */
	hi = (Word16)(L_32 >> 16);
	lo = (L_32 -(hi<<16))>>1;
	n_hi = (Word16)(L_num>>16);
	n_lo = (L_num -(n_hi<<16))>>1;
	L_32 = (n_hi*hi<<1)+(( (n_hi*lo>>15) +(hi*n_lo>>15 )) << 1 );
	L_32 <<= 2;
	return (L_32);
	
}

Word32 Inv_sqrt ( Word32 L_x   )
{
	Word16 exp, i, a, tmp;
	Word32 L_y;
	const Word16 *ptable = table;


	if (L_x <=  0)
		return ((Word32) 0x3fffffffL);

	exp = norm_l (L_x);
	L_x <<= exp;   

	exp =  30- exp;
	//L_x >>= (!(exp&1));
	L_x = (exp&1) == 0 ? L_x>>1 : L_x;
	exp >>= 1;
	exp ++;

	i =  (Word16)(L_x >> 25 );       
	a =  (Word16)(L_x>>10);                  
	a = a & (Word16) 0x7fff;    

	i -= 16 ;
	L_y =  (ptable[i] << 16 );            
	tmp =  (ptable[i] - ptable[i + 1]); 
	L_y -=  tmp*a << 1 ;               
	L_y >>=  exp ;   

	return (L_y);



}

void Log2_norm (Word32 L_x,  Word16 exp,  Word16 *exponent,  Word16 *fraction)
{

	Word16 i, a, tmp;
	Word32 L_y;
	const Word16* pLog2table = log2_table;

	if (L_x <=  0)
	{
		*exponent = 0;      
		*fraction = 0;         
		return;
	}

	*exponent = 30- exp; 

	L_x >>= 9;
	i =  (Word16)(L_x >> 16 );               
	L_x >>= 1;
	a = (Word16)L_x ;                        
	a = a & (Word16) 0x7fff; 

	i =  (i-32);
	L_y =  (pLog2table[i] << 16 );                    
	tmp =  (pLog2table[i]- pLog2table[i + 1]); 
	L_y -=  (tmp*a << 1);        
	*fraction = (Word16)(L_y >> 16 );
	return;
	
}
void Log2 (Word32 L_x,  Word16 *exponent, Word16 *fraction )
{
    Word16 exp;
    Word32 tmp;
	
    exp = norm_l (L_x);
    tmp = exp>0? L_x << exp : L_x >> -exp;
	
    Log2_norm (tmp, exp, exponent, fraction);
	
}

Word32 Pow2 ( Word16 exponent, Word16 fraction)
{
    Word16 exp, i, a, tmp;
    Word32 L_x;
    const Word16*ppow2_table = pow2_table;

    L_x =  (fraction<<6); 
    i =   (Word16)(L_x>>16);  
    L_x =  (L_x>> 1);
    a =  (Word16)L_x;             
    a = a & (Word16) 0x7fff;   

    L_x =  (ppow2_table[i]<<16);  
    tmp = ppow2_table[i]- ppow2_table[i + 1] ;
    L_x -=  tmp*a <<1 ; 

    exp = 30- exponent;
    L_x = L_shr_r (L_x, exp);

    return (L_x);
	

}


Word32 sqrt_l_exp ( Word32 L_x,  Word16 *exp )
{

    Word16 e, i, a, tmp;
    Word32 L_y;
    const Word16 *psqrt_exp_table = sqrt_exp_table;
		
    if (L_x <= (Word32) 0)
    {
        *exp = 0;            
        return (Word32) 0;
    }

    e = norm_l (L_x) & 0xFFFE;      
    L_x <<= e;                                 
    *exp   = e;                         
    L_x >>=  9 ;
    i = (Word16)(L_x>>16);                                         
    L_x >>=  1;   
    a = L_x & (Word16) 0x7fff;  
    i -=  16;   
    L_y = (psqrt_exp_table[i]<<16 );   
    tmp =  psqrt_exp_table[i] - psqrt_exp_table[i + 1] ; 
    L_y -=  tmp*a<<1 ;  

    return (L_y);

}