1 /* SPDX-License-Identifier: BSD-3-Clause
2 *
3 * Copyright(c) 2016 Intel Corporation. All rights reserved.
4 *
5 * Author: Seppo Ingalsuo <seppo.ingalsuo@linux.intel.com>
6 * Liam Girdwood <liam.r.girdwood@linux.intel.com>
7 * Keyon Jie <yang.jie@linux.intel.com>
8 * Shriram Shastry <malladi.sastry@linux.intel.com>
9 */
10
11 #ifndef __SOF_MATH_TRIG_H__
12 #define __SOF_MATH_TRIG_H__
13
14 #include <stdint.h>
15
16 #ifndef UNIT_CORDIC_TEST
17 #define CONFIG_CORDIC_TRIGONOMETRY_FIXED
18 #endif
19
20 #define PI_DIV2_Q4_28 421657428
21 #define PI_DIV2_Q3_29 843314856
22 #define PI_Q4_28 843314857
23 #define PI_MUL2_Q4_28 1686629713
24 #define CORDIC_31B_TABLE_SIZE 31
25 #define CORDIC_15B_TABLE_SIZE 15
26 #define CORDIC_30B_ITABLE_SIZE 30
27 #define CORDIC_16B_ITABLE_SIZE 16
28
29 typedef enum {
30 EN_32B_CORDIC_SINE,
31 EN_32B_CORDIC_COSINE,
32 EN_32B_CORDIC_CEXP,
33 EN_16B_CORDIC_SINE,
34 EN_16B_CORDIC_COSINE,
35 EN_16B_CORDIC_CEXP,
36 } cordic_cfg;
37
38 struct cordic_cmpx {
39 int32_t re;
40 int32_t im;
41 };
42
43 void cordic_approx(int32_t th_rad_fxp, int32_t a_idx, int32_t *sign, int32_t *b_yn, int32_t *xn,
44 int32_t *th_cdc_fxp);
45 int32_t is_scalar_cordic_acos(int32_t realvalue, int16_t numiters);
46 int32_t is_scalar_cordic_asin(int32_t realvalue, int16_t numiters);
47 void cmpx_cexp(int32_t sign, int32_t b_yn, int32_t xn, cordic_cfg type, struct cordic_cmpx *cexp);
48 /* Input is Q4.28, output is Q1.31 */
49 /**
50 * Compute fixed point cordicsine with table lookup and interpolation
51 * The cordic sine algorithm converges, when the angle is in the range
52 * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
53 * pi/2 is added or subtracted from the angle until it is within the range
54 * [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
55 * has range in [-1.0 to 1.0]
56 * +------------------+-----------------+--------+--------+
57 * | thRadFxp | cdcsinth |thRadFxp|cdcsinth|
58 * +----+-----+-------+----+----+-------+--------+--------+
59 * |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
60 * +----+-----+-------+----+----+-------+--------+--------+
61 * | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
62 * +------------------+-----------------+--------+--------+
63 */
sin_fixed_32b(int32_t th_rad_fxp)64 static inline int32_t sin_fixed_32b(int32_t th_rad_fxp)
65 {
66 int32_t th_cdc_fxp;
67 int32_t sign;
68 int32_t b_yn;
69 int32_t xn;
70 cordic_approx(th_rad_fxp, CORDIC_31B_TABLE_SIZE, &sign, &b_yn, &xn, &th_cdc_fxp);
71
72 th_cdc_fxp = sign * b_yn;
73 /*convert Q2.30 to Q1.31 format*/
74 return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
75 }
76
77 /**
78 * Compute fixed point cordicsine with table lookup and interpolation
79 * The cordic cosine algorithm converges, when the angle is in the range
80 * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
81 * pi/2 is added or subtracted from the angle until it is within the range
82 * [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
83 * has range in [-1.0 to 1.0]
84 * +------------------+-----------------+--------+--------+
85 * | thRadFxp | cdccosth |thRadFxp|cdccosth|
86 * +----+-----+-------+----+----+-------+--------+--------+
87 * |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat|
88 * +----+-----+-------+----+----+-------+--------+--------+
89 * | 32 | 28 | 1 | 32 | 31 | 1 | 4.28 | 1.31 |
90 * +------------------+-----------------+--------+--------+
91 */
cos_fixed_32b(int32_t th_rad_fxp)92 static inline int32_t cos_fixed_32b(int32_t th_rad_fxp)
93 {
94 int32_t th_cdc_fxp;
95 int32_t sign;
96 int32_t b_yn;
97 int32_t xn;
98 cordic_approx(th_rad_fxp, CORDIC_31B_TABLE_SIZE, &sign, &b_yn, &xn, &th_cdc_fxp);
99
100 th_cdc_fxp = sign * xn;
101 /*convert Q2.30 to Q1.31 format*/
102 return sat_int32(Q_SHIFT_LEFT((int64_t)th_cdc_fxp, 30, 31));
103 }
104
105 /* Input is Q4.28, output is Q1.15 */
106 /**
107 * Compute fixed point cordic sine with table lookup and interpolation
108 * The cordic sine algorithm converges, when the angle is in the range
109 * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
110 * pi/2 is added or subtracted from the angle until it is within the range
111 * [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
112 * has range in [-1.0 to 1.0]
113 * +------------------+-----------------+--------+------------+
114 * | thRadFxp | cdcsinth |thRadFxp| cdcsinth|
115 * +----+-----+-------+----+----+-------+--------+------------+
116 * |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
117 * +----+-----+-------+----+----+-------+--------+------------+
118 * | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
119 * +------------------+-----------------+--------+------------+
120 */
sin_fixed_16b(int32_t th_rad_fxp)121 static inline int16_t sin_fixed_16b(int32_t th_rad_fxp)
122 {
123 int32_t th_cdc_fxp;
124 int32_t sign;
125 int32_t b_yn;
126 int32_t xn;
127
128 cordic_approx(th_rad_fxp, CORDIC_15B_TABLE_SIZE, &sign, &b_yn, &xn, &th_cdc_fxp);
129
130 th_cdc_fxp = sign * b_yn;
131 /*convert Q2.30 to Q1.15 format*/
132 return sat_int16(Q_SHIFT_RND(th_cdc_fxp, 30, 15));
133 }
134
135 /**
136 * Compute fixed point cordic cosine with table lookup and interpolation
137 * The cordic cos algorithm converges, when the angle is in the range
138 * [-pi/2, pi/2).If an angle is outside of this range, then a multiple of
139 * pi/2 is added or subtracted from the angle until it is within the range
140 * [-pi/2,pi/2).Start with the angle in the range [-2*pi, 2*pi) and output
141 * has range in [-1.0 to 1.0]
142 * +------------------+-----------------+--------+------------+
143 * | thRadFxp | cdccosth |thRadFxp| cdccosth|
144 * +----+-----+-------+----+----+-------+--------+------------+
145 * |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
146 * +----+-----+-------+----+----+-------+--------+------------+
147 * | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
148 * +------------------+-----------------+--------+------------+
149 */
cos_fixed_16b(int32_t th_rad_fxp)150 static inline int16_t cos_fixed_16b(int32_t th_rad_fxp)
151 {
152 int32_t th_cdc_fxp;
153 int32_t sign;
154 int32_t b_yn;
155 int32_t xn;
156
157 cordic_approx(th_rad_fxp, CORDIC_15B_TABLE_SIZE, &sign, &b_yn, &xn, &th_cdc_fxp);
158
159 th_cdc_fxp = sign * xn;
160 /*convert Q2.30 to Q1.15 format*/
161 return sat_int16(Q_SHIFT_RND(th_cdc_fxp, 30, 15));
162 }
163
164 /**
165 * CORDIC-based approximation of complex exponential e^(j*THETA).
166 * computes COS(THETA) + j*SIN(THETA) using CORDIC algorithm
167 * approximation and returns the complex result.
168 * THETA values must be in the range [-2*pi, 2*pi). The cordic
169 * exponential algorithm converges, when the angle is in the
170 * range [-pi/2, pi/2).If an angle is outside of this range,
171 * then a multiple of pi/2 is added or subtracted from the
172 * angle until it is within the range [-pi/2,pi/2).Start
173 * with the angle in the range [-2*pi, 2*pi) and output has
174 * range in [-1.0 to 1.0]
175 * Error (max = 0.000000015832484), THD+N = -167.082852232808847
176 * +------------------+-----------------+--------+------------+
177 * | thRadFxp |cdccexpth |thRadFxp| cdccexpth|
178 * +----+-----+-------+----+----+-------+--------+------------+
179 * |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
180 * +----+-----+-------+----+----+-------+--------+------------+
181 * | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 2.30 |
182 * +------------------+-----------------+--------+------------+
183 */
cmpx_exp_32b(int32_t th_rad_fxp,struct cordic_cmpx * cexp)184 static inline void cmpx_exp_32b(int32_t th_rad_fxp, struct cordic_cmpx *cexp)
185 {
186 int32_t th_cdc_fxp;
187 int32_t sign;
188 int32_t b_yn;
189 int32_t xn;
190
191 cordic_approx(th_rad_fxp, CORDIC_31B_TABLE_SIZE, &sign, &b_yn, &xn, &th_cdc_fxp);
192 cmpx_cexp(sign, b_yn, xn, EN_32B_CORDIC_CEXP, cexp);
193 /* return the complex(re & im) result in Q2.30*/
194 }
195
196 /**
197 * CORDIC-based approximation of complex exponential e^(j*THETA).
198 * computes COS(THETA) + j*SIN(THETA) using CORDIC algorithm
199 * approximation and returns the complex result.
200 * THETA values must be in the range [-2*pi, 2*pi). The cordic
201 * exponential algorithm converges, when the angle is in the
202 * range [-pi/2, pi/2).If an angle is outside of this range,
203 * then a multiple of pi/2 is added or subtracted from the
204 * angle until it is within the range [-pi/2,pi/2).Start
205 * with the angle in the range [-2*pi, 2*pi) and output has
206 * range in [-1.0 to 1.0]
207 * Error (max = 0.000060862861574), THD+N = -89.049303454077403
208 * +------------------+-----------------+--------+------------+
209 * | thRadFxp |cdccexpth |thRadFxp| cdccexpth|
210 * +----+-----+-------+----+----+-------+--------+------------+
211 * |WLen| FLen|Signbit|WLen|FLen|Signbit| Qformat| Qformat |
212 * +----+-----+-------+----+----+-------+--------+------------+
213 * | 32 | 28 | 1 | 32 | 15 | 1 | 4.28 | 1.15 |
214 * +------------------+-----------------+--------+------------+
215 */
cmpx_exp_16b(int32_t th_rad_fxp,struct cordic_cmpx * cexp)216 static inline void cmpx_exp_16b(int32_t th_rad_fxp, struct cordic_cmpx *cexp)
217 {
218 int32_t th_cdc_fxp;
219 int32_t sign;
220 int32_t b_yn;
221 int32_t xn;
222
223 /* compute coeff from angles */
224 cordic_approx(th_rad_fxp, CORDIC_15B_TABLE_SIZE, &sign, &b_yn, &xn, &th_cdc_fxp);
225 cmpx_cexp(sign, b_yn, xn, EN_16B_CORDIC_CEXP, cexp);
226 /* return the complex(re & im) result in Q1.15*/
227 }
228
229 /**
230 * CORDIC-based approximation of inverse sine
231 * inverse sine of cdc_asin_theta based on a CORDIC approximation.
232 * asin(cdc_asin_th) inverse sine angle values in radian produces using the DCORDIC
233 * (Double CORDIC) algorithm.
234 * Inverse sine angle values in rad
235 * Q2.30 cdc_asin_th, value in between range of [-1 to 1]
236 * Q2.30 th_asin_fxp, output value range [-1.5707963258028 to 1.5707963258028]
237 * LUT size set type 15
238 * Error (max = 0.000000027939677), THD+N = -157.454534077921551 (dBc)
239 */
asin_fixed_32b(int32_t cdc_asin_th)240 static inline int32_t asin_fixed_32b(int32_t cdc_asin_th)
241 {
242 int32_t th_asin_fxp;
243
244 if (cdc_asin_th >= 0)
245 th_asin_fxp = is_scalar_cordic_asin(cdc_asin_th,
246 CORDIC_31B_TABLE_SIZE);
247 else
248 th_asin_fxp = -is_scalar_cordic_asin(-cdc_asin_th,
249 CORDIC_31B_TABLE_SIZE);
250
251 return th_asin_fxp; /* Q2.30 */
252 }
253
254 /**
255 * CORDIC-based approximation of inverse cosine
256 * inverse cosine of cdc_acos_theta based on a CORDIC approximation
257 * acos(cdc_acos_th) inverse cosine angle values in radian produces using the DCORDIC
258 * (Double CORDIC) algorithm.
259 * Q2.30 cdc_acos_th , input value range [-1 to 1]
260 * Q3.29 th_acos_fxp, output value range [3.14159265346825 to 0]
261 * LUT size set type 31
262 * Error (max = 0.000000026077032), THD+N = -157.948952635422842 (dBc)
263 */
acos_fixed_32b(int32_t cdc_acos_th)264 static inline int32_t acos_fixed_32b(int32_t cdc_acos_th)
265 {
266 int32_t th_acos_fxp;
267
268 if (cdc_acos_th >= 0)
269 th_acos_fxp = is_scalar_cordic_acos(cdc_acos_th,
270 CORDIC_31B_TABLE_SIZE);
271 else
272 th_acos_fxp =
273 PI_MUL2_Q4_28 - is_scalar_cordic_acos(-cdc_acos_th,
274 CORDIC_31B_TABLE_SIZE);
275
276 return th_acos_fxp; /* Q3.29 */
277 }
278
279 /**
280 * CORDIC-based approximation of inverse sine
281 * inverse sine of cdc_asin_theta based on a CORDIC approximation.
282 * asin(cdc_asin_th) inverse sine angle values in radian produces using the DCORDIC
283 * (Double CORDIC) algorithm.
284 * Inverse sine angle values in rad
285 * Q2.30 cdc_asin_th, value in between range of [-1 to 1]
286 * Q2.14 th_asin_fxp, output value range [-1.5707963258028 to 1.5707963258028]
287 * LUT size set type 31
288 * number of iteration 15
289 * Error (max = 0.000059800222516), THD+N = -89.824282520774048 (dBc)
290 */
asin_fixed_16b(int32_t cdc_asin_th)291 static inline int16_t asin_fixed_16b(int32_t cdc_asin_th)
292 {
293 int32_t th_asin_fxp;
294
295 if (cdc_asin_th >= 0)
296 th_asin_fxp = is_scalar_cordic_asin(cdc_asin_th,
297 CORDIC_16B_ITABLE_SIZE);
298 else
299 th_asin_fxp = -is_scalar_cordic_asin(-cdc_asin_th,
300 CORDIC_16B_ITABLE_SIZE);
301 /*convert Q2.30 to Q2.14 format*/
302 return sat_int16(Q_SHIFT_RND(th_asin_fxp, 30, 14));
303 }
304
305 /**
306 * CORDIC-based approximation of inverse cosine
307 * inverse cosine of cdc_acos_theta based on a CORDIC approximation
308 * acos(cdc_acos_th) inverse cosine angle values in radian produces using the DCORDIC
309 * (Double CORDIC) algorithm.
310 * Q2.30 cdc_acos_th , input value range [-1 to 1]
311 * Q3.13 th_acos_fxp, output value range [3.14159265346825 to 0]
312 * LUT size set type 31
313 * number of iteration 15
314 * Error (max = 0.000059799232976), THD+N = -89.824298401466635 (dBc)
315 */
acos_fixed_16b(int32_t cdc_acos_th)316 static inline int16_t acos_fixed_16b(int32_t cdc_acos_th)
317 {
318 int32_t th_acos_fxp;
319
320 if (cdc_acos_th >= 0)
321 th_acos_fxp = is_scalar_cordic_acos(cdc_acos_th,
322 CORDIC_16B_ITABLE_SIZE);
323 else
324 th_acos_fxp =
325 PI_MUL2_Q4_28 - is_scalar_cordic_acos(-cdc_acos_th,
326 CORDIC_16B_ITABLE_SIZE);
327
328 /*convert Q3.29 to Q3.13 format*/
329 return sat_int16(Q_SHIFT_RND(th_acos_fxp, 29, 13));
330 }
331
332 #endif /* __SOF_MATH_TRIG_H__ */
333
