source: svn/trunk/newcon3bcm2_21bu/dta/src/settop_api/avl_dspfunc.c

/* $Id: dspfunc.c 1.4 2006/12/14 18:07:01 rzopf Exp $ */
/*****************************************************************************
  Copyright 2004 Broadcom Corp.  All Rights Reserved.

  $Log: dspfunc.c $
  Revision 1.4  2006/12/14 18:07:01  rzopf
  *** empty log message ***
 
  Revision 1.3  2006/02/28 21:35:02  rzopf
  modified frame size related code to use a variable instead of a constant.

  Revision 1.2  2006/02/27 21:01:44  rzopf
  Replaced short and long data types with Word16 and Word32
 
  Revision 1.1  2006/02/27 17:07:33  rzopf
  Initial Version.
 
 
******************************************************************************/

#include "avl_dspfunc.h"

/* following is from fixmath.c */

/*-----------------------------------------------------*
 | Table for routine Pow2().                           |
  -----------------------------------------------------*/

Word16 bvxx_tabpow[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 };

/*-----------------------------------------------------*
 *  | Table for routine Log2().                           |
 *   -----------------------------------------------------*/

Word16 bvxx_tablog[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 };

/*___________________________________________________________________________
 |                                                                           |
 |   Function Name : Pow2()                                                  |
 |                                                                           |
 |     L_x = pow(2.0, exponent.fraction)                                     |
 |---------------------------------------------------------------------------|
 |  Algorithm:                                                               |
 |                                                                           |
 |   The function Pow2(L_x) is approximated by a table and linear            |
 |   interpolation.                                                          |
 |                                                                           |
 |   1- i = bit10-b15 of fraction,   0 <= i <= 31                            |
 |   2- a = bit0-b9   of fraction                                            |
 |   3- L_x = tabpow[i]<<16 - (tabpow[i] - tabpow[i+1]) * a * 2              |
 |   4- L_x = L_x >> (30-exponent)     (with rounding)                       |
 |___________________________________________________________________________|
*/

Word32 bvxx_Pow2(   /* (o) Q0  : result       (range: 0<=val<=0x7fffffff) */
  Word16 exponent,  /* (i) Q0  : Integer part.      (range: 0<=val<=30)   */
  Word16 fraction   /* (i) Q15 : Fractional part.   (range: 0.0<=val<1.0) */
)
{
  Word16 exp, i, a, tmp;
  Word32 L_x;

  L_x = ((Word32)fraction)<<6;          /* L_x = fraction<<6           */
  i   = (Word16)EXTRACT_HI(L_x);                 /* Extract b10-b15 of fraction */
  L_x = L_x>>1;
  a   = (Word16)L_x;                 /* Extract b0-b9   of fraction */
  a   = a & (Word16)0x7fff;

  L_x = ((Word32)bvxx_tabpow[i])<<16;    /* tabpow[i] << 16        */
  tmp = bvxx_tabpow[i]-bvxx_tabpow[i+1]; /* tabpow[i] - tabpow[i+1] */
  L_x -= (((Word32)tmp)*a)<<1;             /* L_x -= tmp*a*2        */

  exp = 30-exponent;
  L_x = L_x>>exp;

  return(L_x);
}


/*___________________________________________________________________________
 |                                                                           |
 |   Function Name : Log2()                                                  |
 |                                                                           |
 |       Compute log2(L_x).                                                  |
 |       L_x is positive.                                                    |
 |                                                                           |
 |       if L_x is negative or zero, result is 0.                            |
 |---------------------------------------------------------------------------|
 |  Algorithm:                                                               |
 |                                                                           |
 |   The function Log2(L_x) is approximated by a table and linear            |
 |   interpolation.                                                          |
 |                                                                           |
 |   1- Normalization of L_x.                                                |
 |   2- exponent = 30-exponent                                               |
 |   3- i = bit25-b31 of L_x,    32 <= i <= 63  ->because of normalization.  |
 |   4- a = bit10-b24                                                        |
 |   5- i -=32                                                               |
 |   6- fraction = tablog[i]<<16 - (tablog[i] - tablog[i+1]) * a * 2         |
 |___________________________________________________________________________|
*/ 

void bvxx_Log2(
  Word32 L_x,       /* (i) Q0 : input value                                 */
  Word16 *exponent, /* (o) Q0 : Integer part of Log2.   (range: 0<=val<=30) */
  Word16 *fraction  /* (o) Q15: Fractional  part of Log2. (range: 0<=val<1) */
)
{
  Word16 exp, i, a, tmp;
  Word32 L_y;

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

  exp = (Word16)Norm(L_x);
  L_x <<= exp;               /* L_x is normalized */

  *exponent = 30-exp;

  L_x = L_x>>9;
  i   = (Word16)EXTRACT_HI(L_x);                 /* Extract b25-b31 */
  L_x = L_x>>1;
  a   = (Word16)L_x;                 /* Extract b10-b24 of fraction */
  a   = a & (Word16)0x7fff;

  i   = i-32;

  L_y = ((Word32)(bvxx_tablog[i]))<<16;    /* tablog[i] << 16        */
  tmp = bvxx_tablog[i]-bvxx_tablog[i+1];  /* tablog[i] - tablog[i+1] */
  L_y -= (((Word32)tmp)*a)<<1;             /* L_y -= tmp*a*2        */

  *fraction = (Word16) EXTRACT_HI( L_y);

  return;
}

#define  PWR_SHIFT      9
void CalcPower(short *inbuf, Word32 *power, Word32 *shift, Word32 frsz)
{
   int      i;
   long     acc;
   long     prdct;
   long     lshift;

   acc = 0;
   for (i=0;i<frsz;i++)
   {
      prdct = ((Word32)inbuf[i])*inbuf[i];
      prdct >>= PWR_SHIFT;
      acc   += prdct;
   }
   lshift = Norm(acc);
   lshift = PWR_SHIFT+2-lshift;
   if (lshift<0)
      lshift=0;

   if (lshift&1)
      lshift++;

   acc = 0;
   for (i=0;i<frsz;i++)
   {
      prdct = ((Word32)inbuf[i])*inbuf[i];
      prdct >>= lshift;
      acc   += prdct;
   }

   *shift = lshift;
   *power = acc;
}

Word16 Norm(Word32 in)
{
   Word16 shift;

   shift=0;
   if (in > 0)
   {

      while(in< (((Word32)1)<<(30)))
      {
         in<<=1;
         shift++;
      }
   }
   else if (in < 0)
   {
      while(in>= (((Word32)-1)<<(30)))
      {
         in<<=1;
         shift++;
      }
   }
   return(shift);
}

Word16 Norm16(Word16 in)
{
   Word16 shift;

   shift=0;
   if (in > 0)
   {

      while(in< (((Word16)1)<<(14)))
      {
         in<<=1;
         shift++;
      }
   }
   else if (in < 0)
   {
      while(in>= (((Word16)-1)<<(14)))
      {
         in<<=1;
         shift++;
      }
   }
   return(shift);
}
/*___________________________________________________________________________
 |                                                                           |
 |   Function Name : div_s                                                   |
 |                                                                           |
 |   Purpose :                                                               |
 |                                                                           |
 |   Produces a result which is the fractional integer division of var1  by  |
 |   var2; var1 and var2 must be positive and var2 must be greater or equal  |
 |   to var1; the result is positive (leading bit equal to 0) and truncated  |
 |   to 16 bits.                                                             |
 |   If var1 = var2 then div(var1,var2) = 32767.                             |
 |                                                                           |
 |   Complexity weight : 18                                                  |
 |                                                                           |
 |   Inputs :                                                                |
 |                                                                           |
 |    var1                                                                   |
 |             16 bit short signed integer (Word16) whose value falls in the |
 |             range : 0x0000 0000 <= var1 <= var2 and var2 != 0.            |
 |                                                                           |
 |    var2                                                                   |
 |             16 bit short signed integer (Word16) whose value falls in the |
 |             range : var1 <= var2 <= 0x0000 7fff and var2 != 0.            |
 |                                                                           |
 |   Outputs :                                                               |
 |                                                                           |
 |    none                                                                   |
 |                                                                           |
 |   Return Value :                                                          |
 |                                                                           |
 |    var_out                                                                |
 |             16 bit short signed integer (Word16) whose value falls in the |
 |             range : 0x0000 0000 <= var_out <= 0x0000 7fff.                |
 |             It's a Q15 value (point between b15 and b14).                 |
 |___________________________________________________________________________|
*/
Word16 div_s (Word16 var1, Word16 var2)
{
    Word16 var_out = 0;
    Word16 iteration;
    Word32 L_num;
    Word32 L_denom;

    if (var1 == 0)
    {
        var_out = 0;
    }
    else
    {
        if (var1 == var2)
        {
            var_out = 32767;
        }
        else
        {
            L_num = var1;
            L_denom = var2;
            for (iteration = 0; iteration < 15; iteration++)
            {
                var_out <<= 1;
                L_num <<= 1;

                if (L_num >= L_denom)
                {
                    L_num = L_num-L_denom;
                    var_out += 1;
                }
            }
        }
    }
    return (var_out);
}
/* ------------------------- End of div_s() ------------------------- */

/* assume b < a */
Word16 imod(Word16 a, Word16 b, Word16 *dividend)
{
   Word16 t1, t2, t3, t4, t5;

   t1 = Norm16(a);
   t1 --;
   t2 = a<<t1;
   t3 = Norm16(b);
   t4 = b<<t3;
   t5 = div_s(t2, t4);
   t5 = t5>>(15+t1-t3);
   t1 = a - b*t5;

   *dividend = t5;
   return(t1);
}