Skip to content

Commit 845c278

Browse files
ShriramShastrylgirdwood
ShriramShastry
authored and
lgirdwood
committed
Math: Trignometry: Added cordic sin cos function
Cordic sin cos input value range is [-2*pi to 2*pi] and output range is [-1 to +1] This is common function to calculate trignometric sine and cosine using separate lookup table size for speeds and accuracy calculation. For 32bit sine and cosine Error (max = 0.000000011175871), THD+N = -170.152933 For 16bit sine and cosine Error (max = 0.000061), THD+N = -91.518584 Signed-off-by: ShriramShastry <malladi.sastry@intel.com>
1 parent 7c5e08c commit 845c278

10 files changed

Lines changed: 682 additions & 82 deletions

File tree

‎src/audio/Kconfig‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -96,9 +96,22 @@ config COMP_IIR
9696
help
9797
Select for IIR component
9898

99+
config CORDIC_FIXED
100+
bool "Sine Cosine library function"
101+
default n
102+
help
103+
This option builds the 32 and 16 bit sin_fixed() library function.
104+
The input is Q4.28 format and the output is Q1.31. The cordic sine
105+
cos algorithm converges, when the angle is in the range [-pi/2, pi/2).
106+
If an angle is outside of this range, then a multiple of pi/2 is added
107+
or subtracted from the angle until it is within the range [-pi/2,pi/2).
108+
Start with the angle in the range [-2*pi, 2*pi) and output has range in
109+
[-1.0 to 1.0]
110+
99111
config COMP_TONE
100112
bool "Tone component"
101113
default y
114+
select CORDIC_FIXED
102115
help
103116
Select for Tone component
104117

@@ -152,6 +165,7 @@ config COMP_CROSSOVER
152165

153166
config COMP_DRC
154167
bool "Dynamic Range Compressor component"
168+
select CORDIC_FIXED
155169
default n
156170
help
157171
Select for Dynamic Range Compressor (DRC) component. A DRC can be used
@@ -161,6 +175,7 @@ config COMP_DRC
161175
config COMP_MULTIBAND_DRC
162176
depends on COMP_IIR && COMP_CROSSOVER && COMP_DRC
163177
bool "Multiband Dynamic Range Compressor component"
178+
select CORDIC_FIXED
164179
default n
165180
help
166181
Select for Multiband Dynamic Range Compressor (DRC) component. It

‎src/audio/drc/drc_math_generic.c‎

Lines changed: 4 additions & 5 deletions
Original file line numberDiff line numberDiff line change
@@ -225,12 +225,11 @@ inline int32_t drc_inv_fixed(int32_t x, int32_t precision_x, int32_t precision_y
225225
*/
226226
inline int32_t drc_sin_fixed(int32_t x)
227227
{
228-
const int32_t PI_OVER_TWO = Q_CONVERT_FLOAT(1.57079632679489661923f, 30);
228+
const int32_t PI_OVER_TWO = Q_CONVERT_FLOAT(1.57079632679489661923, 30);
229+
/* input range of sin_fixed_16b() is non-negative */
230+
int32_t abs_sin_val = sin_fixed_16b(q_mult(ABS(x), PI_OVER_TWO, 30, 30, 28));
229231

230-
/* input range of sin_fixed() is non-negative */
231-
int32_t abs_sin_val = sin_fixed(q_mult(ABS(x), PI_OVER_TWO, 30, 30, 28));
232-
233-
return SGN(x) < 0 ? -abs_sin_val : abs_sin_val;
232+
return x < 0 ? -abs_sin_val << 16 : abs_sin_val << 16;
234233
}
235234

236235
/*

‎src/audio/tone.c‎

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -143,11 +143,10 @@ static int32_t tonegen(struct tone_state *sg)
143143
{
144144
int64_t sine;
145145
int64_t w;
146-
147146
/* sg->w is angle in Q4.28 radians format, sin() returns Q1.31 */
148147
/* sg->a is amplitude as Q1.31 */
149148
sine =
150-
q_mults_32x32(sin_fixed(sg->w), sg->a,
149+
q_mults_32x32(sin_fixed_32b(sg->w), sg->a,
151150
Q_SHIFT_BITS_64(31, 31, 31));
152151

153152
/* Next point */

‎src/include/sof/math/trig.h‎

Lines changed: 22 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -12,10 +12,31 @@
1212

1313
#include <stdint.h>
1414

15+
#ifndef UNIT_CORDIC_TEST
16+
#define CONFIG_CORDIC_TRIGONOMETRY_FIXED
17+
#endif
18+
19+
1520
#define PI_DIV2_Q4_28 421657428
1621
#define PI_Q4_28 843314857
1722
#define PI_MUL2_Q4_28 1686629713
23+
#define CORDIC_31B_TABLE_SIZE 31
24+
#define CORDIC_SIN_COS_15B_TABLE_SIZE 15
25+
26+
typedef enum {
27+
EN_32B_CORDIC_SINE,
28+
EN_32B_CORDIC_COSINE,
29+
EN_16B_CORDIC_SINE,
30+
EN_16B_CORDIC_COSINE,
31+
} cordic_cfg;
1832

19-
int32_t sin_fixed(int32_t th_rad_fxp); /* Input is Q4.28, output is Q1.31 */
33+
void cordic_sin_cos(int32_t th_rad_fxp, cordic_cfg type, int32_t *sign, int32_t *b_yn, int32_t *xn,
34+
int32_t *th_cdc_fxp);
35+
/* Input is Q4.28, output is Q1.31 */
36+
int32_t sin_fixed_32b(int32_t th_rad_fxp);
37+
int32_t cos_fixed_32b(int32_t th_rad_fxp);
38+
/* Input is Q4.28, output is Q1.15 */
39+
int16_t sin_fixed_16b(int32_t th_rad_fxp);
40+
int16_t cos_fixed_16b(int32_t th_rad_fxp);
2041

2142
#endif /* __SOF_MATH_TRIG_H__ */

‎src/math/trig.c‎

Lines changed: 181 additions & 67 deletions
Original file line numberDiff line numberDiff line change
@@ -5,98 +5,212 @@
55
// Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
66
// Liam Girdwood <liam.r.girdwood@linux.intel.com>
77
// Keyon Jie <yang.jie@linux.intel.com>
8+
// Shriram Shastry <malladi.sastry@linux.intel.com>
89

910
#include <sof/audio/format.h>
1011
#include <sof/math/trig.h>
1112
#include <stdint.h>
1213

13-
#define CORDICSINE_TABLE_SIZE 31
14-
/* Compute fixed point cordicsine with table lookup and interpolation
15-
* The cordic sine algorithm converges, when the angle is in the range
16-
* [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
17-
* pi/2 is added or subtracted from the angle until it is within the range
18-
* [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
19-
* has range in [-1.0 to 1.0]
20-
* +------------------+-----------------+--------+--------+
21-
* | thRadFxp | cdcsinth |thRadFxp|cdcsinth|
22-
* +----+-----+-------+----+----+-------+--------+--------+
23-
* |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
24-
* +----+-----+-------+----+----+-------+--------+--------+
25-
* | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
26-
* +------------------+-----------------+--------+--------+
27-
*/
28-
14+
#ifdef CONFIG_CORDIC_TRIGONOMETRY_FIXED
2915
/* Use a local definition to avoid adding a dependency on <math.h> */
3016
#define _M_PI 3.14159265358979323846 /* pi */
31-
32-
int32_t sin_fixed(int32_t th_rad_fxp)
33-
{
34-
/*cordic_atan2_lookup_table = atan(2.^-(0:N-1)) N = 31
35-
*CORDIC Gain is cordic_gain = prod(sqrt(1 + 2.^(-2*(0:31-1))))
36-
*Inverse CORDIC Gain,inverse_cordic_gain = 1 / cordic_gain
37-
*/
38-
static const int32_t cordicsine_lookup[CORDICSINE_TABLE_SIZE] = { 843314857, 497837829,
17+
/*cordic_atan2_lookup_table = atan(2.^-(0:N-1)) N = 31/16
18+
*CORDIC Gain is cordic_gain = prod(sqrt(1 + 2.^(-2*(0:31/16-1))))
19+
*Inverse CORDIC Gain,inverse_cordic_gain = 1 / cordic_gain
20+
*/
21+
static const int32_t cordic_lookup[CORDIC_31B_TABLE_SIZE] = { 843314857, 497837829,
3922
263043837, 133525159, 67021687, 33543516, 16775851, 8388437, 4194283, 2097149,
4023
1048576, 524288, 262144, 131072, 65536, 32768, 16384, 8192, 4096, 2048, 1024,
4124
512, 256, 128, 64, 32, 16, 8, 4, 2, 1 };
42-
int32_t b_idx;
25+
26+
/* 652032874 , deg = 69.586061*/
27+
const int32_t cordic_sine_cos_lut_q29fl = Q_CONVERT_FLOAT(1.214505869895220, 29);
28+
/* 1686629713, deg = 90.000000 */
29+
const int32_t cordic_sine_cos_piovertwo_q30fl = Q_CONVERT_FLOAT(_M_PI / 2, 30);
30+
/* 421657428 , deg = 90.000000 */
31+
const int32_t cord_sincos_piovertwo_q28fl = Q_CONVERT_FLOAT(_M_PI / 2, 28);
32+
/* 843314857, deg = 90.000000 */
33+
const int32_t cord_sincos_piovertwo_q29fl = Q_CONVERT_FLOAT(_M_PI / 2, 29);
34+
35+
/**
36+
* \Compute fixed point cordicsine with table lookup and interpolation
37+
* \The cordic sine algorithm converges, when the angle is in the range
38+
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
39+
* \pi/2 is added or subtracted from the angle until it is within the range
40+
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
41+
* \has range in [-1.0 to 1.0]
42+
* \+------------------+-----------------+--------+--------+
43+
* \| thRadFxp | cdcsinth |thRadFxp|cdcsinth|
44+
* \+----+-----+-------+----+----+-------+--------+--------+
45+
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
46+
* \+----+-----+-------+----+----+-------+--------+--------+
47+
* \| 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
48+
* \+------------------+-----------------+--------+--------+
49+
*/
50+
inline int32_t sin_fixed_32b(int32_t th_rad_fxp)
51+
{
52+
int32_t sign;
53+
int32_t b_yn;
54+
int32_t xn;
55+
int32_t th_cdc_fxp;
56+
cordic_cfg type = EN_32B_CORDIC_SINE;
57+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
58+
th_cdc_fxp = sign * b_yn;
59+
/*convert Q2.30 to Q1.31 format*/
60+
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
61+
}
62+
/**
63+
* \Compute fixed point cordicsine with table lookup and interpolation
64+
* \The cordic cosine algorithm converges, when the angle is in the range
65+
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
66+
* \pi/2 is added or subtracted from the angle until it is within the range
67+
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
68+
* \has range in [-1.0 to 1.0]
69+
* \+------------------+-----------------+--------+--------+
70+
* \| thRadFxp | cdccosth |thRadFxp|cdccosth|
71+
* \+----+-----+-------+----+----+-------+--------+--------+
72+
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
73+
* \+----+-----+-------+----+----+-------+--------+--------+
74+
* \| 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
75+
* \+------------------+-----------------+--------+--------+
76+
*/
77+
inline int32_t cos_fixed_32b(int32_t th_rad_fxp)
78+
{
79+
int32_t sign;
80+
int32_t b_yn;
81+
int32_t xn;
82+
int32_t th_cdc_fxp;
83+
cordic_cfg type = EN_32B_CORDIC_COSINE;
84+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
85+
th_cdc_fxp = sign * xn;
86+
/*convert Q2.30 to Q1.31 format*/
87+
return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
88+
}
89+
90+
/**
91+
* \Compute fixed point cordic sine with table lookup and interpolation
92+
* \The cordic sine algorithm converges, when the angle is in the range
93+
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
94+
* \pi/2 is added or subtracted from the angle until it is within the range
95+
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
96+
* \has range in [-1.0 to 1.0]
97+
* \+------------------+-----------------+--------+------------+
98+
* \| thRadFxp | cdcsinth |thRadFxp| cdcsinth|
99+
* \+----+-----+-------+----+----+-------+--------+------------+
100+
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
101+
* \+----+-----+-------+----+----+-------+--------+------------+
102+
* \| 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
103+
* \+------------------+-----------------+--------+------------+
104+
*/
105+
inline int16_t sin_fixed_16b(int32_t th_rad_fxp)
106+
{
107+
int32_t sign;
43108
int32_t b_yn;
44109
int32_t xn;
110+
int32_t th_cdc_fxp;
111+
cordic_cfg type = EN_16B_CORDIC_SINE;
112+
/* compute coeff from angles*/
113+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
114+
th_cdc_fxp = sign * b_yn;
115+
/*convert Q1.31 to Q1.15 format*/
116+
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
117+
31, 15));
118+
}
119+
120+
/**
121+
* \Compute fixed point cordic cosine with table lookup and interpolation
122+
* \The cordic cos algorithm converges, when the angle is in the range
123+
* \[-pi/2, pi/2).If an angle is outside of this range, then a multiple of
124+
* \pi/2 is added or subtracted from the angle until it is within the range
125+
* \[-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
126+
* \has range in [-1.0 to 1.0]
127+
* \+------------------+-----------------+--------+------------+
128+
* \| thRadFxp | cdccosth |thRadFxp| cdccosth|
129+
* \+----+-----+-------+----+----+-------+--------+------------+
130+
* \|WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
131+
* \+----+-----+-------+----+----+-------+--------+------------+
132+
* \| 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
133+
* \+------------------+-----------------+--------+------------+
134+
*/
135+
inline int16_t cos_fixed_16b(int32_t th_rad_fxp)
136+
{
137+
int32_t sign;
138+
int32_t b_yn;
139+
int32_t xn;
140+
int32_t th_cdc_fxp;
141+
cordic_cfg type = EN_16B_CORDIC_COSINE;
142+
/* compute coeff from angles*/
143+
cordic_sin_cos(th_rad_fxp, type, &sign, &b_yn, &xn, &th_cdc_fxp);
144+
th_cdc_fxp = sign * xn;
145+
/*convert Q1.31 to Q1.15 format*/
146+
return sat_int16(Q_SHIFT_RND((sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31))),
147+
31, 15));
148+
}
149+
150+
/**
151+
* \CORDIC-based approximation of sine and cosine
152+
*/
153+
void cordic_sin_cos(int32_t th_rad_fxp, cordic_cfg type, int32_t *sign, int32_t *b_yn, int32_t *xn,
154+
int32_t *th_cdc_fxp)
155+
{
156+
int32_t b_idx;
45157
int32_t xtmp;
46158
int32_t ytmp;
47-
int sign = 1;
48-
/* 652032874 , deg = 69.586061*/
49-
const int32_t CORDICSINE_LUT_Q29FL = Q_CONVERT_FLOAT(1.214505869895220, 29);
50-
/* 421657428 , deg = 90.000000 */
51-
const int32_t CORDICSINE_PIOVERTWO_Q28FL = Q_CONVERT_FLOAT(_M_PI / 2, 28);
52-
/* 843314857, deg = 90.000000 */
53-
const int32_t CORDICSINE_PIOVERTWO_Q29FL = Q_CONVERT_FLOAT(_M_PI / 2, 29);
54-
/* 1686629713, deg = 90.000000 */
55-
const int32_t CORDICSINE_PIOVERTWO_Q30FL = Q_CONVERT_FLOAT(_M_PI / 2, 30);
56-
57-
/*Addition or subtraction by a multiple of pi/2 is done in the data type
58-
*of the input. When the fraction length is 29, then the quantization error
59-
*introduced by the addition or subtraction of pi/2 is done with 29 bits of
60-
*precision.Input range of cordicsin must be in the range [-2*pi, 2*pi),
61-
*a signed type with fractionLength = wordLength-4 will fit this range
62-
*without overflow.Increase of fractionLength makes the addition or
63-
*subtraction of a multiple of pi/2 more precise
159+
int32_t a_idx = CORDIC_31B_TABLE_SIZE;
160+
*sign = 1;
161+
/* Addition or subtraction by a multiple of pi/2 is done in the data type
162+
* of the input. When the fraction length is 29, then the quantization error
163+
* introduced by the addition or subtraction of pi/2 is done with 29 bits of
164+
* precision.Input range of cordicsin must be in the range [-2*pi, 2*pi),
165+
* a signed type with fractionLength = wordLength-4 will fit this range
166+
* without overflow.Increase of fractionLength makes the addition or
167+
* subtraction of a multiple of pi/2 more precise
64168
*/
65-
if (th_rad_fxp > CORDICSINE_PIOVERTWO_Q28FL) {
66-
if ((th_rad_fxp - CORDICSINE_PIOVERTWO_Q29FL) <= CORDICSINE_PIOVERTWO_Q28FL) {
67-
th_rad_fxp -= CORDICSINE_PIOVERTWO_Q29FL;
68-
sign = -1;
169+
if (th_rad_fxp > cord_sincos_piovertwo_q28fl) {
170+
if ((th_rad_fxp - cord_sincos_piovertwo_q29fl) <= cord_sincos_piovertwo_q28fl) {
171+
th_rad_fxp -= cord_sincos_piovertwo_q29fl;
172+
*sign = -1;
69173
} else {
70-
th_rad_fxp -= CORDICSINE_PIOVERTWO_Q30FL;
174+
th_rad_fxp -= cordic_sine_cos_piovertwo_q30fl;
71175
}
72-
} else if (th_rad_fxp < -CORDICSINE_PIOVERTWO_Q28FL) {
73-
if ((th_rad_fxp + CORDICSINE_PIOVERTWO_Q29FL) >= -CORDICSINE_PIOVERTWO_Q28FL) {
74-
th_rad_fxp += CORDICSINE_PIOVERTWO_Q29FL;
75-
sign = -1;
176+
} else if (th_rad_fxp < -cord_sincos_piovertwo_q28fl) {
177+
if ((th_rad_fxp + cord_sincos_piovertwo_q29fl) >= -cord_sincos_piovertwo_q28fl) {
178+
th_rad_fxp += cord_sincos_piovertwo_q29fl;
179+
*sign = -1;
76180
} else {
77-
th_rad_fxp += CORDICSINE_PIOVERTWO_Q30FL;
181+
th_rad_fxp += cordic_sine_cos_piovertwo_q30fl;
78182
}
79183
}
184+
80185
th_rad_fxp <<= 2;
81-
b_yn = 0;
82-
xn = CORDICSINE_LUT_Q29FL;
83-
xtmp = CORDICSINE_LUT_Q29FL;
186+
*b_yn = 0;
187+
*xn = cordic_sine_cos_lut_q29fl;
188+
xtmp = cordic_sine_cos_lut_q29fl;
84189
ytmp = 0;
85-
for (b_idx = 0; b_idx < CORDICSINE_TABLE_SIZE; b_idx++) {
190+
191+
if ((type == EN_16B_CORDIC_SINE) || (type == EN_16B_CORDIC_COSINE))
192+
a_idx = CORDIC_SIN_COS_15B_TABLE_SIZE;
193+
194+
/* Calculate the correct coefficient values from rotation angle.
195+
* Find difference between the coefficients from the lookup table
196+
* and those from the calculation
197+
*/
198+
for (b_idx = 0; b_idx < a_idx; b_idx++) {
86199
if (th_rad_fxp < 0) {
87-
th_rad_fxp += cordicsine_lookup[b_idx];
88-
xn += ytmp;
89-
b_yn -= xtmp;
200+
th_rad_fxp += cordic_lookup[b_idx];
201+
*xn += ytmp;
202+
*b_yn -= xtmp;
90203
} else {
91-
th_rad_fxp -= cordicsine_lookup[b_idx];
92-
xn -= ytmp;
93-
b_yn += xtmp;
204+
th_rad_fxp -= cordic_lookup[b_idx];
205+
*xn -= ytmp;
206+
*b_yn += xtmp;
94207
}
95-
xtmp = xn >> (b_idx + 1);
96-
ytmp = b_yn >> (b_idx + 1);
208+
xtmp = *xn >> (b_idx + 1);
209+
ytmp = *b_yn >> (b_idx + 1);
97210
}
211+
/* Q2.30 format */
212+
*th_cdc_fxp = th_rad_fxp;
98213

99-
th_rad_fxp = sign * b_yn;
100-
/*convert Q2.30 to Q1.31 format*/
101-
return sat_int32(Q_SHIFT_LEFT((int64_t)th_rad_fxp, 30, 31));
102214
}
215+
216+
#endif

0 commit comments

Comments
 (0)