diff --git a/src/include/sof/math/trig.h b/src/include/sof/math/trig.h index 0c0b2d23a388..5f8dc28e22c9 100644 --- a/src/include/sof/math/trig.h +++ b/src/include/sof/math/trig.h @@ -13,14 +13,17 @@ #include -#define PI_DIV2_Q4_28 421657428 -#define PI_DIV2_Q3_29 843314856 -#define PI_Q4_28 843314857 -#define PI_MUL2_Q4_28 1686629713 -#define CORDIC_31B_TABLE_SIZE 31 -#define CORDIC_15B_TABLE_SIZE 15 -#define CORDIC_30B_ITABLE_SIZE 30 -#define CORDIC_16B_ITABLE_SIZE 16 +#define PI_Q4_28 843314857 /* int32(pi * 2^28) */ +#define PI_MUL2_Q4_28 1686629713 /* int32(2 * pi * 2^28) */ +#define PI_DIV2_Q3_29 843314857 /* int32(pi / 2 * 2^29) */ +#define PI_Q3_29 1686629713 /* int32(pi * 2^29) */ + +#define CORDIC_31B_TABLE_SIZE 31 +#define CORDIC_15B_TABLE_SIZE 15 +#define CORDIC_30B_ITABLE_SIZE 30 +#define CORDIC_16B_ITABLE_SIZE 16 +#define CORDIC_31B_ITERS_MINUS_ONE (CORDIC_31B_TABLE_SIZE - 1) +#define CORDIC_16B_ITERS_MINUS_ONE (CORDIC_16B_ITABLE_SIZE - 1) typedef enum { EN_32B_CORDIC_SINE, @@ -36,13 +39,49 @@ struct cordic_cmpx { int32_t im; }; +/** + * cordic_approx() - CORDIC-based approximation of sine and cosine + * @th_rad_fxp: Input angle in radian Q4.28 format + * @a_idx: Used LUT size + * @sign: Output pointer to sine/cosine sign + * @b_yn: Output pointer to sine value in Q2.30 format + * @xn: Output pointer to cosine value in Q2.30 format + * @th_cdc_fxp: Output pointer to the residual angle in Q2.30 format + */ void cordic_approx(int32_t th_rad_fxp, int32_t a_idx, int32_t *sign, int32_t *b_yn, int32_t *xn, int32_t *th_cdc_fxp); -int32_t is_scalar_cordic_acos(int32_t realvalue, int16_t numiters); -int32_t is_scalar_cordic_asin(int32_t realvalue, int16_t numiters); + +/** + * is_scalar_cordic_acos() - CORDIC-based approximation for inverse cosine + * @realvalue: Input cosine value in Q2.30 format + * @numiters_minus_one: Number of iterations minus one + * Return: Inverse cosine angle in Q3.29 format + */ +int32_t is_scalar_cordic_acos(int32_t realvalue, int numiters_minus_one); + +/** + * is_scalar_cordic_asin() - CORDIC-based approximation for inverse sine + * @realvalue: Input sine value in Q2.30 format + * @numiters_minus_one: Number of iterations minus one + * Return: Inverse sine angle in Q2.30 format + */ +int32_t is_scalar_cordic_asin(int32_t realvalue, int numiters_minus_one); + +/** + * cmpx_cexp() - CORDIC-based approximation of complex exponential e^(j*THETA) + * @sign: Sine sign + * @b_yn: Sine value in Q2.30 format + * @xn: Cosine value in Q2.30 format + * @type: CORDIC type + * @cexp: Output pointer to complex result in struct cordic_cmpx + */ void cmpx_cexp(int32_t sign, int32_t b_yn, int32_t xn, cordic_cfg type, struct cordic_cmpx *cexp); -/* Input is Q4.28, output is Q1.31 */ + /** + * sin_fixed_32b() - Sine function using CORDIC algorithm + * @th_rad_fxp: Input angle in radian Q4.28 format + * Return: Sine value in Q1.31 format + * * Compute fixed point cordicsine with table lookup and interpolation * The cordic sine algorithm converges, when the angle is in the range * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of @@ -71,6 +110,10 @@ static inline int32_t sin_fixed_32b(int32_t th_rad_fxp) } /** + * cos_fixed_32b() - Cosine function using CORDIC algorithm + * @th_rad_fxp: Input angle in radian Q4.28 format + * Return: Cosine value in Q1.31 format + * * Compute fixed point cordicsine with table lookup and interpolation * The cordic cosine algorithm converges, when the angle is in the range * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of @@ -98,8 +141,11 @@ static inline int32_t cos_fixed_32b(int32_t th_rad_fxp) return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31)); } -/* Input is Q4.28, output is Q1.15 */ /** + * sin_fixed_16b() - Sine function using CORDIC algorithm + * @th_rad_fxp: Input angle in radian Q4.28 format + * Return: Sine value in Q1.15 format + * * Compute fixed point cordic sine with table lookup and interpolation * The cordic sine algorithm converges, when the angle is in the range * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of @@ -129,6 +175,10 @@ static inline int16_t sin_fixed_16b(int32_t th_rad_fxp) } /** + * cos_fixed_16b() - Cosine function using CORDIC algorithm + * @th_rad_fxp: Input angle in radian Q4.28 format + * Return: Cosine value in Q1.15 format + * * Compute fixed point cordic cosine with table lookup and interpolation * The cordic cos algorithm converges, when the angle is in the range * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of @@ -158,7 +208,10 @@ static inline int16_t cos_fixed_16b(int32_t th_rad_fxp) } /** - * CORDIC-based approximation of complex exponential e^(j*THETA). + * cmpx_exp_32b() - CORDIC-based approximation of complex exponential e^(j*THETA). + * @th_rad_fxp: Input angle in radian Q4.28 format + * @cexp: Output pointer to complex result in struct cordic_cmpx in Q2.30 format + * * computes COS(THETA) + j*SIN(THETA) using CORDIC algorithm * approximation and returns the complex result. * THETA values must be in the range [-2*pi, 2*pi). The cordic @@ -190,7 +243,10 @@ static inline void cmpx_exp_32b(int32_t th_rad_fxp, struct cordic_cmpx *cexp) } /** - * CORDIC-based approximation of complex exponential e^(j*THETA). + * cmpx_exp_16b() - CORDIC-based approximation of complex exponential e^(j*THETA). + * @th_rad_fxp: Input angle in radian Q4.28 format + * @cexp: Output pointer to complex result in struct cordic_cmpx in Q1.15 format + * * computes COS(THETA) + j*SIN(THETA) using CORDIC algorithm * approximation and returns the complex result. * THETA values must be in the range [-2*pi, 2*pi). The cordic @@ -223,7 +279,10 @@ static inline void cmpx_exp_16b(int32_t th_rad_fxp, struct cordic_cmpx *cexp) } /** - * CORDIC-based approximation of inverse sine + * asin_fixed_32b() - CORDIC-based approximation of inverse sine + * @cdc_asin_th: Input value in Q2.30 format + * Return: Inverse sine angle in Q2.30 format + * * inverse sine of cdc_asin_theta based on a CORDIC approximation. * asin(cdc_asin_th) inverse sine angle values in radian produces using the DCORDIC * (Double CORDIC) algorithm. @@ -238,17 +297,18 @@ static inline int32_t asin_fixed_32b(int32_t cdc_asin_th) int32_t th_asin_fxp; if (cdc_asin_th >= 0) - th_asin_fxp = is_scalar_cordic_asin(cdc_asin_th, - CORDIC_31B_TABLE_SIZE); + th_asin_fxp = is_scalar_cordic_asin(cdc_asin_th, CORDIC_31B_ITERS_MINUS_ONE); else - th_asin_fxp = -is_scalar_cordic_asin(-cdc_asin_th, - CORDIC_31B_TABLE_SIZE); + th_asin_fxp = -is_scalar_cordic_asin(-cdc_asin_th, CORDIC_31B_ITERS_MINUS_ONE); return th_asin_fxp; /* Q2.30 */ } /** - * CORDIC-based approximation of inverse cosine + * acos_fixed_32b() - CORDIC-based approximation of inverse cosine + * @cdc_acos_th: Input value in Q2.30 format + * Return: Inverse cosine angle in Q3.29 format + * * inverse cosine of cdc_acos_theta based on a CORDIC approximation * acos(cdc_acos_th) inverse cosine angle values in radian produces using the DCORDIC * (Double CORDIC) algorithm. @@ -262,18 +322,19 @@ static inline int32_t acos_fixed_32b(int32_t cdc_acos_th) int32_t th_acos_fxp; if (cdc_acos_th >= 0) - th_acos_fxp = is_scalar_cordic_acos(cdc_acos_th, - CORDIC_31B_TABLE_SIZE); + th_acos_fxp = is_scalar_cordic_acos(cdc_acos_th, CORDIC_31B_ITERS_MINUS_ONE); else th_acos_fxp = - PI_MUL2_Q4_28 - is_scalar_cordic_acos(-cdc_acos_th, - CORDIC_31B_TABLE_SIZE); + PI_Q3_29 - is_scalar_cordic_acos(-cdc_acos_th, CORDIC_31B_ITERS_MINUS_ONE); return th_acos_fxp; /* Q3.29 */ } /** - * CORDIC-based approximation of inverse sine + * asin_fixed_16b() - CORDIC-based approximation of inverse sine + * @cdc_asin_th: Input value in Q2.30 format + * Return: Inverse sine angle in Q2.14 format + * * inverse sine of cdc_asin_theta based on a CORDIC approximation. * asin(cdc_asin_th) inverse sine angle values in radian produces using the DCORDIC * (Double CORDIC) algorithm. @@ -289,17 +350,18 @@ static inline int16_t asin_fixed_16b(int32_t cdc_asin_th) int32_t th_asin_fxp; if (cdc_asin_th >= 0) - th_asin_fxp = is_scalar_cordic_asin(cdc_asin_th, - CORDIC_16B_ITABLE_SIZE); + th_asin_fxp = is_scalar_cordic_asin(cdc_asin_th, CORDIC_16B_ITERS_MINUS_ONE); else - th_asin_fxp = -is_scalar_cordic_asin(-cdc_asin_th, - CORDIC_16B_ITABLE_SIZE); + th_asin_fxp = -is_scalar_cordic_asin(-cdc_asin_th, CORDIC_16B_ITERS_MINUS_ONE); /*convert Q2.30 to Q2.14 format*/ return sat_int16(Q_SHIFT_RND(th_asin_fxp, 30, 14)); } /** - * CORDIC-based approximation of inverse cosine + * acos_fixed_16b() - CORDIC-based approximation of inverse cosine + * @cdc_acos_th: Input value in Q2.30 format + * Return: Inverse cosine angle in Q3.13 format + * * inverse cosine of cdc_acos_theta based on a CORDIC approximation * acos(cdc_acos_th) inverse cosine angle values in radian produces using the DCORDIC * (Double CORDIC) algorithm. @@ -314,12 +376,10 @@ static inline int16_t acos_fixed_16b(int32_t cdc_acos_th) int32_t th_acos_fxp; if (cdc_acos_th >= 0) - th_acos_fxp = is_scalar_cordic_acos(cdc_acos_th, - CORDIC_16B_ITABLE_SIZE); + th_acos_fxp = is_scalar_cordic_acos(cdc_acos_th, CORDIC_16B_ITERS_MINUS_ONE); else th_acos_fxp = - PI_MUL2_Q4_28 - is_scalar_cordic_acos(-cdc_acos_th, - CORDIC_16B_ITABLE_SIZE); + PI_Q3_29 - is_scalar_cordic_acos(-cdc_acos_th, CORDIC_16B_ITERS_MINUS_ONE); /*convert Q3.29 to Q3.13 format*/ return sat_int16(Q_SHIFT_RND(th_acos_fxp, 29, 13)); diff --git a/src/math/trig.c b/src/math/trig.c index 32613d393034..13f72f3d7ec1 100644 --- a/src/math/trig.c +++ b/src/math/trig.c @@ -13,18 +13,13 @@ #include #include -/* Use a local definition to avoid adding a dependency on */ -#define _M_PI 3.14159265358979323846 /* pi */ +#define CORDIC_SINE_COS_LUT_Q29 652032874 /* deg = 69.586061, int32(1.214505869895220 * 2^29) */ + +#define CORDIC_SINCOS_PIOVERTWO_Q28 421657428 /* int32(pi / 2 * 2^28) */ +#define CORDIC_SINCOS_PI_Q28 843314857 /* int32(pi * 2^28) */ +#define CORDIC_SINCOS_TWOPI_Q28 1686629713 /* int32(2 * pi * 2^28) */ +#define CORDIC_SINCOS_ONEANDHALFPI_Q28 1264972285 /* int32(1.5 * pi * 2^28) */ -/* 652032874 , deg = 69.586061*/ -const int32_t cordic_sine_cos_lut_q29fl = Q_CONVERT_FLOAT(1.214505869895220, 29); -/* 1686629713, deg = 90.000000 */ -const int32_t cordic_sine_cos_piovertwo_q30fl = Q_CONVERT_FLOAT(_M_PI / 2, 30); -/* 421657428 , deg = 90.000000 */ -const int32_t cord_sincos_piovertwo_q28fl = Q_CONVERT_FLOAT(_M_PI / 2, 28); -/* 843314857, deg = 90.000000 */ -const int32_t cord_sincos_piovertwo_q29fl = Q_CONVERT_FLOAT(_M_PI / 2, 29); -/* arc trignometry constant*/ /** * CORDIC-based approximation of sine and cosine * \+----------+----------------------------------------+--------------------+-------------------+ @@ -36,20 +31,25 @@ const int32_t cord_sincos_piovertwo_q29fl = Q_CONVERT_FLOAT(_M_PI / 2, 29); * \|1686629713| Q_CONVERT_FLOAT(1.5707963267341256, 30)| 89.9999999965181| 1.57079632673413 | * \+----------+----------------------------------------+--------------------+-------------------+ */ -/* 379625062, deg = 81.0284683480568475 or round(1.4142135605216026*2^28) */ -const int32_t cord_arcsincos_q28fl = Q_CONVERT_FLOAT(1.4142135605216026 / 2, 28); -/* 1073741824, deg = 57.2957795130823229 or round(1*2^30)*/ -const int32_t cord_arcsincos_q30fl = Q_CONVERT_FLOAT(1.0000000000000000, 30); + +#define CORDIC_ARCSINCOS_SQRT2_DIV4_Q30 379625062 /* int32(sqrt(2) / 4 * 2^30) */ +#define CORDIC_ARCSINCOS_ONE_Q30 1073741824 /* int32(1 * 2^30) */ + /** * CORDIC-based approximation of sine, cosine and complex exponential */ void cordic_approx(int32_t th_rad_fxp, int32_t a_idx, int32_t *sign, int32_t *b_yn, int32_t *xn, int32_t *th_cdc_fxp) { + int32_t direction; + int32_t abs_th; int32_t b_idx; - int32_t xtmp; - int32_t ytmp; - *sign = 1; + int32_t xn_local = CORDIC_SINE_COS_LUT_Q29; + int32_t yn_local = 0; + int32_t xtmp = CORDIC_SINE_COS_LUT_Q29; + int32_t ytmp = 0; + int shift; + /* Addition or subtraction by a multiple of pi/2 is done in the data type * of the input. When the fraction length is 29, then the quantization error * introduced by the addition or subtraction of pi/2 is done with 29 bits of @@ -58,57 +58,46 @@ void cordic_approx(int32_t th_rad_fxp, int32_t a_idx, int32_t *sign, int32_t *b_ * without overflow.Increase of fractionLength makes the addition or * subtraction of a multiple of pi/2 more precise */ - if (th_rad_fxp > cord_sincos_piovertwo_q28fl) { - if ((th_rad_fxp - cord_sincos_piovertwo_q29fl) <= cord_sincos_piovertwo_q28fl) { - th_rad_fxp -= cord_sincos_piovertwo_q29fl; - *sign = -1; - } else { - th_rad_fxp -= cordic_sine_cos_piovertwo_q30fl; - } - } else if (th_rad_fxp < -cord_sincos_piovertwo_q28fl) { - if ((th_rad_fxp + cord_sincos_piovertwo_q29fl) >= -cord_sincos_piovertwo_q28fl) { - th_rad_fxp += cord_sincos_piovertwo_q29fl; - *sign = -1; + abs_th = (th_rad_fxp >= 0) ? th_rad_fxp : -th_rad_fxp; + direction = (th_rad_fxp >= 0) ? 1 : -1; + *sign = 1; + if (abs_th > CORDIC_SINCOS_PIOVERTWO_Q28) { + if (abs_th <= CORDIC_SINCOS_ONEANDHALFPI_Q28) { + th_rad_fxp -= direction * CORDIC_SINCOS_PI_Q28; + *sign = -1; } else { - th_rad_fxp += cordic_sine_cos_piovertwo_q30fl; + th_rad_fxp -= direction * CORDIC_SINCOS_TWOPI_Q28; } } th_rad_fxp <<= 2; - *b_yn = 0; - *xn = cordic_sine_cos_lut_q29fl; - xtmp = cordic_sine_cos_lut_q29fl; - ytmp = 0; /* Calculate the correct coefficient values from rotation angle. * Find difference between the coefficients from the lookup table * and those from the calculation */ for (b_idx = 0; b_idx < a_idx; b_idx++) { - if (th_rad_fxp < 0) { - th_rad_fxp += cordic_lookup[b_idx]; - *xn += ytmp; - *b_yn -= xtmp; - } else { - th_rad_fxp -= cordic_lookup[b_idx]; - *xn -= ytmp; - *b_yn += xtmp; - } - xtmp = *xn >> (b_idx + 1); - ytmp = *b_yn >> (b_idx + 1); + direction = (th_rad_fxp >= 0) ? 1 : -1; + shift = b_idx + 1; + th_rad_fxp -= direction * cordic_lookup[b_idx]; + xn_local -= direction * ytmp; + yn_local += direction * xtmp; + xtmp = xn_local >> shift; + ytmp = yn_local >> shift; } - /* Q2.30 format -sine, cosine*/ + + /* Write back results once */ + *xn = xn_local; + *b_yn = yn_local; *th_cdc_fxp = th_rad_fxp; } EXPORT_SYMBOL(cordic_approx); /** * CORDIC-based approximation for inverse cosine - * Arguments : int32_t cosvalue - * int16_t numiters - * Return Type : int32_t + * cosvalue is Q2.30, return value is angle in Q3.29 format */ -int32_t is_scalar_cordic_acos(int32_t cosvalue, int16_t numiters) +int32_t is_scalar_cordic_acos(int32_t cosvalue, int numiters_minus_one) { int32_t xdshift; int32_t ydshift; @@ -118,25 +107,22 @@ int32_t is_scalar_cordic_acos(int32_t cosvalue, int16_t numiters) int32_t y = 0; int32_t z = 0; int32_t sign; - int32_t b_i; - int i; + int b_i; int j; - int k; /* Initialize the variables for the cordic iteration * angles less than pi/4, we initialize (x,y) along the x-axis. * angles greater than or equal to pi/4, we initialize (x,y) * along the y-axis. This improves the accuracy of the algorithm * near the edge of the domain of convergence + * + * Note: not pi/4 but sqrt(2)/4 is used as the threshold */ - if ((cosvalue >> 1) < cord_arcsincos_q28fl) { - x = 0; - y = cord_arcsincos_q30fl; + if (cosvalue < CORDIC_ARCSINCOS_SQRT2_DIV4_Q30) { + y = CORDIC_ARCSINCOS_ONE_Q30; z = PI_DIV2_Q3_29; } else { - x = cord_arcsincos_q30fl; - y = 0; - z = 0; + x = CORDIC_ARCSINCOS_ONE_Q30; } /* DCORDIC(Double CORDIC) algorithm */ @@ -144,20 +130,14 @@ int32_t is_scalar_cordic_acos(int32_t cosvalue, int16_t numiters) /* CORDIC method,where the iteration step value changes EVERY time, i.e. on */ /* each iteration, in the double iteration method, the iteration step value */ /* is repeated twice and changes only through one iteration */ - i = numiters - 1; - for (b_i = 0; b_i < i; b_i++) { + for (b_i = 0; b_i < numiters_minus_one; b_i++) { j = (b_i + 1) << 1; if (j >= 31) j = 31; - if (b_i < 31) - k = b_i; - else - k = 31; - - xshift = x >> k; + xshift = x >> b_i; + yshift = y >> b_i; xdshift = x >> j; - yshift = y >> k; ydshift = y >> j; /* Do nothing if x currently equals the target value. Allowed for * double rotations algorithms, as it is equivalent to rotating by @@ -184,11 +164,9 @@ int32_t is_scalar_cordic_acos(int32_t cosvalue, int16_t numiters) /** * CORDIC-based approximation for inverse sine - * Arguments : int32_t sinvalue - * int16_t numiters - * Return Type : int32_t + * sinvalue is Q2.30, return value is angle in Q2.30 format */ -int32_t is_scalar_cordic_asin(int32_t sinvalue, int16_t numiters) +int32_t is_scalar_cordic_asin(int32_t sinvalue, int numiters_minus_one) { int32_t xdshift; int32_t ydshift; @@ -198,25 +176,22 @@ int32_t is_scalar_cordic_asin(int32_t sinvalue, int16_t numiters) int32_t y = 0; int32_t z = 0; int32_t sign; - int32_t b_i; - int i; + int b_i; int j; - int k; /* Initialize the variables for the cordic iteration * angles less than pi/4, we initialize (x,y) along the x-axis. * angles greater than or equal to pi/4, we initialize (x,y) * along the y-axis. This improves the accuracy of the algorithm * near the edge of the domain of convergence + * + * Note: Instead of pi/4, sqrt(2)/4 is used as the threshold */ - if ((sinvalue >> 1) > cord_arcsincos_q28fl) { - x = 0; - y = cord_arcsincos_q30fl; + if (sinvalue > CORDIC_ARCSINCOS_SQRT2_DIV4_Q30) { + y = CORDIC_ARCSINCOS_ONE_Q30; z = PI_DIV2_Q3_29; } else { - x = cord_arcsincos_q30fl; - y = 0; - z = 0; + x = CORDIC_ARCSINCOS_ONE_Q30; } /* DCORDIC(Double CORDIC) algorithm */ @@ -224,21 +199,16 @@ int32_t is_scalar_cordic_asin(int32_t sinvalue, int16_t numiters) /* CORDIC method,where the iteration step value changes EVERY time, i.e. on */ /* each iteration, in the double iteration method, the iteration step value */ /* is repeated twice and changes only through one iteration */ - i = numiters - 1; - for (b_i = 0; b_i < i; b_i++) { + // i = numiters - 1; + for (b_i = 0; b_i < numiters_minus_one; b_i++) { j = (b_i + 1) << 1; if (j >= 31) j = 31; - if (b_i < 31) - k = b_i; - else - k = 31; - - xshift = x >> k; - xdshift = x >> j; - yshift = y >> k; + xshift = x >> b_i; + yshift = y >> b_i; ydshift = y >> j; + xdshift = x >> j; /* Do nothing if x currently equals the target value. Allowed for * double rotations algorithms, as it is equivalent to rotating by * the same angle in opposite directions sequentially. Accounts for @@ -263,13 +233,9 @@ int32_t is_scalar_cordic_asin(int32_t sinvalue, int16_t numiters) } /** - * approximated complex result - * Arguments : int32_t sign - * int32_t b_yn - * int32_t xn - * enum type - * struct cordic_cmpx - * Return Type : none + * cmpx_cexp() - CORDIC-based approximation of complex exponential e^(j*THETA) + * + * The sine and cosine values are in Q2.30 format from cordic_approx()function. */ void cmpx_cexp(int32_t sign, int32_t b_yn, int32_t xn, cordic_cfg type, struct cordic_cmpx *cexp) {