k60-keil

K60-Keil版本（下载安装MDK4.23）

   */
  static __INLINE q7_t clip_q31_to_q7(
				      q31_t x)
  {
    return ((q31_t) (x >> 24) != ((q31_t) x >> 23)) ?
      ((0x7F ^ ((q7_t) (x >> 31)))) : (q7_t) x;
  }

  /**
   * @brief Clips Q31 to Q15 values.
   */
  static __INLINE q15_t clip_q31_to_q15(
					q31_t x)
  {
    return ((q31_t) (x >> 16) != ((q31_t) x >> 15)) ?
      ((0x7FFF ^ ((q15_t) (x >> 31)))) : (q15_t) x;
  }

  /**
   * @brief Multiplies 32 X 64 and returns 32 bit result in 2.30 format.
   */

  static __INLINE q63_t mult32x64(
				  q63_t x,
				  q31_t y)
  {
    return ((((q63_t) (x & 0x00000000FFFFFFFF) * y) >> 32) +
            (((q63_t) (x >> 32) * y)));
  }


#if defined (ARM_MATH_CM0) && defined ( __CC_ARM   )
#define __CLZ __clz
#endif 

#if defined (ARM_MATH_CM0) && ((defined (__ICCARM__)) ||(defined (__GNUC__)) || defined (__TASKING__) )

  static __INLINE  uint32_t __CLZ(q31_t data);


  static __INLINE uint32_t __CLZ(q31_t data)
  {
	  uint32_t count = 0;
	  uint32_t mask = 0x80000000;

	  while((data & mask) ==  0)
	  {
		  count += 1u;
		  mask = mask >> 1u;
	  }

	  return(count);

  }

#endif 

  /**
   * @brief Function to Calculates 1/in(reciprocal) value of Q31 Data type.
   */

  static __INLINE uint32_t arm_recip_q31(
					 q31_t in,
					 q31_t * dst,
					 q31_t * pRecipTable)
  {

    uint32_t out, tempVal;
    uint32_t index, i;
    uint32_t signBits;

    if(in > 0)
      {
	signBits = __CLZ(in) - 1;
      }
    else
      {
	signBits = __CLZ(-in) - 1;
      }

    /* Convert input sample to 1.31 format */
    in = in << signBits;

    /* calculation of index for initial approximated Val */
    index = (uint32_t) (in >> 24u);
    index = (index & INDEX_MASK);

    /* 1.31 with exp 1 */
    out = pRecipTable[index];

    /* calculation of reciprocal value */
    /* running approximation for two iterations */
    for (i = 0u; i < 2u; i++)
      {
	tempVal = (q31_t) (((q63_t) in * out) >> 31u);
	tempVal = 0x7FFFFFFF - tempVal;
	/*      1.31 with exp 1 */
	//out = (q31_t) (((q63_t) out * tempVal) >> 30u);
	out = (q31_t) clip_q63_to_q31(((q63_t) out * tempVal) >> 30u);
      }

    /* write output */
    *dst = out;

    /* return num of signbits of out = 1/in value */
    return (signBits + 1u);

  }

  /**
   * @brief Function to Calculates 1/in(reciprocal) value of Q15 Data type.
   */
  static __INLINE uint32_t arm_recip_q15(
					 q15_t in,
					 q15_t * dst,
					 q15_t * pRecipTable)
  {

    uint32_t out = 0, tempVal = 0;
    uint32_t index = 0, i = 0;
    uint32_t signBits = 0;

    if(in > 0)
      {
	signBits = __CLZ(in) - 17;
      }
    else
      {
	signBits = __CLZ(-in) - 17;
      }

    /* Convert input sample to 1.15 format */
    in = in << signBits;

    /* calculation of index for initial approximated Val */
    index = in >> 8;
    index = (index & INDEX_MASK);

    /*      1.15 with exp 1  */
    out = pRecipTable[index];

    /* calculation of reciprocal value */
    /* running approximation for two iterations */
    for (i = 0; i < 2; i++)
      {
	tempVal = (q15_t) (((q31_t) in * out) >> 15);
	tempVal = 0x7FFF - tempVal;
	/*      1.15 with exp 1 */
	out = (q15_t) (((q31_t) out * tempVal) >> 14);
      }

    /* write output */
    *dst = out;

    /* return num of signbits of out = 1/in value */
    return (signBits + 1);

  }


  /*
   * @brief C custom defined intrinisic function for only M0 processors
   */
#if defined(ARM_MATH_CM0)

  static __INLINE q31_t __SSAT(
			       q31_t x,
			       uint32_t y)
  {
    int32_t posMax, negMin;
    uint32_t i;

    posMax = 1;
    for (i = 0; i < (y - 1); i++)
      {
	posMax = posMax * 2;
      }

    if(x > 0)
      {
	posMax = (posMax - 1);

	if(x > posMax)
	  {
	    x = posMax;
	  }
      }
    else
      {
	negMin = -posMax;

	if(x < negMin)
	  {
	    x = negMin;
	  }
      }
    return (x);


  }

#endif /* end of ARM_MATH_CM0 */



  /*
   * @brief C custom defined intrinsic function for M3 and M0 processors
   */
#if defined (ARM_MATH_CM3) || defined (ARM_MATH_CM0)

  /*
   * @brief C custom defined QADD8 for M3 and M0 processors
   */
  static __INLINE q31_t __QADD8(
				q31_t x,
				q31_t y)
  {

    q31_t sum;
    q7_t r, s, t, u;

    r = (char) x;
    s = (char) y;

    r = __SSAT((q31_t) (r + s), 8);
    s = __SSAT(((q31_t) (((x << 16) >> 24) + ((y << 16) >> 24))), 8);
    t = __SSAT(((q31_t) (((x << 8) >> 24) + ((y << 8) >> 24))), 8);
    u = __SSAT(((q31_t) ((x >> 24) + (y >> 24))), 8);

    sum = (((q31_t) u << 24) & 0xFF000000) | (((q31_t) t << 16) & 0x00FF0000) |
      (((q31_t) s << 8) & 0x0000FF00) | (r & 0x000000FF);

    return sum;

  }

  /*
   * @brief C custom defined QSUB8 for M3 and M0 processors
   */
  static __INLINE q31_t __QSUB8(
				q31_t x,
				q31_t y)
  {

    q31_t sum;
    q31_t r, s, t, u;

    r = (char) x;
    s = (char) y;

    r = __SSAT((r - s), 8);
    s = __SSAT(((q31_t) (((x << 16) >> 24) - ((y << 16) >> 24))), 8) << 8;
    t = __SSAT(((q31_t) (((x << 8) >> 24) - ((y << 8) >> 24))), 8) << 16;
    u = __SSAT(((q31_t) ((x >> 24) - (y >> 24))), 8) << 24;

    sum =
      (u & 0xFF000000) | (t & 0x00FF0000) | (s & 0x0000FF00) | (r & 0x000000FF);

    return sum;
  }

  /*
   * @brief C custom defined QADD16 for M3 and M0 processors
   */

  /*
   * @brief C custom defined QADD16 for M3 and M0 processors
   */
  static __INLINE q31_t __QADD16(
				 q31_t x,
				 q31_t y)
  {

    q31_t sum;
    q31_t r, s;

    r = (short) x;
    s = (short) y;

    r = __SSAT(r + s, 16);
    s = __SSAT(((q31_t) ((x >> 16) + (y >> 16))), 16) << 16;

    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

    return sum;

  }

  /*
   * @brief C custom defined SHADD16 for M3 and M0 processors
   */
  static __INLINE q31_t __SHADD16(
				  q31_t x,
				  q31_t y)
  {

    q31_t sum;
    q31_t r, s;

    r = (short) x;
    s = (short) y;

    r = ((r >> 1) + (s >> 1));
    s = ((q31_t) ((x >> 17) + (y >> 17))) << 16;

    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

    return sum;

  }

  /*
   * @brief C custom defined QSUB16 for M3 and M0 processors
   */
  static __INLINE q31_t __QSUB16(
				 q31_t x,
				 q31_t y)
  {

    q31_t sum;
    q31_t r, s;

    r = (short) x;
    s = (short) y;

    r = __SSAT(r - s, 16);
    s = __SSAT(((q31_t) ((x >> 16) - (y >> 16))), 16) << 16;

    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

    return sum;
  }

  /*
   * @brief C custom defined SHSUB16 for M3 and M0 processors
   */
  static __INLINE q31_t __SHSUB16(
				  q31_t x,
				  q31_t y)
  {

    q31_t diff;
    q31_t r, s;

    r = (short) x;
    s = (short) y;

    r = ((r >> 1) - (s >> 1));
    s = (((x >> 17) - (y >> 17)) << 16);

    diff = (s & 0xFFFF0000) | (r & 0x0000FFFF);

    return diff;
  }

  /*
   * @brief C custom defined QASX for M3 and M0 processors
   */
  static __INLINE q31_t __QASX(
			       q31_t x,
			       q31_t y)
  {

    q31_t sum = 0;

    sum = ((sum + clip_q31_to_q15((q31_t) ((short) (x >> 16) + (short) y))) << 16) +
      clip_q31_to_q15((q31_t) ((short) x - (short) (y >> 16)));

    return sum;
  }

  /*
   * @brief C custom defined SHASX for M3 and M0 processors
   */
  static __INLINE q31_t __SHASX(
				q31_t x,
				q31_t y)
  {

    q31_t sum;
    q31_t r, s;

    r = (short) x;
    s = (short) y;

    r = ((r >> 1) - (y >> 17));
    s = (((x >> 17) + (s >> 1)) << 16);

    sum = (s & 0xFFFF0000) | (r & 0x0000FFFF);

    return sum;
  }


  /*
   * @brief C custom defined QSAX for M3 and M0 processors
   */
  static __INLINE q31_t __QSAX(
			       q31_t x,
			       q31_t y)
  {

    q31_t sum = 0;