marvell/services/dxslic/api_lib/src/dxs_misc.c

/******************************************************************************

  Copyright (c) 2006-2015 Lantiq Deutschland GmbH
  Copyright (c) 2015 Lantiq Beteiligungs-GmbH & Co.KG
  Copyright 2018, Intel Corporation.

  For licensing information, see the file 'LICENSE' in the root folder of
  this software module.

******************************************************************************/

/**
   \file dxs_misc.c
   This file contains the implementation of the miscellaneous functions.
*/

/* ========================================================================= */
/*                                 Includes                                  */
/* ========================================================================= */
#include "dxs_misc.h"

/* ========================================================================= */
/*                             Macro definitions                             */
/* ========================================================================= */
#define Q                      15
#define Q_PRECISION            1
#define Q_ABS(a)               ((a) < 0 ? -(a) : (a))

/* ========================================================================= */
/*                         Function implementation                           */
/* ========================================================================= */

/**
   Rotate right (circular shift) an arbitrary data block for up to 8 bits
   FIXME - data block length is a multiple of bytes, change to multiple of bits

   \param   data  - pointer to the beginning of the data block
   \param   n     - length of the data block in bytes
   \param   shift - number of bits to shift (min=0, max=8)

   \return
   - none
*/
static void dxs_misc_data_blk_ror8 (unsigned char *data, int n, int shift)
{
   int i;
   unsigned char tmp = data[n-1];

   for (i=n-1; i>0; i--)
      data[i] = (data[i] >> shift)|((data[i-1] & (0xff >> (8 - shift))) << (8-shift));
   data[0] = (data[0] >> shift)|((tmp & (0xff >> (8 - shift))) << (8-shift));
}

/**
   Rotate right (circular shift) an arbitrary data block

   \param   data  - pointer to the beginning of the data block
   \param   len   - length of the data block in bytes
   \param   shift - number of bits to shift

   \return
   - none
*/
void DXS_Misc_DataBlkRor (unsigned char *data, int len, int shift)
{
   int i;

   for (i=0; i<(shift >> 3); i++)
      dxs_misc_data_blk_ror8(data, len, 8);
   dxs_misc_data_blk_ror8(data, len, (shift % 8));
}

/**
   Function DXS_Misc_MulQ15

   \param   a    - Q15 number
   \param   b    - Q15 number

   \return
   - uint16_t multiplication result of two Q15 numbers
*/
uint16_t DXS_Misc_MulQ15 (int16_t a, int16_t b)
{
   signed long int  temp;

   temp = (long int)a * (long int)b;
   /* Rounding up */
   temp += (1 << (Q-1));

   /* Correct by dividing by base */
   return (temp >> Q);
}

/**
   Function DXS_Misc_LeveldB_to_Factor

   \param   dB_ten    - integer value in units of 0.1 dB
   \remark  Parameter dB_ten must be zero or negative.

   \return
   - uint16_t factor in Q15 format
*/
uint16_t DXS_Misc_LeveldB_to_Factor (int16_t dB_ten)
{
   int16_t n, r, hundr, tens, units, mul1 = 32767, mul2 = 32767;

   /* switch to 0.01 dB units */
   dB_ten *= 10;

   /* calculate factors of 0.5 */
   n = dB_ten / -602;

   /* calculate reminder */
   r = -dB_ten - n * 602;

   hundr = r / 100;
   tens = (r % 100) / 10;
   units = (r % 100) % 10;

   while (n--)
   {
      /* multiply 0.5 n times */
      mul1 = DXS_Misc_MulQ15(mul1, 16384);
   }

   while (hundr--)
   {
      /* multiply 0.891251 (1dB) hundr times */
      mul2 = DXS_Misc_MulQ15(mul2, 29205);
   }

   while (tens--)
   {
      /* multiply 0.988553 (0.1dB) tens times */
      mul2 = DXS_Misc_MulQ15(mul2, 32393);
   }

   while (units--)
   {
      /* multiply 0.998849 (0.01dB) units times */
      mul2 = DXS_Misc_MulQ15(mul2, 32730);
   }

   return (DXS_Misc_MulQ15(mul1, mul2));
}

/*
 * alt_method
 *
 *    - parameter dB_t - integer, in 0.1 dB units, must be negative
 *
 */
uint16_t DXS_Misc_PowerdB_to_Factor (int16_t dB_t)
{
   int16_t n, r, hundr, tens, units, mul1 = 32767, mul2 = 32767;

   /* switch to 0.01 dB units */
   dB_t *= 10;

   /* calculate factors of 0.5 */
   n = dB_t / -301;

   /* calculate reminder */
   r = -dB_t - n * 301;

   hundr = r / 100;
   tens = (r % 100) / 10;
   units = (r % 100) % 10;

   while (n--)
   {
      /* multiply 0.5 n times */
      mul1 = DXS_Misc_MulQ15(mul1, 16384);
   }

   while (hundr--)
   {
      /* multiply 0.794328 (1dB) hundr times */
      mul2 = DXS_Misc_MulQ15(mul2, 26029);
   }

   while (tens--)
   {
      /* multiply 0.977237 (0.1dB) tens times */
      mul2 = DXS_Misc_MulQ15(mul2, 32022);
   }

   while (units--)
   {
      /* multiply 0.9977 (0.01dB) units times */
      mul2 = DXS_Misc_MulQ15(mul2, 32693);
   }

   return (DXS_Misc_MulQ15 (mul1, mul2));
}

/**
   Function DXS_Misc_RoundDiv

   \param   dividend
   \param   divisor

   \return
   - uint32_t division result
*/
uint32_t DXS_Misc_RoundDiv(uint32_t dividend, uint32_t divisor)
{
   return (dividend + (divisor >> 1)) / divisor;
}

/******************************************************************************/
/*                Fixed Point Calculations in Q4.27 format                    */
/******************************************************************************/
#if 0
/**
   Q4.27 Division

   \param  a  - dividend in Q4.27
   \param  b  - divisor in Q4.27
   \return    - a/b in Q4.27
*/
static int32_t q4_27_div (int32_t a, int32_t b)
{
   int64_t temp = ((int64_t)a) << 27;

   /* round up */
   temp += (b >> 1);

   return (temp / b);
}

/**
   Q4.27 Square Root

   \param   a - argument in Q4.27
   \return
   - sqrt(a) in Q4.27
*/
int32_t DXS_Misc_Q4_27_Sqrt(int32_t a)
{
   int32_t x0, x1;

   if (a == 0) return a;

   /* initial guess */
   x0 = a;

   /* Newton's formula */
   while (1)
   {
      x1 = (x0 + q4_27_div(a, x0)) >> 1;

      if (Q_ABS(x1-x0) <= Q_PRECISION)
         break;

      x0 = x1;
   }

   return x1;
}
#endif /* 0 */

/**
   Convert float to Q4.27

   \param   f - floating point number
   \return
   - Q4.27 representation of f
*/
int32_t DXS_Misc_Float_to_Q4_27 (float f)
{
   f *= 134217728.0f;
   /* round up */
   f += (f >= 0) ? 0.5 : -0.5;

   return (int32_t)f;
}

/**
   Convert Q4.27 to float

   \param   q - Q4.27 number
   \return
   - floating point representaion of Q4.27 q
*/
float DXS_Misc_Q4_27_to_Float (int32_t q)
{
   return ((float)q / 134217728.0f);
}

/**
   Q4.27 Multiplication

   \param   a - Q4.27 number
   \param   b - Q4.27 number
   \return
   - (a*b) in Q4.27 format
*/
int32_t DXS_Misc_Q4_27_Mul (int32_t a, int32_t b)
{
   int64_t tmp = (int64_t)a * (int64_t)b;

   /* round up */
   tmp += (1 << 26);

   return (int32_t)(tmp >> 27);
}

/******************************************************************************/
/*               Fixed Point Calculations in Q15.16 format                    */
/******************************************************************************/
#define  Q15_16_LN_10   0x24D76

/**
   Q15.16 Division

   \param  a  - dividend in Q15.16
   \param  b  - divisor in Q15.16
   \return    - a/b in Q15.16
*/
static int32_t q15_16_div (int32_t a, int32_t b)
{
   int64_t temp = ((int64_t)a) << 16;

   /* round up */
   temp += (b >> 1);

   return (temp / b);
}

/**
   Q15.16 Multiplication

   \param  a  - Q15.16 number
   \param  b  - Q15.16 number
   \return    - (a*b) in Q15.16
*/
static int32_t q15_16_mul (int32_t a, int32_t b)
{
   int64_t tmp = (int64_t)a * (int64_t)b;

   /* round up */
   tmp += (1 << 15);

   return (int32_t)(tmp >> 16);
}

/**
   Q15.16 Power

   \param  a  - Q15.16 number
   \param  pw - power, conventional integer, not Q
   \return    - (a^pw) in Q15.16
*/
static int32_t q15_16_pow (int32_t a, int32_t pw)
{
   int32_t result = Q15_16_ONE;
   int32_t (*op)(int32_t a, int32_t b);

   if (pw < 0)
   {
      op = q15_16_div;
      pw = -pw;
   }
   else
      op = q15_16_mul;

   while (pw--)
      result = op(result, a);

   return result;
}

/**
   Convert float to Q15.16

   \param  f  - floating point number
   \return    - Q15.16 representation of f
*/
int32_t DXS_Misc_Float_to_Q15_16 (float f)
{
   f *= 65536.0f;
   /* round up */
   f += (f >= 0) ? 0.5 : -0.5;

   return (int32_t)f;
}

/**
   Convert Q15.16 to float

   \param  q  - Q15.16 number
   \return    - floating point representation of q
*/
float DXS_Misc_Q15_16_to_Float (int32_t q)
{
   return ((float)q / 65536.0f);
}

/**
   Common logarithm calculation

   \param  a  - argument in Q15.16
   \return    - log10(a) in Q15.16
*/
int32_t DXS_Misc_Q15_16_Log10 (int32_t a)
{
   int32_t k=Q15_16_ONE, p=1, result=0, tmp=0;

   while (1)
   {
      /* Taylor series */
      result +=
         (q15_16_mul(q15_16_pow(-Q15_16_ONE, (p+1)),
                     q15_16_div(q15_16_pow((a-Q15_16_ONE), p), k)));

      if (Q_ABS(result-tmp) <= Q_PRECISION)
         break;

      tmp = result;
      k += Q15_16_ONE;
      p++;
   }

   return q15_16_div(result, Q15_16_LN_10);
}

int32_t DXS_Misc_Q15_16_Mul(int32_t a, int32_t b)
{
   return q15_16_mul (a, b);
}

int32_t DXS_Misc_Q15_16_Div(int32_t a, int32_t b)
{
   return q15_16_div (a, b);
}