tmk_core/tool/mbed/mbed-sdk/libraries/dsp/cmsis_dsp/FastMathFunctions/arm_sqrt_q31.c

   1 /* ----------------------------------------------------------------------
   2 * Copyright (C) 2010-2013 ARM Limited. All rights reserved.
   3 *
   4 * $Date:        17. January 2013
   5 * $Revision:    V1.4.1
   6 *
   7 * Project:      CMSIS DSP Library
   8 * Title:                arm_sqrt_q31.c
   9 *
  10 * Description:  Q31 square root function.
  11 *
  12 * Target Processor: Cortex-M4/Cortex-M3/Cortex-M0
  13 *
  14 * Redistribution and use in source and binary forms, with or without
  15 * modification, are permitted provided that the following conditions
  16 * are met:
  17 *   - Redistributions of source code must retain the above copyright
  18 *     notice, this list of conditions and the following disclaimer.
  19 *   - Redistributions in binary form must reproduce the above copyright
  20 *     notice, this list of conditions and the following disclaimer in
  21 *     the documentation and/or other materials provided with the
  22 *     distribution.
  23 *   - Neither the name of ARM LIMITED nor the names of its contributors
  24 *     may be used to endorse or promote products derived from this
  25 *     software without specific prior written permission.
  26 *
  27 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
  28 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
  29 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
  30 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
  31 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  32 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
  33 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
  35 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  36 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
  37 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
  38 * POSSIBILITY OF SUCH DAMAGE.
  39 * -------------------------------------------------------------------- */
  40 #include "arm_math.h"
  41 #include "arm_common_tables.h"
  42
  43 /**
  44  * @ingroup groupFastMath
  45  */
  46
  47 /**
  48  * @addtogroup SQRT
  49  * @{
  50  */
  51
  52 /**
  53  * @brief Q31 square root function.
  54  * @param[in]   in    input value.  The range of the input value is [0 +1) or 0x00000000 to 0x7FFFFFFF.
  55  * @param[out]  *pOut square root of input value.
  56  * @return The function returns ARM_MATH_SUCCESS if the input value is positive
  57  * and ARM_MATH_ARGUMENT_ERROR if the input is negative.  For
  58  * negative inputs, the function returns *pOut = 0.
  59  */
  60
  61 arm_status arm_sqrt_q31(
  62   q31_t in,
  63   q31_t * pOut)
  64 {
  65   q31_t number, temp1, bits_val1, var1, signBits1, half;
  66   float32_t temp_float1;
  67   union
  68   {
  69       q31_t fracval;
  70       float32_t floatval;
  71   } tempconv;
  72
  73   number = in;
  74
  75   /* If the input is a positive number then compute the signBits. */
  76   if(number > 0)
  77   {
  78     signBits1 = __CLZ(number) - 1;
  79
  80     /* Shift by the number of signBits1 */
  81     if((signBits1 % 2) == 0)
  82     {
  83       number = number << signBits1;
  84     }
  85     else
  86     {
  87       number = number << (signBits1 - 1);
  88     }
  89
  90     /* Calculate half value of the number */
  91     half = number >> 1;
  92     /* Store the number for later use */
  93     temp1 = number;
  94
  95     /*Convert to float */
  96     temp_float1 = number * 4.6566128731e-010f;
  97     /*Store as integer */
  98     tempconv.floatval = temp_float1;
  99     bits_val1 = tempconv.fracval;
 100     /* Subtract the shifted value from the magic number to give intial guess */
 101     bits_val1 = 0x5f3759df - (bits_val1 >> 1);  // gives initial guess
 102     /* Store as float */
 103     tempconv.fracval = bits_val1;
 104     temp_float1 = tempconv.floatval;
 105     /* Convert to integer format */
 106     var1 = (q31_t) (temp_float1 * 1073741824);
 107
 108     /* 1st iteration */
 109     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
 110                                      ((q31_t)
 111                                       ((((q31_t)
 112                                          (((q63_t) var1 * var1) >> 31)) *
 113                                         (q63_t) half) >> 31))) >> 31)) << 2;
 114     /* 2nd iteration */
 115     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
 116                                      ((q31_t)
 117                                       ((((q31_t)
 118                                          (((q63_t) var1 * var1) >> 31)) *
 119                                         (q63_t) half) >> 31))) >> 31)) << 2;
 120     /* 3rd iteration */
 121     var1 = ((q31_t) ((q63_t) var1 * (0x30000000 -
 122                                      ((q31_t)
 123                                       ((((q31_t)
 124                                          (((q63_t) var1 * var1) >> 31)) *
 125                                         (q63_t) half) >> 31))) >> 31)) << 2;
 126
 127     /* Multiply the inverse square root with the original value */
 128     var1 = ((q31_t) (((q63_t) temp1 * var1) >> 31)) << 1;
 129
 130     /* Shift the output down accordingly */
 131     if((signBits1 % 2) == 0)
 132     {
 133       var1 = var1 >> (signBits1 / 2);
 134     }
 135     else
 136     {
 137       var1 = var1 >> ((signBits1 - 1) / 2);
 138     }
 139     *pOut = var1;
 140
 141     return (ARM_MATH_SUCCESS);
 142   }
 143   /* If the number is a negative number then store zero as its square root value */
 144   else
 145   {
 146     *pOut = 0;
 147     return (ARM_MATH_ARGUMENT_ERROR);
 148   }
 149 }
 150
 151 /**
 152  * @} end of SQRT group
 153  */