www.pudn.com > speakFreely-code.zip > adv40.c
/*===========================================================================*/ /* Lucent Technologies Network Wireless Systems */ /* */ /* Copyright (C) 1997 Lucent Technologies. All rights reserved. */ /* Lucent Technologies proprietary and confidential. */ /*---------------------------------------------------------------------------*/ /*===========================================================================*/ /* ..Includes. */ /*---------------------------------------------------------------------------*/ #include#include #include "adv40.h" #include "math40.h" /*===========================================================================*/ /* Divide num by denom. Note that both must be positive, and */ /* num <= denom. The output is set to 0 if invalid input is provided. */ /* */ /* In the case where num==denom the function returns 0x7fff. The output */ /* is undefined for invalid inputs. This implementation returns zero */ /* and issues a warning via stdio if invalid input is presented. */ /*---------------------------------------------------------------------------*/ Acc _A_div ( int32 num, int32 denom ) { /*....(local) variables....*/ register int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_div if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_div(%ld,%ld):\n",num,denom); #endif #ifdef DSP_OP_COUNT DSP40_OP_COUNT(18); #endif if (num < 0 || denom < 0 || num > denom) { /* undefined output for invalid input */ fprintf(stderr,"WARNING: _A_div(): invalid input, return 0.\n"); return((Acc)0); } if (num == denom) { fprintf(stderr,"WARNING: _A_div(): num = denom ==> OVERFLOW!\n"); return(ACC_MAX16); } lval = (((int32)0x8000 * num) / denom); if (lval > 32767) lval = 32767; if (lval < -32768) lval = -32768; #if DSP40_DEBUG_A_div if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_div(%ld,%ld)= %.0f\n",num,denom,(Acc)lval); #endif return((Acc)lval); } /*===========================================================================*/ /* Fractional integer division of two 32 bit numbers. num / denom. */ /* num and denom must be positive and num < denom. */ /* */ /* Algorithm: */ /* */ /* - find = 1/denom. */ /* First approximation: approx = 1 / extract_h(denom) */ /* 1/denom = approx * (2.0 - denom * approx ) */ /* */ /* - result = num * (1/denom) */ /*---------------------------------------------------------------------------*/ Acc _A_div_ll ( int32 num, int32 denom ) { /*....(local) variables....*/ Acc acc; int16 sval; int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_div_ll if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_div_ll(%ld,%ld):\n",num,denom); #endif #ifdef DSP_OP_COUNT DSP40_OP_COUNT(1); #endif if (num < 0 || denom < 0 || num > denom) { printf("WARNING: _A_div_ll(): Invalid input, returning 0.\n"); return((Acc)0); } /*...First approximation: 1 / denom = 1/S_get_hi(denom)...*/ acc = A_set_l(denom); sval = S_get_hi(acc); acc = A_div_ss((int16)0x3fff,sval); sval = S_get_lo(acc); /*...1/denom = sval_approx * (2.0 - denom * sval_approx)...*/ acc = A_mul_ls(denom,sval); acc = -acc; acc = A_add_al(acc,(int32)0x7fffffff); lval = L_get(acc); acc = A_mul_ls(lval,sval); lval = L_get(acc); /*...num * (1/denom)...*/ acc = A_mul_ll(num,lval); acc = A_shl_a(acc,2); #if DSP40_DEBUG_A_div_ll if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_div_ll(%ld,%ld)= %.0f\n",num,denom,acc); #endif return(acc); } /*===========================================================================*/ /* Perform a single precision square root function on an int32 (Q31). */ /* Input assumed to be normalized. */ /* */ /* The algorithm is based around a six term Taylor expansion : */ /* */ /* y^0.5 = (1+x)^0.5 */ /* ~= 1 + (x/2) - 0.5*((x/2)^2) + 0.5*((x/2)^3) */ /* - 0.625*((x/2)^4) + 0.875*((x/2)^5) */ /* */ /* Max error less than 0.08 % for normalized input ( 0.5 <= x < 1 ) */ /* */ /*---------------------------------------------------------------------------*/ Acc _A_sqrt ( Acc sqrt_in ) { #define PLUS_HALF 0x40000000L /* 0.5 */ #define MINUS_ONE 0x80000000L /* -1 */ #define TERM5_MULTIPLER 0x5000 /* 0.625 */ #define TERM6_MULTIPLER 0x7000 /* 0.875 */ /*....(local) variables....*/ Acc acc; int32 ltmp0; int32 ltmp1; int16 stmp1; int16 stmp2; int16 stmp3; int16 stmp4; /*....execute....*/ #if DSP40_DEBUG_A_sqrt if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_sqrt(%.0f):\n",sqrt_in); #endif #ifdef DSP_OP_COUNT DSP40_OP_COUNT(25); #endif /*...determine 2nd term x/2 = (y-1)/2...*/ acc = sqrt_in; acc = A_shr_a(acc,1); /* acc = y/2 */ acc -= (Acc)PLUS_HALF; /* acc = (y-1)/2 */ stmp1 = S_get_hi(acc); /* stmp1 = x/2 */ /*...add contribution of 2nd term...*/ acc += (Acc)MINUS_ONE; /* must be += for flt-pt */ ltmp1 = L_get(acc); /* ltmp1 = 1 + x/2 */ /*...determine 3rd term...*/ acc = -(Acc)(stmp1 * stmp1) * 2.0; /* acc = -(x/2)^2 */ stmp2 = S_get_hi(acc); /* stmp2 = -(x/2)^2 */ acc = A_shr_a(acc,1); /* acc = -0.5 * (x/2)^2 */ /*...add contribution of 3rd term...*/ acc += (Acc)ltmp1; ltmp0 = L_get(acc); /*...determine 4th term...*/ acc = -(Acc)(stmp1 * stmp2) * 2.0; stmp3 = S_get_hi(acc); /* stmp3 = -(x/2)^3 */ acc = A_shr_a(acc,1); /* acc = -0.5 * (x/2)^3 */ /*...add contribution of 4th term...*/ acc += (Acc)ltmp0; ltmp1 = L_get(acc); /*...determine partial 5th term...*/ acc = (Acc)(stmp1 * stmp3) * 2.0; stmp4 = S_get_hi_r(acc); /* stmp4 = (x/2)^4 */ /*...determine partial 6th term...*/ acc = -(Acc)(stmp2 * stmp3) * 2.0; stmp2 = S_get_hi_r(acc); /* stmp2 = (x/2)^5 */ /*...determine 5th term and add its contribution...*/ acc = -(Acc)(TERM5_MULTIPLER * stmp4) * 2.0; acc += (Acc)ltmp1; /*...determine 6th term and add its contribution...*/ acc = A_mac_ss(acc,TERM6_MULTIPLER,stmp2); /* acc = 1 + x/2 - 0.5*(x/2)^2 + 0.5*(x/2)^3 - 0.625*(x/2)^4 + 0.875*(x/2)^5 */ acc = A_shr_r_a(acc,16); #if DSP40_DEBUG_A_sqrt if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_sqrt(%.0f)= %.0f\n",sqrt_in,acc); #endif return(acc); } /*===========================================================================*/ /* Take the log base 2 of input and divide by 32 and return; */ /* i.e. output = log2(input)/32 */ /* */ /* log2(x) = 4.0 * (-.3372223*x*x + .9981958*x -.6626105) */ /* c0 c1 c2 (includes sign) */ /* */ /*---------------------------------------------------------------------------*/ Acc _A_log2 ( Acc log2_in ) { /*....(static) variables....*/ static int16 swC0 = -0x2b2a; static int16 swC1 = 0x7fc5; static int16 swC2 = -0x54d0; /*....(local) variables....*/ Acc acc; int32 shift; int16 swInSqrd; int16 swIn; int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_log2 if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_log2(%.0f):\n",log2_in); #endif #ifdef DSP_OP_COUNT DSP40_OP_COUNT(0); #endif /*...normalize input and store shifts required...*/ acc = log2_in; shift = I_norm_a(&acc,31); swIn = S_get_hi(acc); acc = A_set_l(shift); acc = A_add_as(acc,1); acc = -acc; shift = S_get_lo(acc); /*...calculate x*x*c0...*/ acc = A_mul_ss(swIn,swIn); swInSqrd = S_get_hi_r(acc); acc = A_mul_ss(swInSqrd,swC0); /*...add x*c1...*/ acc = A_mac_ss(acc,swIn,swC1); /*...add c2...*/ lval = L_get(acc); acc = A_set_hi(swC2); acc = A_add_al(acc,lval); /*...apply *(4/32)...*/ acc = A_shr_a(acc,3); lval = L_get(acc); lval = lval & (int32)0x03ffffff; #ifdef DSP_OP_COUNT DSP40_OP_COUNT(1); #endif acc = A_set_lo(shift); acc = A_shl_a(acc,26); acc = A_add_al(acc,lval); #if DSP40_DEBUG_A_log2 if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_log2(%.0f)= %.0f\n",log2_in,acc); #endif return(acc); } /*===========================================================================*/ /* Take the natural log of input and divide by 32 and return; */ /* i.e. output = log(input)/32 */ /* */ /* log(x) = log(2) * log2(x) */ /* = 0.693147 * log2(x) */ /* */ /* log2(x) = 4.0 * (-.3372223*x*x + .9981958*x -.6626105) */ /* c0 c1 c2 (includes sign) */ /*---------------------------------------------------------------------------*/ Acc _A_nlog ( Acc nlog_in ) { /*....(local) variables....*/ Acc acc; int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_nlog if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_nlog(%.0f):\n",nlog_in); #endif acc = A_log2_a(nlog_in); lval = L_get(acc); acc = A_mul_ls(lval,22713); /* 0.693147 = log(2) */ #if DSP40_DEBUG_A_nlog if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_nlog(%.0f)= %.0f\n",nlog_in,acc); #endif return(acc); } /*===========================================================================*/ /* Return log base 10 of input divided by 32; i.e. output = log10(input)/32 */ /* */ /* log10(x) = log10(2) * log2(x) */ /* = 0.30103 * log2(x) */ /* */ /* log2(x) = 4.0 * (-.3372223*x*x + .9981958*x -.6626105) */ /* c0 c1 c2 (includes sign) */ /*---------------------------------------------------------------------------*/ Acc _A_log10 ( Acc log10_in ) { /*....(local) variables....*/ Acc acc; int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_log10 if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_log10(%.0f):\n",log10_in); #endif acc = A_log2_a(log10_in); lval = L_get(acc); acc = A_mul_ls(lval,9864); #if DSP40_DEBUG_A_log10 if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_log10(%.0f)= %.0f\n",log10_in,acc); #endif return(acc); } /*===========================================================================*/ /* Base two exponential 2**(32*x) by polynomial approximation */ /* */ /* 2**(32*X) = 0.1713425*X*X + 0.6674432*X + 0.9979554 */ /* c2 c1 c0 */ /* */ /*---------------------------------------------------------------------------*/ Acc _A_exp2 ( Acc exp2_in /* (i) > 0 */ ) { /*....(static) variables....*/ static int16 pswPCoefE[3] = { /* c2, c1, c0 */ 0x15ef,0x556f,0x7fbd }; /*....(local) variables....*/ Acc acc; int16 stmp1; int16 stmp2; int16 stmp3; int16 stmp4; int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_exp2 if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_exp2(%.0f):\n",exp2_in); #endif #ifdef DSP_OP_COUNT DSP40_OP_COUNT(0); #endif /*...initialize...*/ stmp3 = 0x0020; /*...determine normlization shift count...*/ acc = exp2_in; stmp1 = S_get_hi(acc); acc = A_mul_ss(stmp1,stmp3); stmp2 = S_get_hi(acc); lval = L_get(acc); /*...determine un-normalized shift count...*/ stmp3 = -0x0001; acc = A_sub_ss(stmp3,stmp2); stmp4 = S_get_lo(acc); /*...normalize input...*/ lval = lval & (int32)0xffff; #ifdef DSP_OP_COUNT DSP40_OP_COUNT(1); #endif acc = A_set_hi(stmp3); acc = A_add_al(acc,lval); acc = A_shr_a(acc,1); stmp1 = S_get_lo(acc); /*...calculate x*x*c2...*/ acc = A_mul_ss(stmp1,stmp1); stmp2 = S_get_hi_r(acc); acc = A_mul_ss(stmp2,pswPCoefE[0]); /*...calculate x*x*c2 + x*c1...*/ acc = A_mac_ss(acc,stmp1,pswPCoefE[1]); lval = L_get(acc); /*...calculate x*x*c2 + x*c1 + c0...*/ acc = A_set_hi(pswPCoefE[2]); acc = A_add_al(acc,lval); /*...un-normalize exponent if its requires it...*/ if (stmp4 > 0) { acc = A_shr_a(acc,stmp4); } #if DSP40_DEBUG_A_exp2 if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_exp2(%.0f)= %.0f\n",exp2_in,acc); #endif return(acc); } /*===========================================================================*/ /* Base ten exponential 10**(32*x) by polynomial approximation */ /* */ /* 10**(32*X) = 2**((32*X) / log10(2)) */ /* = 2**((32*X) / 0.30103) */ /* */ /* 2**(32*X) = 0.1713425*X*X + 0.6674432*X + 0.9979554 */ /* c2 c1 c0 */ /*---------------------------------------------------------------------------*/ Acc _A_exp10 ( Acc exp10_in ) { /*....(local) variables....*/ Acc acc; int32 lval; /*....execute....*/ #if DSP40_DEBUG_A_exp10 if (acc40_debug & 0x1) fprintf(stderr,"dsp40lib: _A_exp10(%.0f):\n",exp10_in); #endif lval = L_get(exp10_in); acc = A_mul_ls(lval,27213); /* (1/log10(2))/4 */ acc = A_shl_a(acc,2); lval = L_get(acc); acc = A_exp2_l(lval); #if DSP40_DEBUG_A_exp10 if (acc40_debug & 0x2) fprintf(stderr,"dsp40lib: _A_exp10(%.0f)= %.0f\n",exp10_in,acc); #endif return(acc); }