fxpt.h

手机加密通话软件

  fxpt_16 swOut = (fxpt_16) L_var1;

  if( swOut != L_var1 )
    swOut = (L_var1 < 0) ? FX16_MIN : FX16_MAX;

  return swOut;
}

INLINE fxpt_16
fxpt_saturate16_fix (fxpt_32 L_var1, int fix )
{
    fxpt_32 tmp;
    fxpt_16 swOut;

    if( L_var1>=0 )
        tmp = (L_var1>>fix);
    else
        tmp= -((-L_var1)>>fix);

  swOut = (fxpt_16) tmp;

  if( swOut != tmp )
    swOut = (tmp < 0) ? FX16_MIN : FX16_MAX;

  return swOut;
}

INLINE fxpt_16
fxpt_saturate16_round (fxpt_32 L_var1, int fix )
{
    fxpt_16 swOut;

    if( L_var1>=0 )
        L_var1 = ((L_var1>>(fix-1))+1)>>1;
    else
        L_var1= -((((-L_var1)>>(fix-1))+1)>>1);

  swOut = (fxpt_16) L_var1;

  if( swOut != L_var1 )
    swOut = (L_var1 < 0) ? FX16_MIN : FX16_MAX;

  return swOut;
}

INLINE fxpt_32
fxpt_saturate32 (fxpt_64 LL_var1)
{
  fxpt_32 swOut = (fxpt_32) LL_var1;

  if( swOut != LL_var1 )
    swOut = (LL_var1 < 0) ? FX32_MIN : FX32_MAX;

  return swOut;
}

INLINE fxpt_32
fxpt_saturate32_round(fxpt_64 LL_var1, int fix)
{
    fxpt_32 swOut;

    if( LL_var1>=0 )
        LL_var1 = ((LL_var1>>(fix-1))+1)>>1;
    else
        LL_var1= -((((-LL_var1)>>(fix-1))+1)>>1);
    
    swOut = (fxpt_32) LL_var1;

  if( swOut != LL_var1 )
    swOut = (LL_var1 < 0) ? FX32_MIN : FX32_MAX;

  return swOut;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_deposit_h
 *
 *   PURPOSE:
 *
 *     Put the 16 bit input into the 16 MSB's of the output fxpt_32.  The
 *     LS 16 bits are zeroed.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff 0000.
 *
 *
 *   KEYWORDS: deposit, assign, fractional assign
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_deposit_h (fxpt_16 var1)
{
  fxpt_32 L_var2;

  L_var2 = (fxpt_32) var1 << 16;
  return (L_var2);
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_deposit_l
 *
 *   PURPOSE:
 *
 *     Put the 16 bit input into the 16 LSB's of the output fxpt_32 with
 *     sign extension i.e. the top 16 bits are set to either 0 or 0xffff.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0xffff 8000 <= L_var1 <= 0x0000 7fff.
 *
 *   KEYWORDS: deposit, assign
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_deposit_l (fxpt_16 var1)
{
  return (fxpt_32)var1;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_mult_round16
 *
 *   PURPOSE:
 *
 *     Perform a fractional multipy and round of the two 16 bit
 *     input numbers with saturation.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     This routine is defined as the concatenation of the multiply
 *     operation and the round operation.
 *
 *     The fractional multiply (L_mult) produces a saturated 32 bit
 *     output.  This is followed by a an add of 0x0000 8000 to the
 *     32 bit result.  The result may overflow due to the add.  If
 *     so, the result is saturated.  The 32 bit rounded number is
 *     then shifted down 16 bits and returned as a fxpt_16.
 *
 *
 *   KEYWORDS: multiply and round, round, fxpt_mult_r16, mpyr
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_mult_round16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_16 swOut;

  swOut = fxpt_round16 (fxpt_mult32 (var1, var2));
  return (swOut);
}

/*
 *  Same as above without overflow checking.
 */
INLINE fxpt_16
fxpt_mult_round16_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 lw = (var1 * var2) + 0x4000;
  lw >>= 15;
  return (fxpt_16)lw;
}

/*
 * multiply, no rounding
 */
INLINE fxpt_16
fxpt_mult16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_16 swOut;

  swOut = fxpt_extract_h (fxpt_mult32 (var1, var2));
  return (swOut);
}

/*
 * multiply, no overflow check, no rounding
 */
#ifdef DORSEY
INLINE fxpt_16
fxpt_mult16_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 lw = (var1 * var2);

  /* When you right shift -1, it stays -1.  This is problematic
   * in many situations, so we modify the behavior so that a -1
   * right shifted becomes a 0 instead.
   */
  if (lw >= 0)
    lw >>= 15;
  else {
    lw = -lw >> 15;
    lw = -lw;
  }
  return (fxpt_16)lw;
}
#else

INLINE fxpt_16
fxpt_mult16_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 lw = (var1 * var2);

  lw >>= 15;
  return (fxpt_16)lw;
}

#endif

/*
 * multiply, custom radix
 */
INLINE fxpt_16
fxpt_mult16_fix (fxpt_16 var1, fxpt_16 var2, int fix)
{
  fxpt_32 lw = (var1 * var2);

  /* When you right shift -1, it stays -1.  This is problematic
   * in many situations, so we modify the behavior so that a -1
   * right shifted becomes a 0 instead.
   */
  if (lw >= 0)
    lw >>= fix;
  else {
    lw = -lw >> fix;
    lw = -lw;
  }

  return (fxpt_16)lw;
}

INLINE fxpt_16
fxpt_mult16_fix_fast (fxpt_16 var1, fxpt_16 var2, int fix)
{
  fxpt_32 lw = (var1 * var2);

  /* When you right shift -1, it stays -1.  This is problematic
   * in many situations, so we modify the behavior so that a -1
   * right shifted becomes a 0 instead.
   */
  if (lw >= 0)
    lw >>= fix;
  else {
    lw = -lw >> fix;
    lw = -lw;
  }

  return (fxpt_16)lw;
}

/*
 * multiply with rounding, custom radix
 */
#if 0
INLINE fxpt_16
fxpt_mult16_round (fxpt_16 var1, fxpt_16 var2, fxpt_16 fix)
{
  int sign;
  fxpt_32 lw = (var1 * var2);

  if (fix == 0) {
    return lw;
  }

  /* Note sign */
  if (lw < 0) {
    sign = -1;
    lw = -lw;
  }
  else
    sign = 0;

  lw >>= (fix - 1);
  /* Round */
  lw++;
  lw >>= 1;

  /* Sign extend if necessary */
  if (sign)
    return (fxpt_16)(-lw);
  else
    return (fxpt_16)lw;
}
#endif

INLINE fxpt_16
fxpt_mult16_round (fxpt_16 var1, fxpt_16 var2, int fix)
{
  fxpt_32 lw = (var1 * var2);

  if (fix == 0) {
    return (fxpt_16)lw;
  }

  if (lw<0) {
    lw = - ( (-lw + (1<<(fix-1))) >> fix);
  }
  else {
    lw = (lw + (1<<(fix-1))) >> fix;
  }

  if (lw> FX16_MAX )
      lw= FX16_MAX;
  else if( lw< FX16_MIN)
      lw= FX16_MIN;

  return (fxpt_16)lw;
}

INLINE fxpt_16
fxpt_square16 (fxpt_16 var1)
{
  if (var1 == FX16_MIN)
    return FX16_MAX;

  return fxpt_mult16_fast (var1, var1);
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_add16
 *
 *   PURPOSE:
 *
 *     Perform the addition of the two 16 bit input variable with
 *     saturation.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the addition of the two 16 bit input variable with
 *     saturation.
 *
 *     swOut = var1 + var2
 *
 *     swOut is set to 0x7fff if the operation results in an
 *     overflow.  swOut is set to 0x8000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: add, addition
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_add16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_sum;
  fxpt_16 swOut;

  L_sum = (fxpt_32) var1 + var2;
  swOut = fxpt_saturate16 (L_sum);
  return (swOut);
}

/*
 *  Same as above w/o saturation.
 */
INLINE fxpt_16
fxpt_add16_fast (fxpt_16 var1, fxpt_16 var2)
{
  return var1 + var2;
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_sub16
 *
 *   PURPOSE:
 *
 *     Perform the subtraction of the two 16 bit input variable with
 *     saturation.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0xffff 8000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the subtraction of the two 16 bit input variable with
 *     saturation.
 *
 *     swOut = var1 - var2
 *
 *     swOut is set to 0x7fff if the operation results in an
 *     overflow.  swOut is set to 0x8000 if the operation results
 *     in an underflow.
 *
 *   KEYWORDS: sub, subtraction
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_sub16 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_diff;
  fxpt_16 swOut;

  L_diff = (fxpt_32) var1 - var2;
  swOut = fxpt_saturate16 (L_diff);

  return (swOut);
}

/*
 *  Same as above w/o saturation.
 */
INLINE fxpt_16
fxpt_sub16_fast(fxpt_16 var1, fxpt_16 var2)
{
  return var1 - var2;
}

/*
 * non standard radix point routines.
 */

INLINE fxpt_16
fxpt_mult16_q15q11 (fxpt_16 a, fxpt_16 b)
{
  fxpt_32       product;

  product = fxpt_mult32_fast (a, b);
  product = fxpt_shl32_fast (product, 4);
  return fxpt_extract_h (product);
}

/*
 * ----------------------------------------------------------------
 *                      non-inline functions (mostly)
 * ----------------------------------------------------------------
 */

fxpt_32 fxpt_div32 (fxpt_32 L_num, fxpt_32 L_den, int fr_bits);

/*
 * Divide two 32-bit values and store the result in a 16-bit
 * value with the specified number of fractional bits.
 */
INLINE fxpt_16
fxpt_div16 (fxpt_32 num, fxpt_32 den, int fr_bits)
{
  return fxpt_saturate16(fxpt_div32(num, den, fr_bits));
}

/*
 * given FREQ and SAMPLE_RATE, compute the sine_incr value
 * to use to generate a sine wave for freq FREQ.  This is
 * typically used with the SINE_INCR and ROUND_SINE_ACC macros to
 * generate sine waves.
 */
sine_incr_t fxpt_compute_sine_incr (int freq, int sample_rate);

fxpt_16 fxpt_sine16 (fxpt_16 x);
fxpt_32 fxpt_sine32 (fxpt_32 x);

/*
 * The following implementation is BAAAAAAD:
INLINE fxpt_32
fxpt_sine32 (fxpt_32 x)
{
  return fxpt_deposit_h(fxpt_sine16(fxpt_extract_h(x)));
}
 */

/*
 * The cos(omega) is equal to the sin(omega + 0.5) if omega is
 * measured in gradians.  Fortunately, twos complement numbers
 * overflow in precisely the correct behavior for this to
 * work properly with fixed point numbers in 0.15 format.
 */
INLINE fxpt_16
fxpt_cos16 (fxpt_16 x)
{
  return fxpt_sine16(x + 16384);
}

INLINE fxpt_32
fxpt_cos32 (fxpt_32 x)
{
  return fxpt_sine32(x + (16384 << 16));
}

/*
 * Compute the integer square root of x.
 * The value returned is the smallest y such that y*y <= x
 */
int16 integer_sqrt (int32 x);

/*
 * compute the fixed point square root of x.
 */
fxpt_16 fxpt_sqrt16 (fxpt_16 x);

/*
 * compute the log base 10 of x, where x is an integer 0 .. 32767
 * return the result as a Q12 number, e.g., 4.12
 *
 * Note, fxpt_log10_q12 (0) is defined as 0, not -32768
 */
fxpt_16 fxpt_log10_q12 (fxpt_16 x);

/*
 *  32*32 bit multiply with rounding and custom radix
 */
INLINE fxpt_32
fxpt_mult64_round_good (fxpt_32 var1, fxpt_32 var2, int fix)
{
  fxpt_64 LL_product;

  LL_product = ((fxpt_64) var1 * var2);

  if (fix == 0) {
    return fxpt_saturate32( LL_product );
  }

  return fxpt_saturate32_round( LL_product, fix );
}

#endif  /*  FXPT_INCLUDE_IMPLEMENTATION */
#endif /* _FXPT_H_ */