fxpt.h

手机加密通话软件

/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
/* -*-C-*-
*******************************************************************************
*
* RCS:          $Id: fxpt.h,v 1.10 2003/11/22 16:34:28 itsme Exp $
* Description:  fixed point primitives
*
* (C) Copyright 1999, Starium Ltd., all rights reserved.
*
*******************************************************************************
*/

#ifndef __FXPT_H__
#define __FXPT_H__

#include "hal_types.h"

#ifdef _USE_INLINED_FXPT

// !! this cannot be 'static' - otherwise duplicate symbols
#define INLINE static inline
#define FXPT_INCLUDE_HEADER 1
//#define FXPT_INCLUDE_PROTOTYPES 1
#define FXPT_INCLUDE_IMPLEMENTATION 1
#else
#ifdef FXPT_FROM_FXPT_C 
#define INLINE
#define FXPT_INCLUDE_HEADER 1
#define FXPT_INCLUDE_PROTOTYPES 1
#define FXPT_INCLUDE_IMPLEMENTATION 1
#else
#define INLINE
#define FXPT_INCLUDE_HEADER 1
#define FXPT_INCLUDE_PROTOTYPES 1
//#define FXPT_INCLUDE_IMPLEMENTATION 1
#endif
#endif /* _USE_INLINED_FXPT */

#ifdef FXPT_INCLUDE_HEADER

typedef int16   fxpt_16;
typedef uint16  fxpt_u16;

typedef int32   fxpt_32;
typedef uint32  fxpt_u32;

typedef int64   fxpt_64;
typedef uint64  fxpt_u64;


/*
 *  Some fxpt32 notes:
 *
 *  FX32_MIN in decimal is -2147483648
 *  FX32_MAX in decimal is [phone]
 *  -FX32_MIN overflows
 *  -(FX32_MIN+1) in decimal is [phone] (FX32_MAX)
 *  -FX32_MAX in decimal is -2147483648 (FX32_MIN)
 */
#define FX32_SIGN (fxpt_32) 0x80000000       /* sign bit for big type */
#define FX32_MIN  (fxpt_32) 0x80000000
#define FX32_MAX  (fxpt_32) 0x7fffffff

#define FX16_SIGN (fxpt_16) 0x8000           /* sign bit for small type */
#define FX16_MIN  (fxpt_16) 0x8000
#define FX16_MAX  (fxpt_16) 0x7fff

/* convert floating point constant in the range [-1..1) to a fxpt_16 */
#define FXPT_FLOAT_TO_Q15(float_const) ((fxpt_16) ((float_const) * (FX16_MAX + 1)))
#define FXPT_FLOAT_TO_Q12(float_const) ((fxpt_16) ((double)(float_const) * (1 << 12)))

typedef fxpt_32 sine_acc_t;     /* sine accumulator     */
typedef fxpt_32 sine_incr_t;    /* sine phase increment */

#define SINE_INCR(freq, sample_rate) \
        ((sine_incr_t) ((64.0*1024*(freq)*64)/(sample_rate)))

#define ROUND_SINE_ACC(acc) \
        ((acc) >> 6)

/*
 * These are needed to deactivate debugging code in main program.
 * Debugging cannot be used without a special version of the fxpt
 * library outfitted with overflow checking code (not for production
 * use).
 */
#define FXPT_ASSERT(x,y)
#define FXPT_SET_WARNERROR(x)
#define FXPT_SET_PUNTERROR(x)
#define FXPT_PUSHSTATE(x,y,z)
#define FXPT_POPSTATE()

#endif  /* FXPT_INCLUDE_HEADER */

#ifdef FXPT_INCLUDE_PROTOTYPES

INLINE fxpt_16 fxpt_abs16 (fxpt_16 var1);
INLINE fxpt_32 fxpt_abs32 (fxpt_32 L_var1);
INLINE fxpt_32 fxpt_add32 (fxpt_32 a, fxpt_32 b);
INLINE fxpt_32 fxpt_add32_fast (fxpt_32 a, fxpt_32 b);
INLINE fxpt_32 fxpt_sub32 (fxpt_32 a, fxpt_32 b);
INLINE fxpt_32 fxpt_mult32 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_32 fxpt_mult32_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_32 fxpt_mult64_fix (fxpt_32 var1, fxpt_32 var2, int fix);
INLINE fxpt_32 fxpt_mult64_round (fxpt_32 var1, fxpt_32 var2, int fix);
INLINE fxpt_32 fxpt_mac32 (fxpt_32 L_var3, fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_32 fxpt_shl32 (fxpt_32 L_var1, int var2);
INLINE fxpt_32 fxpt_shl32_fast (fxpt_32 L_var1, int var2);
INLINE fxpt_32 fxpt_shr32 (fxpt_32 L_var1, int var2);
INLINE fxpt_32 fxpt_shr32_fast (fxpt_32 L_var1, int var2);
INLINE fxpt_16 fxpt_shl16 (fxpt_16 var1, int var2);
INLINE fxpt_16 fxpt_shl16_fast(fxpt_16 var1, int var2);
INLINE fxpt_16 fxpt_shr16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_shr16_fast(fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_shr16_round(fxpt_16 var1, int var2);
INLINE fxpt_16 fxpt_extract_h (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_extract_l (fxpt_32 L_var1);
INLINE fxpt_32 fxpt_negate32 (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_negate16 (fxpt_16 var1);
INLINE fxpt_16 fxpt_round16 (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_saturate16 (fxpt_32 L_var1);
INLINE fxpt_16 fxpt_saturate16_fix (fxpt_32 L_var1, int fix );
INLINE fxpt_16 fxpt_saturate16_round (fxpt_32 L_var1, int fix );
INLINE fxpt_32 fxpt_saturate32 (fxpt_64 LL_var1);
INLINE fxpt_32 fxpt_saturate32_round(fxpt_64 LL_var1, int fix);
INLINE fxpt_32 fxpt_deposit_h (fxpt_16 var1);
INLINE fxpt_32 fxpt_deposit_l (fxpt_16 var1);
INLINE fxpt_16 fxpt_mult_round16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult_round16_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16_fix (fxpt_16 var1, fxpt_16 var2, int fix);
INLINE fxpt_16 fxpt_mult16_fix_fast (fxpt_16 var1, fxpt_16 var2, int fix);
INLINE fxpt_16 fxpt_mult16_round (fxpt_16 var1, fxpt_16 var2, int fix);
INLINE fxpt_16 fxpt_square16 (fxpt_16 var1);
INLINE fxpt_16 fxpt_add16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_add16_fast (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_sub16 (fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_sub16_fast(fxpt_16 var1, fxpt_16 var2);
INLINE fxpt_16 fxpt_mult16_q15q11 (fxpt_16 a, fxpt_16 b);
INLINE fxpt_16 fxpt_div16 (fxpt_32 num, fxpt_32 den, int fr_bits);
INLINE fxpt_32 fxpt_sine32 (fxpt_32 x);
INLINE fxpt_16 fxpt_cos16 (fxpt_16 x);
INLINE fxpt_32 fxpt_cos32 (fxpt_32 x);
INLINE fxpt_32 fxpt_mult64_round_good (fxpt_32 var1, fxpt_32 var2, int fix);

#endif /* FXPT_INCLUDE_PROTOTYPES */

#ifdef FXPT_INCLUDE_IMPLEMENTATION

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_abs16
 *
 *   PURPOSE:
 *
 *     Take the absolute value of the 16 bit input.  An input of
 *     -0x8000 results in a return value of 0x7fff.
 *
 *   INPUTS:
 *
 *     var1
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var1 <= 0x0000 7fff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     swOut
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range
 *                     0x0000 0000 <= swOut <= 0x0000 7fff.
 *
 *   IMPLEMENTATION:
 *
 *     Take the absolute value of the 16 bit input.  An input of
 *     -0x8000 results in a return value of 0x7fff.
 *
 *   KEYWORDS: absolute value, abs
 *
 *************************************************************************/

INLINE fxpt_16
fxpt_abs16 (fxpt_16 var1)
{
  if (var1 < 0) {
    if (var1 == FX16_MIN)
      var1 = FX16_MAX;
    else
      var1 = -var1;
  }

  return var1;
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_abs32
 *
 *   PURPOSE:
 *
 *     Take the absolute value of the 32 bit input.  An input of
 *     -0x8000 0000 results in a return value of 0x7fff ffff.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *
 *
 *   KEYWORDS: absolute value, abs
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_abs32 (fxpt_32 L_var1)
{
  if (L_var1 < 0) {
    if (L_var1 == FX32_MIN)
      L_var1 = FX32_MAX;
    else
      L_var1 = -L_var1;
  }

  return L_var1;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_add32
 *
 *   PURPOSE:
 *
 *     Perform the addition of the two 32 bit input variables with
 *     saturation.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *     L_var2
 *                     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var2 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (Longword) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the addition of the two 32 bit input variables with
 *     saturation.
 *
 *     L_Out = L_var1 + L_var2
 *
 *     L_Out is set to 0x7fff ffff if the operation results in an
 *     overflow.  L_Out is set to 0x8000 0000 if the operation
 *     results in an underflow.
 *
 *   KEYWORDS: add, addition
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_add32 (fxpt_32 a, fxpt_32 b)
{
  fxpt_32 sum;

  sum = a + b;

  if ((a ^ b) > 0  &&  (a ^ sum) < 0) {
    if (b > 0)
      sum = FX32_MAX;
    else
      sum = FX32_MIN;
  }

  return sum;
}

INLINE fxpt_32
fxpt_add32_fast (fxpt_32 a, fxpt_32 b)
{
  return a + b;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_sub32
 *
 *   PURPOSE:
 *
 *     Perform the subtraction of the two 32 bit input variables with
 *     saturation.
 *
 *   INPUTS:
 *
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *     L_var2
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var2 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Perform the subtraction of the two 32 bit input variables with
 *     saturation.
 *
 *     L_Out = L_var1 - L_var2
 *
 *     L_Out is set to 0x7fff ffff if the operation results in an
 *     overflow.  L_Out is set to 0x8000 0000 if the operation
 *     results in an underflow.
 *
 *   KEYWORDS: sub, subtraction
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_sub32 (fxpt_32 a, fxpt_32 b)
{
  fxpt_32 diff;

  diff = a - b;

  if ((a ^ b) < 0  &&  (a ^ diff) < 0) {
    if (b < 0)
      diff = FX32_MAX;
    else
      diff = FX32_MIN;
  }

  return diff;
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_mult32
 *
 *   PURPOSE:
 *
 *     Perform a fractional multipy of the two 16 bit input numbers
 *     with saturation.  Output a 32 bit number.
 *
 *   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:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Multiply the two the two 16 bit input numbers. If the
 *     result is within this range, left shift the result by one
 *     and output the 32 bit number.  The only possible overflow
 *     occurs when var1==var2==-0x8000.  In this case output
 *     0x7fff ffff.
 *
 *   KEYWORDS: multiply, mult, mpy
 *
 *************************************************************************/

INLINE fxpt_32
fxpt_mult32 (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_product;

  if (var1 == FX16_MIN && var2 == FX16_MIN)
    L_product = FX32_MAX;                 /* overflow */
  else
  {
    L_product = (fxpt_32) var1 * var2; /* integer multiply */
    L_product = L_product << 1;
  }
  return (L_product);

}


/*
 *  Same as above without overflow checking.
 */
INLINE fxpt_32
fxpt_mult32_fast (fxpt_16 var1, fxpt_16 var2)
{
  fxpt_32 L_product = (var1 * var2);
  L_product <<= 1;
  return L_product;
}


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

  LL_product = ((fxpt_64) var1 * var2);

  if( fix==0 )
      return (fxpt_32)LL_product;

  if( fix>0 )
  {
      if (LL_product >= 0)
        L_product = (fxpt_32)(LL_product >> fix);
      else {
        L_product = (fxpt_32)((-LL_product) >> fix);
        L_product = -L_product;
      }
  }
  else
  {
      if (LL_product >= 0)
        L_product = ((fxpt_32)LL_product) << (-fix);
      else {
        L_product = ((fxpt_32)(-LL_product)) << (-fix);
        L_product = -L_product;
      }
  }

  return L_product;
}

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

  LL_product = ((fxpt_64) var1 * var2);

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

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

  if( fix-1>0 )
      L_product = (fxpt_32) (LL_product >> (fix - 1));
  else
      L_product = (fxpt_32) (LL_product << (-fix + 1));
  /* Round */
  L_product++;
  L_product >>= 1;

  if (sign)
    return -L_product;
  else
    return L_product;
}

/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_mac32
 *
 *   PURPOSE:
 *
 *     Multiply accumulate.  Fractionally multiply two 16 bit
 *     numbers together with saturation.  Add that result to the
 *     32 bit input with saturation.  Return the 32 bit result.
 *
 *   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.
 *     L_var3
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var2 <= 0x7fff ffff.
 *
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE:
 *
 *     L_Out
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *
 *   IMPLEMENTATION:
 *
 *     Fractionally multiply two 16 bit numbers together with
 *     saturation.  The only numbers which will cause saturation on
 *     the multiply are 0x8000 * 0x8000.
 *
 *     Add that result to the 32 bit input with saturation.
 *     Return the 32 bit result.
 *
 *     Please note that this is not a true multiply accumulate as
 *     most processors would implement it.  The 0x8000*0x8000
 *     causes and overflow for this instruction.  On most
 *     processors this would cause an overflow only if the 32 bit
 *     input added to it were positive or zero.
 *
 *   KEYWORDS: mac, multiply accumulate
 *
 *************************************************************************/
INLINE fxpt_32
fxpt_mac32 (fxpt_32 L_var3, fxpt_16 var1, fxpt_16 var2)
{
  return (fxpt_add32 (L_var3, fxpt_mult32 (var1, var2)));
}


/***************************************************************************
 *
 *   FUNCTION NAME: fxpt_shl32
 *
 *   PURPOSE:
 *
 *     Arithmetic shift left (or right).
 *     Arithmetically shift the input left by var2.   If var2 is
 *     negative then an arithmetic shift right (fxpt_shr32) of L_var1 by
 *     -var2 is performed.
 *
 *   INPUTS:
 *
 *     var2
 *                     16 bit short signed integer (fxpt_16) whose value
 *                     falls in the range 0xffff 8000 <= var2 <= 0x0000 7fff.
 *     L_var1
 *                     32 bit long signed integer (fxpt_32) whose value
 *                     falls in the range
 *                     0x8000 0000 <= L_var1 <= 0x7fff ffff.
 *   OUTPUTS:
 *
 *     none
 *
 *   RETURN VALUE: