text: /******************************************************************************
   * @file     arm_math_utils.h
   * @brief    Public header file for CMSIS DSP Library
   * @version  V1.9.0
   * @date     20. July 2020
   ******************************************************************************/
  /*
   * Copyright (c) 2010-2020 Arm Limited or its affiliates. All rights reserved.
   *
   * SPDX-License-Identifier: Apache-2.0
   *
   * Licensed under the Apache License, Version 2.0 (the License); you may
   * not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   * www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an AS IS BASIS, WITHOUT
   * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  #ifndef _ARM_MATH_UTILS_H_
  
  #define _ARM_MATH_UTILS_H_
  
  #include "cmsis/CMSIS/DSP/Include/arm_math_types.h"
  
  #ifdef   __cplusplus
  extern "C"
  {
  #endif
  
    /**
     * @brief Macros required for reciprocal calculation in Normalized LMS
     */
  
  #define INDEX_MASK         0x0000003F
  
  
  #define SQ(x) ((x) * (x))
  
  #define ROUND_UP(N, S) ((((N) + (S) - 1) / (S)) * (S))
  
  
    /**
     * @brief Function to Calculates 1/in (reciprocal) value of Q31 Data type.
     */
    __STATIC_FORCEINLINE uint32_t arm_recip_q31(
          q31_t in,
          q31_t * dst,
    const q31_t * pRecipTable)
    {
      q31_t out;
      uint32_t tempVal;
      uint32_t index, i;
      uint32_t signBits;
  
      if (in > 0)
      {
        signBits = ((uint32_t) (__CLZ( in) - 1));
      }
      else
      {
        signBits = ((uint32_t) (__CLZ(-in) - 1));
      }
  
      /* Convert input sample to 1.31 format */
      in = (in << signBits);
  
      /* calculation of index for initial approximated Val */
      index = (uint32_t)(in >> 24);
      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 = (uint32_t) (((q63_t) in * out) >> 31);
        tempVal = 0x7FFFFFFFu - tempVal;
        /*      1.31 with exp 1 */
        /* out = (q31_t) (((q63_t) out * tempVal) >> 30); */
        out = clip_q63_to_q31(((q63_t) out * tempVal) >> 30);
      }
  
      /* 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_FORCEINLINE uint32_t arm_recip_q15(
          q15_t in,
          q15_t * dst,
    const q15_t * pRecipTable)
    {
      q15_t out = 0;
      uint32_t tempVal = 0;
      uint32_t index = 0, i = 0;
      uint32_t signBits = 0;
  
      if (in > 0)
      {
        signBits = ((uint32_t)(__CLZ( in) - 17));
      }
      else
      {
        signBits = ((uint32_t)(__CLZ(-in) - 17));
      }
  
      /* Convert input sample to 1.15 format */
      in = (in << signBits);
  
      /* calculation of index for initial approximated Val */
      index = (uint32_t)(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 = 0U; i < 2U; i++)
      {
        tempVal = (uint32_t) (((q31_t) in * out) >> 15);
        tempVal = 0x7FFFu - tempVal;
        /*      1.15 with exp 1 */
        out = (q15_t) (((q31_t) out * tempVal) >> 14);
        /* out = clip_q31_to_q15(((q31_t) out * tempVal) >> 14); */
      }
  
      /* write output */
      *dst = out;
  
      /* return num of signbits of out = 1/in value */
      return (signBits + 1);
    }
  
  
  /**
   * @brief  64-bit to 32-bit unsigned normalization
   * @param[in]  in           is input unsigned long long value
   * @param[out] normalized   is the 32-bit normalized value
   * @param[out] norm         is norm scale
   */
  __STATIC_INLINE  void arm_norm_64_to_32u(uint64_t in, int32_t * normalized, int32_t *norm)
  {
      int32_t     n1;
      int32_t     hi = (int32_t) (in >> 32);
      int32_t     lo = (int32_t) ((in << 32) >> 32);
  
      n1 = __CLZ(hi) - 32;
      if (!n1)
      {
          /*
           * input fits in 32-bit
           */
          n1 = __CLZ(lo);
          if (!n1)
          {
              /*
               * MSB set, need to scale down by 1
               */
              *norm = -1;
              *normalized = (((uint32_t) lo) >> 1);
          } else
          {
              if (n1 == 32)
              {
                  /*
                   * input is zero
                   */
                  *norm = 0;
                  *normalized = 0;
              } else
              {
                  /*
                   * 32-bit normalization
                   */
                  *norm = n1 - 1;
                  *normalized = lo << *norm;
              }
          }
      } else
      {
          /*
           * input fits in 64-bit
           */
          n1 = 1 - n1;
          *norm = -n1;
          /*
           * 64 bit normalization
           */
          *normalized = (((uint32_t) lo) >> n1) | (hi << (32 - n1));
      }
  }
  
  __STATIC_INLINE q31_t arm_div_q63_to_q31(q63_t num, q31_t den)
  {
      q31_t   result;
      uint64_t   absNum;
      int32_t   normalized;
      int32_t   norm;
  
      /*
       * if sum fits in 32bits
       * avoid costly 64-bit division
       */
      absNum = num > 0 ? num : -num;
      arm_norm_64_to_32u(absNum, &normalized, &norm);
      if (norm > 0)
          /*
           * 32-bit division
           */
          result = (q31_t) num / den;
      else
          /*
           * 64-bit division
           */
          result = (q31_t) (num / den);
  
      return result;
  }
  
  
  #ifdef   __cplusplus
  }
  #endif
  
  #endif /*ifndef _ARM_MATH_UTILS_H_ */
  