1 /*
2 * SPDX-FileCopyrightText: 2019-2025 SiFli Technologies(Nanjing) Co., Ltd
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #ifndef __BF0_HAL_MATH_H
8 #define __BF0_HAL_MATH_H
9
10 #ifdef __cplusplus
11 extern "C" {
12 #endif
13
14 #ifndef SF32LB52X
15 #if defined(__CC_ARM) || defined(__CLANG_ARM) || (defined(__GNUC__) && (__GNUC__ > 9))
16 #define HAL_MATH_CDE_SUPPORT
17 #endif /* defined(__CC_ARM) || defined(__CLANG_ARM) || (defined(__GNUC__) && (__GNUC__ > 9)) */
18 #endif /* SF32LB52X */
19
20 /* Includes ------------------------------------------------------------------*/
21 #if defined(__CC_ARM) || defined(__ARMCC_VERSION)
22 #include <arm_acle.h>
23 #ifdef HAL_MATH_CDE_SUPPORT
24 #include <arm_cde.h>
25 #endif /* HAL_MATH_CDE_SUPPORT */
26 #elif defined(__ICCARM__)
27 #include <intrinsics.h>
28 #elif defined(__GNUC__)
29 #include <arm_acle.h>
30 #ifdef HAL_MATH_CDE_SUPPORT
31 #include <arm_cde.h>
32 #endif /* HAL_MATH_CDE_SUPPORT */
33 #endif /* defined(__CC_ARM) */
34
35 #include "bf0_hal_def.h"
36
37 /** @addtogroup MATH
38 * @ingroup BF0_HAL_Driver
39 * @{
40 */
41
HAL_MATH_DisableInterrupt(void)42 __STATIC_INLINE uint32_t HAL_MATH_DisableInterrupt(void)
43 {
44 uint32_t mask;
45
46 mask = __get_PRIMASK();
47 __set_PRIMASK(1);
48 return mask;
49 }
50
HAL_MATH_EnableInterrupt(uint32_t mask)51 __STATIC_INLINE void HAL_MATH_EnableInterrupt(uint32_t mask)
52 {
53 __set_PRIMASK(mask);
54 }
55
56
57 /** normal coprocessor for math */
58 #define CP_MATH_NORMAL (2)
59 /** coprocessor used by CDE */
60 #define CP_MATH_CDE (1)
61
62 #define CP_MATH_NORM_OP_SET_XY (0)
63 #define CP_MATH_NORM_OP_SET_X (0)
64 #define CP_MATH_NORM_OP_SET_Y (1)
65 #define CP_MATH_NORM_OP_SET_ANGLE (2)
66 #define CP_MATH_NORM_OP_SET_ITER_NUM (3)
67 #define CP_MATH_NORM_OP_SET_SCALE (4)
68 #define CP_MATH_NORM_OP_SET_MODE (5)
69
70 #define CP_MATH_NORM_MODE_COS (1)
71 #define CP_MATH_NORM_MODE_SIN (2)
72 #define CP_MATH_NORM_MODE_ATAN (3)
73 #define CP_MATH_NORM_MODE_MOD (4)
74 #define CP_MATH_NORM_MODE_ANG (5)
75 #define CP_MATH_NORM_MODE_ROT (6)
76 #define CP_MATH_NORM_MODE_MUL (7)
77 #define CP_MATH_NORM_MODE_DIV (8)
78 #define CP_MATH_NORM_MODE_COSH (9)
79 #define CP_MATH_NORM_MODE_SINH (10)
80 #define CP_MATH_NORM_MODE_ATANH (11)
81 #define CP_MATH_NORM_MODE_MODH (12)
82 #define CP_MATH_NORM_MODE_ANGH (13)
83 #define CP_MATH_NORM_MODE_SQRT (14)
84 #define CP_MATH_NORM_MODE_EXP (15)
85 #define CP_MATH_NORM_MODE_LN (16)
86
87 #define CP_MATH_CDE_MODE_COS (0)
88 #define CP_MATH_CDE_MODE_SIN (0)
89 #define CP_MATH_CDE_MODE_ATAN (0)
90 #define CP_MATH_CDE_MODE_MOD (1)
91 #define CP_MATH_CDE_MODE_ANG (2)
92 #define CP_MATH_CDE_MODE_ROT (3)
93 #define CP_MATH_CDE_MODE_MUL (4)
94 #define CP_MATH_CDE_MODE_DIV (5)
95 #define CP_MATH_CDE_MODE_COSH (6)
96 #define CP_MATH_CDE_MODE_SINH (7)
97 #define CP_MATH_CDE_MODE_ATANH (8)
98 #define CP_MATH_CDE_MODE_MODH (9)
99 #define CP_MATH_CDE_MODE_ANGH (10)
100 #define CP_MATH_CDE_MODE_SQRT (11)
101 #define CP_MATH_CDE_MODE_EXP (12)
102 #define CP_MATH_CDE_MODE_LN (13)
103
104
105 /**
106 * @brief Move x and y to 64bit register, y is put in high 32bit and x is put in low 32bit
107 *
108 * @param[in] x x value
109 * @param[in] y y value
110 *
111 * @return void
112 */
113 #define CP_MATH_SET_XY(x,y) __arm_mcrr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_XY, (((uint64_t)((uint32_t)(y))) << 32) | ((uint32_t)(x)), 0)
114 #define CP_MATH_SET_X(x) __arm_mcr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_X, (x), 0, 0, 0)
115 #define CP_MATH_SET_Y(y) __arm_mcr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_Y, (y), 0, 0, 0)
116 #define CP_MATH_SET_ANGLE(angle) __arm_mcr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_ANGLE, (angle), 0, 1, 2)
117 #define CP_MATH_SET_ITER_NUM(num) __arm_mcr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_ITER_NUM, (num), 0, 1, 3)
118 #define CP_MATH_SET_SCALE(scale) __arm_mcr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_SCALE, (scale), 0, 0, 0)
119 #define CP_MATH_SET_MODE(mode) __arm_mcr(CP_MATH_NORMAL, CP_MATH_NORM_OP_SET_MODE, (mode), 0, 1, 4)
120
121 #define CP_MATH_GET_RESULT_UINT64() __arm_mrrc(CP_MATH_NORMAL, 0, 0)
122 #define CP_MATH_GET_SCALE_AND_VAL(arg_result, arg_scale, arg_val) \
123 do \
124 { \
125 if (arg_scale) \
126 { \
127 *arg_scale = (int8_t)((arg_result >> 32) & UINT8_MAX); \
128 } \
129 if (arg_val) \
130 { \
131 *arg_val = (int32_t)(arg_result & UINT32_MAX); \
132 } \
133 } \
134 while (0)
135
136 #define CP_MATH_CDE_MAKE_CTRL_CODE(mode, iter_num) ((((uint8_t)mode) & 0xF) | (((uint8_t)iter_num & 0x1F) << 4))
137
138
139 /**
140 * @brief Calculate cosine and sine of an angle
141 *
142 * @param[in] angle angle in radians, devided by pi, Q1.31 format, range: [-1, 1], it's mapped to [-pi, pi]
143 * @param[out] cos_val cosine of the angle, Q1.31 format
144 * @param[out] sin_val sine of the angle, Q1.31 format
145 *
146 * @retval cosine value
147 */
HAL_CP_CosSin(int32_t angle,int32_t * cos_val,int32_t * sin_val)148 __STATIC_INLINE void HAL_CP_CosSin(int32_t angle, int32_t *cos_val, int32_t *sin_val)
149 {
150 uint64_t cos_sin;
151
152 CP_MATH_SET_ANGLE(angle);
153 CP_MATH_SET_ITER_NUM(11);
154 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_COS);
155
156 cos_sin = CP_MATH_GET_RESULT_UINT64();
157 if (cos_val)
158 {
159 *cos_val = (int32_t)(cos_sin & UINT32_MAX);
160 }
161 if (sin_val)
162 {
163 *sin_val = (int32_t)((cos_sin >> 32) & UINT32_MAX);
164 }
165
166 return;
167 }
168
169 /**
170 * @brief Calculate sine of an angle
171 *
172 * @param[in] angle angle in radians, devided by pi, Q1.31 format, range: [-1, 1], it's mapped to [-pi, pi]
173 *
174 * @retval sine value in Q1.31 format
175 */
HAL_CP_Sin(int32_t angle)176 __STATIC_INLINE int32_t HAL_CP_Sin(int32_t angle)
177 {
178 int32_t result;
179
180 CP_MATH_SET_ANGLE(angle);
181 CP_MATH_SET_ITER_NUM(11);
182 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_SIN);
183
184 result = (int32_t)(CP_MATH_GET_RESULT_UINT64() & UINT32_MAX);
185
186 return result;
187 }
188
189 /**
190 * @brief Calculate arctangent
191 * formula: atan*pi=atan(data*(2^scale))
192 *
193 * @param[in] scale scaling factor, range is [0,12]
194 * @param[in] data data in Q1.31 format
195 *
196 * @retval arctangent in radians, divided by pi, Q1.31 format, range is [-1, 1] which is mapped to [-pi, pi]
197 */
HAL_CP_Atan(int8_t scale,int32_t data)198 __STATIC_INLINE int32_t HAL_CP_Atan(int8_t scale, int32_t data)
199 {
200 int32_t result;
201
202 CP_MATH_SET_Y(data);
203 CP_MATH_SET_ITER_NUM(11);
204 CP_MATH_SET_SCALE(scale);
205 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_ATAN);
206
207 result = (int32_t)(CP_MATH_GET_RESULT_UINT64() & UINT32_MAX);
208 return result;
209 }
210
211 /**
212 * @brief Calculate magnitude(i.e. modulus) of the vector v=(x,y),
213 * formula: magnitude=mod*(2^scale)=sqrt(x^2 + y^2)
214 *
215 * @param[in] x x coordinate, Q1.31 format
216 * @param[in] y y coordinate, Q1.31 format
217 * @param[out] scale points to output scaling value, range is [-30, 1]
218 * @param[out] mod points to output modulus, Q1.31 format
219 *
220 * @retval void
221 */
HAL_CP_Mod(int32_t x,int32_t y,int8_t * scale,int32_t * mod)222 __STATIC_INLINE void HAL_CP_Mod(int32_t x, int32_t y, int8_t *scale, int32_t *mod)
223 {
224 uint64_t result ;
225
226 CP_MATH_SET_XY(x, y);
227 CP_MATH_SET_ITER_NUM(11);
228 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_MOD);
229
230 result = CP_MATH_GET_RESULT_UINT64();
231 CP_MATH_GET_SCALE_AND_VAL(result, scale, mod);
232
233 return;
234 }
235
236 /**
237 * @brief Calculate phase angle of the vector v=(x,y),
238 * formula: angle*pi=arctangent(y/x)
239 *
240 * @param[in] x x coordinate, Q1.31 format
241 * @param[in] y y coordinate, Q1.31 format
242 *
243 * @retval angle in radians devided by pi, range is [-1,1] which is mapped to [-pi, pi]
244 */
HAL_CP_Ang(int32_t x,int32_t y)245 __STATIC_INLINE int32_t HAL_CP_Ang(int32_t x, int32_t y)
246 {
247 uint64_t result ;
248
249 CP_MATH_SET_XY(x, y);
250 CP_MATH_SET_ITER_NUM(11);
251 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_ANG);
252
253 result = CP_MATH_GET_RESULT_UINT64();
254 return (result & UINT32_MAX);
255 }
256
257 /**
258 * @brief Calculate the rotated vector
259 *
260 * @param[in] x x coordinate, Q1.31 format, range is (-0.707, 0.707)
261 * @param[in] y y coordinate, Q1.31 format, range is (-0.707, 0.707)
262 * @param[in] angle angle in radians, devided by pi, Q1.31 format, range: [-1, 1], which is mapped to [-pi, pi]
263 * @param[out] x2 points to x coordinate of the rotated vector
264 * @param[out] y2 points to y coordinate of the rotated vector
265
266 * @return void
267 */
HAL_CP_Rot(int32_t x,int32_t y,int32_t angle,int32_t * x2,int32_t * y2)268 __STATIC_INLINE void HAL_CP_Rot(int32_t x, int32_t y, int32_t angle, int32_t *x2, int32_t *y2)
269 {
270 uint64_t result;
271
272 CP_MATH_SET_XY(x, y);
273 CP_MATH_SET_ANGLE(angle);
274 CP_MATH_SET_ITER_NUM(11);
275 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_ROT);
276
277 result = CP_MATH_GET_RESULT_UINT64();
278 if (x2)
279 {
280 *x2 = (int32_t)(result & UINT32_MAX);
281 }
282 if (y2)
283 {
284 *y2 = (int32_t)(result >> 32);
285 }
286
287 return;
288 }
289
290 /**
291 * @brief Multiple data0 by data1
292 * formula: val*(2^scale)=data0*data1
293 *
294 * @param[in] data0 data0 in Q1.31 format
295 * @param[in] data1 data1 in Q1.31 format
296 * @param[out] scale points to scaling factor, Q8.0 format, range is [-60,0]
297 * @param[out] val points to output value, Q1.31 format
298
299 * @return void
300 */
HAL_CP_Mul(int32_t data0,int32_t data1,int8_t * scale,int32_t * val)301 __STATIC_INLINE void HAL_CP_Mul(int32_t data0, int32_t data1, int8_t *scale, int32_t *val)
302 {
303 uint64_t result;
304
305 CP_MATH_SET_XY(data0, data1);
306 CP_MATH_SET_ITER_NUM(11);
307 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_MUL);
308
309 result = CP_MATH_GET_RESULT_UINT64();
310 CP_MATH_GET_SCALE_AND_VAL(result, scale, val);
311
312 return;
313 }
314
315
316 /**
317 * @brief Divide numeritor by denominator
318 * formula: num/den=val*(2^scale)
319 *
320 * @param[in] num numerator in Q1.31 format
321 * @param[in] den denominator in Q1.31 format
322 * @param[out] scale points to scaling factor, Q8.0 format, range is [-30,30]
323 * @param[out] val points to output vallue, Q1.31 format
324
325 * @return void
326 */
HAL_CP_Div(int32_t num,int32_t den,int8_t * scale,int32_t * val)327 __STATIC_INLINE void HAL_CP_Div(int32_t num, int32_t den, int8_t *scale, int32_t *val)
328 {
329 uint64_t result;
330
331 CP_MATH_SET_XY(den, num);
332 CP_MATH_SET_ITER_NUM(11);
333 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_DIV);
334
335 result = CP_MATH_GET_RESULT_UINT64();
336 CP_MATH_GET_SCALE_AND_VAL(result, scale, val);
337
338 return;
339 }
340
341 /**
342 * @brief Calculate cosineh of an angle
343 * formula: cosh*(2^output_scale)=cosh(angle*(2^scale)),
344 * angle*(2^scale) range is (-1.118, 1.118)
345 *
346 * @param[in] scale scaling factor, Q8.0 format, range is [0, 1]
347 * @param[in] angle angle in Q1.31 format, range: [-1, 1)
348 * @param[out] output_scale points to output scaling factor, Q8.0 format, range is [0, 1]
349 * @param[out] cosh points to output cosh value
350 *
351 * @return void
352 */
HAL_CP_Cosh(int8_t scale,int32_t angle,int8_t * output_scale,int32_t * cosh)353 __STATIC_INLINE void HAL_CP_Cosh(int8_t scale, int32_t angle, int8_t *output_scale, int32_t *cosh)
354 {
355 uint64_t result;
356
357 CP_MATH_SET_ANGLE(angle);
358 CP_MATH_SET_SCALE(scale);
359 CP_MATH_SET_ITER_NUM(12);
360 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_COSH);
361
362 result = CP_MATH_GET_RESULT_UINT64();
363 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, cosh);
364
365 return;
366 }
367
368 /**
369 * @brief Calculate sineh of an angle
370 * formula: sinh*(2^output_scale)=sinh(angle*(2^scale)),
371 * angle*(2^scale) range is (-1.118, 1.118)
372 *
373 * @param[in] scale scaling factor, Q8.0 format, range is [0,1]
374 * @param[in] angle angle in Q1.31 format, range: [-1, 1)
375 * @param[out] output_scale points to output scaling factor, Q8.0 format, range is [0, 1]
376 * @param[out] sinh points to output sinh value
377 *
378 * @return void
379 */
HAL_CP_Sinh(int8_t scale,int32_t angle,int8_t * output_scale,int32_t * sinh)380 __STATIC_INLINE void HAL_CP_Sinh(int8_t scale, int32_t angle, int8_t *output_scale, int32_t *sinh)
381 {
382 uint64_t result;
383
384 CP_MATH_SET_ANGLE(angle);
385 CP_MATH_SET_SCALE(scale);
386 CP_MATH_SET_ITER_NUM(12);
387 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_SINH);
388
389 result = CP_MATH_GET_RESULT_UINT64();
390 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, sinh);
391
392 return;
393 }
394
395 /**
396 * @brief Calculate atanh of data, data range is (-0.806, 0.806)
397 * formula: atanh*(2^scale)=atanh(data)
398 *
399 * @param[in] data data in Q1.31 format
400 * @param[out] scale points to output scaling factor, Q8.0 format, range is [0, 1]
401 * @param[out] atanh points to output atanh value
402 *
403 * @return void
404 */
HAL_CP_Atanh(int32_t data,int8_t * scale,int32_t * atanh)405 __STATIC_INLINE void HAL_CP_Atanh(int32_t data, int8_t *scale, int32_t *atanh)
406 {
407 uint64_t result;
408
409 CP_MATH_SET_Y(data);
410 CP_MATH_SET_ITER_NUM(12);
411 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_ATANH);
412
413 result = CP_MATH_GET_RESULT_UINT64();
414 CP_MATH_GET_SCALE_AND_VAL(result, scale, atanh);
415
416 return;
417 }
418
419 /**
420 * @brief Calculate sqrt(x^2-y^2)
421 * modh*(2^scale)=sqrt(x^2-y^2),
422 * x range is (-1, 1), y range is (-0.806, 0.806), and abs(x)>=abs(y)
423 *
424 * @param[in] x x coordinate, Q1.31 format
425 * @param[in] y y coordinate, Q1.31 format
426 * @param[out] scale points to output scaling factor in Q8.0 format
427 * @param[out] modh points to output modulush, Q1.31 format
428 *
429 * @return void
430 */
HAL_CP_Modh(int32_t x,int32_t y,int8_t * scale,int32_t * modh)431 __STATIC_INLINE void HAL_CP_Modh(int32_t x, int32_t y, int8_t *scale, int32_t *modh)
432 {
433 uint64_t result;
434
435 CP_MATH_SET_XY(x, y);
436 CP_MATH_SET_ITER_NUM(12);
437 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_MODH);
438
439 result = CP_MATH_GET_RESULT_UINT64();
440 CP_MATH_GET_SCALE_AND_VAL(result, scale, modh);
441
442 return;
443 }
444
445 /**
446 * @brief Calculate arctanh(y/x)
447 * angh*(2^scale)=arctanh(y/x),
448 * x range is (-1, 1), y range is (-0.806, 0.806), and abs(x)>=abs(y)
449 *
450 * @param[in] x x coordinate, Q1.31 format
451 * @param[in] y y coordinate, Q1.31 format
452 * @param[out] scale points to output scaling factor in Q8.0 format
453 * @param[out] angh points to output angh, Q1.31 format
454 *
455 * @return void
456 */
HAL_CP_Angh(int32_t x,int32_t y,int8_t * scale,int32_t * angh)457 __STATIC_INLINE void HAL_CP_Angh(int32_t x, int32_t y, int8_t *scale, int32_t *angh)
458 {
459 uint64_t result;
460
461 CP_MATH_SET_XY(x, y);
462 CP_MATH_SET_ITER_NUM(12);
463 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_ANGH);
464
465 result = CP_MATH_GET_RESULT_UINT64();
466 CP_MATH_GET_SCALE_AND_VAL(result, scale, angh);
467
468 return;
469 }
470
471 /**
472 * @brief Square root
473 * formula: output_val*(2^output_scale)=sqrt(input_val*(2^input_scale)),
474 * input_val*(2^input_scale) range is (0.027, 2.34)
475 *
476 * @param[in] input_scale input scaling factor in Q8.0 format, range is [0, 2]
477 * @param[in] input_val input value in Q1.31 format
478 * @param[out] output_scale ponits to output scaling factor in Q8.0 format, range is [0, 2]
479 * @param[out] output_val points to output value in Q1.31 format
480 *
481 * @return void
482 */
HAL_CP_Sqrt(int8_t input_scale,int32_t input_val,int8_t * output_scale,int32_t * output_val)483 __STATIC_INLINE void HAL_CP_Sqrt(int8_t input_scale, int32_t input_val, int8_t *output_scale, int32_t *output_val)
484 {
485 uint64_t result;
486
487 CP_MATH_SET_X(input_val);
488 CP_MATH_SET_SCALE(input_scale);
489 CP_MATH_SET_ITER_NUM(12);
490 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_SQRT);
491
492 result = CP_MATH_GET_RESULT_UINT64();
493 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, output_val);
494 return;
495 }
496
497 /**
498 * @brief Exp of the given value
499 * formula: output_val*(2^output_scale)=exp(input_val*(2^input_scale)),
500 * input_val*(2^input_scale) range is (-1.118, 1.118)
501 *
502 * @param[in] input_scale input scaling factor in Q8.0 format, range is [0, 1]
503 * @param[in] input_val input value in Q1.31 format
504 * @param[out] output_scale ponits to output scaling factor in Q8.0 format, range is [0, 2]
505 * @param[out] output_val points to output value in Q1.31 format
506 *
507 * @return void
508 */
HAL_CP_Exp(int8_t input_scale,int32_t input_val,int8_t * output_scale,int32_t * output_val)509 __STATIC_INLINE void HAL_CP_Exp(int8_t input_scale, int32_t input_val, int8_t *output_scale, int32_t *output_val)
510 {
511 uint64_t result;
512
513 CP_MATH_SET_ANGLE(input_val);
514 CP_MATH_SET_SCALE(input_scale);
515 CP_MATH_SET_ITER_NUM(12);
516 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_EXP);
517
518 result = CP_MATH_GET_RESULT_UINT64();
519 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, output_val);
520
521 return;
522 }
523
524 /**
525 * @brief Ln of the given value
526 * formula: output_val*(2^output_scale)=ln(input_val*(2^input_scale)),
527 input_val*(2^input_scale) range is (0.107, 9.35)
528 *
529 * @param[in] input_scale input scaling factor in Q8.0 format, range is [0, 4]
530 * @param[in] input_val input value in Q1.31 format
531 * @param[out] output_scale ponits to output scaling factor in Q8.0 format, range is [1, 2]
532 * @param[out] output_val points to output value in Q1.31 format
533 *
534 * @return void
535 */
HAL_CP_Ln(int8_t input_scale,int32_t input_val,int8_t * output_scale,int32_t * output_val)536 __STATIC_INLINE void HAL_CP_Ln(int8_t input_scale, int32_t input_val, int8_t *output_scale, int32_t *output_val)
537 {
538 uint64_t result;
539
540 CP_MATH_SET_X(input_val);
541 CP_MATH_SET_SCALE(input_scale);
542 CP_MATH_SET_ITER_NUM(12);
543 CP_MATH_SET_MODE(CP_MATH_NORM_MODE_LN);
544
545 result = CP_MATH_GET_RESULT_UINT64();
546 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, output_val);
547
548 return;
549 }
550
551
552 #ifdef HAL_MATH_CDE_SUPPORT
553 /**
554 * @brief Calculate cosine and sine of an angle
555 *
556 * @param[in] angle angle in radians, devided by pi, Q1.31 format, range: [-1, 1], it's mapped to [-pi, pi]
557 * @param[out] cos_val cosine of the angle, Q1.31 format
558 * @param[out] sin_val sine of the angle, Q1.31 format
559 *
560 * @retval cosine value
561 */
HAL_CDE_CosSin(int32_t angle,int32_t * cos_val,int32_t * sin_val)562 __STATIC_INLINE void HAL_CDE_CosSin(int32_t angle, int32_t *cos_val, int32_t *sin_val)
563 {
564 volatile uint64_t cos_sin;
565 uint32_t mask;
566
567 mask = HAL_MATH_DisableInterrupt();
568 cos_sin = __arm_cx2d(CP_MATH_CDE, angle,
569 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_COS, 11));
570 HAL_MATH_EnableInterrupt(mask);
571
572 if (cos_val)
573 {
574 *cos_val = (int32_t)(cos_sin & UINT32_MAX);
575 }
576 if (sin_val)
577 {
578 *sin_val = (int32_t)((cos_sin >> 32) & UINT32_MAX);
579 }
580
581 return;
582 }
583
584 /**
585 * @brief Calculate sine of an angle
586 *
587 * @param[in] angle angle in radians, devided by pi, Q1.31 format, range: [-1, 1], it's mapped to [-pi, pi]
588 *
589 * @retval sine value in Q1.31 format
590 */
HAL_CDE_Sin(int32_t angle)591 __STATIC_INLINE int32_t HAL_CDE_Sin(int32_t angle)
592 {
593 volatile int32_t result;
594 uint32_t mask;
595
596 mask = HAL_MATH_DisableInterrupt();
597 result = (int32_t)__arm_cx2(CP_MATH_CDE, angle,
598 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_SIN, 11));
599 HAL_MATH_EnableInterrupt(mask);
600
601 return result;
602 }
603
604 /**
605 * @brief Calculate arctangent
606 * formula: atan*pi=atan(data*(2^scale))
607 *
608 * @param[in] scale scaling factor, range is [0,12]
609 * @param[in] data data in Q1.31 format
610 *
611 * @retval arctangent in radians, divided by pi, Q1.31 format, range is [-1, 1] which is mapped to [-pi, pi]
612 */
HAL_CDE_Atan(int8_t scale,int32_t data)613 __STATIC_INLINE int32_t HAL_CDE_Atan(int8_t scale, int32_t data)
614 {
615 volatile int32_t result;
616 uint32_t mask;
617
618 mask = HAL_MATH_DisableInterrupt();
619 result = (int32_t)(__arm_cx2da(CP_MATH_CDE, scale, data,
620 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_ATAN, 11)) & UINT32_MAX);
621 HAL_MATH_EnableInterrupt(mask);
622
623 return result;
624 }
625
626 /**
627 * @brief Calculate magnitude(i.e. modulus) of the vector v=(x,y),
628 * formula: magnitude=mod*(2^scale)=sqrt(x^2 + y^2)
629 *
630 * @param[in] x x coordinate, Q1.31 format
631 * @param[in] y y coordinate, Q1.31 format
632 * @param[out] scale points to output scaling value, range is [-30, 1]
633 * @param[out] mod points to output modulus, Q1.31 format
634 *
635 * @retval void
636 */
HAL_CDE_Mod(int32_t x,int32_t y,int8_t * scale,int32_t * mod)637 __STATIC_INLINE void HAL_CDE_Mod(int32_t x, int32_t y, int8_t *scale, int32_t *mod)
638 {
639 volatile uint64_t result;
640 uint32_t mask;
641
642 mask = HAL_MATH_DisableInterrupt();
643 result = __arm_cx2da(CP_MATH_CDE, y, x,
644 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_MOD, 11));
645 HAL_MATH_EnableInterrupt(mask);
646
647 CP_MATH_GET_SCALE_AND_VAL(result, scale, mod);
648
649 return;
650 }
651
652 /**
653 * @brief Calculate phase angle of the vector v=(x,y),
654 * formula: angle*pi=arctangent(y/x)
655 *
656 * @param[in] x x coordinate, Q1.31 format
657 * @param[in] y y coordinate, Q1.31 format
658 *
659 * @retval angle in radians devided by pi, range is [-1,1] which is mapped to [-pi, pi]
660 */
HAL_CDE_Ang(int32_t x,int32_t y)661 __STATIC_INLINE int32_t HAL_CDE_Ang(int32_t x, int32_t y)
662 {
663 volatile uint64_t result;
664 uint32_t mask;
665
666 mask = HAL_MATH_DisableInterrupt();
667 result = __arm_cx2da(CP_MATH_CDE, y, x,
668 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_ANG, 11));
669 HAL_MATH_EnableInterrupt(mask);
670
671 return (result & UINT32_MAX);
672 }
673
674 /**
675 * @brief Calculate the rotated vector
676 *
677 * @param[in] x x coordinate, Q1.31 format, range is (-0.707, 0.707)
678 * @param[in] y y coordinate, Q1.31 format, range is (-0.707, 0.707)
679 * @param[in] angle angle in radians, devided by pi, Q1.31 format, range: [-1, 1], which is mapped to [-pi, pi]
680 * @param[out] x2 points to x coordinate of the rotated vector
681 * @param[out] y2 points to y coordinate of the rotated vector
682
683 * @return void
684 */
HAL_CDE_Rot(int32_t x,int32_t y,int32_t angle,int32_t * x2,int32_t * y2)685 __STATIC_INLINE void HAL_CDE_Rot(int32_t x, int32_t y, int32_t angle, int32_t *x2, int32_t *y2)
686 {
687 volatile uint64_t result;
688 uint32_t mask;
689
690 result = (uint64_t)((uint32_t)angle);
691 result = (result << 32) | (uint32_t)y;
692
693 mask = HAL_MATH_DisableInterrupt();
694 result = __arm_cx2da(CP_MATH_CDE, result, x,
695 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_ROT, 11));
696 HAL_MATH_EnableInterrupt(mask);
697
698 if (x2)
699 {
700 *x2 = (int32_t)(result & UINT32_MAX);
701 }
702 if (y2)
703 {
704 *y2 = (int32_t)(result >> 32);
705 }
706
707 return;
708 }
709
710 /**
711 * @brief Multiple data0 by data1
712 * formula: val*(2^scale)=data0*data1
713 *
714 * @param[in] data0 data0 in Q1.31 format
715 * @param[in] data1 data1 in Q1.31 format
716 * @param[out] scale points to scaling factor, Q8.0 format, range is [-60,0]
717 * @param[out] val points to output value, Q1.31 format
718
719 * @return void
720 */
HAL_CDE_Mul(int32_t data0,int32_t data1,int8_t * scale,int32_t * val)721 __STATIC_INLINE void HAL_CDE_Mul(int32_t data0, int32_t data1, int8_t *scale, int32_t *val)
722 {
723 volatile uint64_t result;
724 uint32_t mask;
725
726 mask = HAL_MATH_DisableInterrupt();
727 result = __arm_cx2da(CP_MATH_CDE, data1, data0,
728 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_MUL, 11));
729 HAL_MATH_EnableInterrupt(mask);
730
731 CP_MATH_GET_SCALE_AND_VAL(result, scale, val);
732 return;
733 }
734
735
736 /**
737 * @brief Divide numeritor by denominator
738 * formula: num/den=val*(2^scale)
739 *
740 * @param[in] num numerator in Q1.31 format
741 * @param[in] den denominator in Q1.31 format
742 * @param[out] scale points to scaling factor, Q8.0 format, range is [-30,30]
743 * @param[out] val points to output vallue, Q1.31 format
744
745 * @return void
746 */
HAL_CDE_Div(int32_t num,int32_t den,int8_t * scale,int32_t * val)747 __STATIC_INLINE void HAL_CDE_Div(int32_t num, int32_t den, int8_t *scale, int32_t *val)
748 {
749 volatile uint64_t result;
750 uint32_t mask;
751
752 mask = HAL_MATH_DisableInterrupt();
753 result = __arm_cx2da(CP_MATH_CDE, num, den,
754 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_DIV, 11));
755 HAL_MATH_EnableInterrupt(mask);
756
757 CP_MATH_GET_SCALE_AND_VAL(result, scale, val);
758
759 return;
760 }
761
762 /**
763 * @brief Calculate cosineh of an angle
764 * formula: cosh*(2^output_scale)=cosh(angle*(2^scale)),
765 * angle*(2^scale) range is (-1.118, 1.118)
766 *
767 * @param[in] scale scaling factor, Q8.0 format, range is [0, 1]
768 * @param[in] angle angle in Q1.31 format, range: [-1, 1)
769 * @param[out] output_scale points to output scaling factor, Q8.0 format, range is [0, 1]
770 * @param[out] cosh points to output cosh value
771 *
772 * @return void
773 */
HAL_CDE_Cosh(int8_t scale,int32_t angle,int8_t * output_scale,int32_t * cosh)774 __STATIC_INLINE void HAL_CDE_Cosh(int8_t scale, int32_t angle, int8_t *output_scale, int32_t *cosh)
775 {
776 volatile uint64_t result;
777 uint32_t mask;
778
779 mask = HAL_MATH_DisableInterrupt();
780 result = __arm_cx2da(CP_MATH_CDE, scale, angle,
781 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_COSH, 12));
782 HAL_MATH_EnableInterrupt(mask);
783
784 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, cosh);
785
786 return;
787 }
788
789 /**
790 * @brief Calculate sineh of an angle
791 * formula: sinh*(2^output_scale)=sinh(angle*(2^scale)),
792 * angle*(2^scale) range is (-1.118, 1.118)
793 *
794 * @param[in] scale scaling factor, Q8.0 format, range is [0,1]
795 * @param[in] angle angle in Q1.31 format, range: [-1, 1)
796 * @param[out] output_scale points to output scaling factor, Q8.0 format, range is [0, 1]
797 * @param[out] sinh points to output sinh value
798 *
799 * @return void
800 */
HAL_CDE_Sinh(int8_t scale,int32_t angle,int8_t * output_scale,int32_t * sinh)801 __STATIC_INLINE void HAL_CDE_Sinh(int8_t scale, int32_t angle, int8_t *output_scale, int32_t *sinh)
802 {
803 volatile uint64_t result;
804 uint32_t mask;
805
806 mask = HAL_MATH_DisableInterrupt();
807 result = __arm_cx2da(CP_MATH_CDE, scale, angle,
808 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_SINH, 12));
809 HAL_MATH_EnableInterrupt(mask);
810
811 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, sinh);
812
813 return;
814 }
815
816 /**
817 * @brief Calculate atanh of data, data range is (-0.806, 0.806)
818 * formula: atanh*(2^scale)=atanh(data)
819 *
820 * @param[in] data data in Q1.31 format
821 * @param[out] scale points to output scaling factor, Q8.0 format, range is [0, 1]
822 * @param[out] atanh points to output atanh value
823 *
824 * @return void
825 */
HAL_CDE_Atanh(int32_t data,int8_t * scale,int32_t * atanh)826 __STATIC_INLINE void HAL_CDE_Atanh(int32_t data, int8_t *scale, int32_t *atanh)
827 {
828 volatile uint64_t result;
829 uint32_t mask;
830
831 mask = HAL_MATH_DisableInterrupt();
832 result = __arm_cx2da(CP_MATH_CDE, 0, data,
833 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_ATANH, 12));
834 HAL_MATH_EnableInterrupt(mask);
835
836 CP_MATH_GET_SCALE_AND_VAL(result, scale, atanh);
837
838 return;
839 }
840
841 /**
842 * @brief Calculate sqrt(x^2-y^2)
843 * modh*(2^scale)=sqrt(x^2-y^2),
844 * x range is (-1, 1), y range is (-0.806, 0.806), and abs(x)>=abs(y)
845 *
846 * @param[in] x x coordinate, Q1.31 format
847 * @param[in] y y coordinate, Q1.31 format
848 * @param[out] scale points to output scaling factor in Q8.0 format
849 * @param[out] modh points to output modulush, Q1.31 format
850 *
851 * @return void
852 */
HAL_CDE_Modh(int32_t x,int32_t y,int8_t * scale,int32_t * modh)853 __STATIC_INLINE void HAL_CDE_Modh(int32_t x, int32_t y, int8_t *scale, int32_t *modh)
854 {
855 volatile uint64_t result;
856 uint32_t mask;
857
858 mask = HAL_MATH_DisableInterrupt();
859 result = __arm_cx2da(CP_MATH_CDE, y, x,
860 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_MODH, 12));
861 HAL_MATH_EnableInterrupt(mask);
862
863 CP_MATH_GET_SCALE_AND_VAL(result, scale, modh);
864
865 return;
866 }
867
868 /**
869 * @brief Calculate arctanh(y/x)
870 * angh*(2^scale)=arctanh(y/x),
871 * x range is (-1, 1), y range is (-0.806, 0.806), and abs(x)>=abs(y)
872 *
873 * @param[in] x x coordinate, Q1.31 format
874 * @param[in] y y coordinate, Q1.31 format
875 * @param[out] scale points to output scaling factor in Q8.0 format
876 * @param[out] angh points to output angh, Q1.31 format
877 *
878 * @return void
879 */
HAL_CDE_Angh(int32_t x,int32_t y,int8_t * scale,int32_t * angh)880 __STATIC_INLINE void HAL_CDE_Angh(int32_t x, int32_t y, int8_t *scale, int32_t *angh)
881 {
882 volatile uint64_t result;
883 uint32_t mask;
884
885 mask = HAL_MATH_DisableInterrupt();
886 result = __arm_cx2da(CP_MATH_CDE, y, x,
887 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_ANGH, 12));
888 HAL_MATH_EnableInterrupt(mask);
889
890 CP_MATH_GET_SCALE_AND_VAL(result, scale, angh);
891
892 return;
893 }
894
895 /**
896 * @brief Square root
897 * formula: output_val*(2^output_scale)=sqrt(input_val*(2^input_scale)),
898 * input_val*(2^input_scale) range is (0.027, 2.34)
899 *
900 * @param[in] input_scale input scaling factor in Q8.0 format, range is [0, 2]
901 * @param[in] input_val input value in Q1.31 format
902 * @param[out] output_scale ponits to output scaling factor in Q8.0 format, range is [0, 2]
903 * @param[out] output_val points to output value in Q1.31 format
904 *
905 * @return void
906 */
HAL_CDE_Sqrt(int8_t input_scale,int32_t input_val,int8_t * output_scale,int32_t * output_val)907 __STATIC_INLINE void HAL_CDE_Sqrt(int8_t input_scale, int32_t input_val, int8_t *output_scale, int32_t *output_val)
908 {
909 volatile uint64_t result;
910 uint32_t mask;
911
912 mask = HAL_MATH_DisableInterrupt();
913 result = __arm_cx2da(CP_MATH_CDE, input_scale, input_val,
914 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_SQRT, 12));
915 HAL_MATH_EnableInterrupt(mask);
916
917 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, output_val);
918
919 return;
920 }
921
922 /**
923 * @brief Exp of the given value
924 * formula: output_val*(2^output_scale)=exp(input_val*(2^input_scale)),
925 * input_val*(2^input_scale) range is (-1.118, 1.118)
926 *
927 * @param[in] input_scale input scaling factor in Q8.0 format, range is [0, 1]
928 * @param[in] input_val input value in Q1.31 format
929 * @param[out] output_scale ponits to output scaling factor in Q8.0 format, range is [0, 2]
930 * @param[out] output_val points to output value in Q1.31 format
931 *
932 * @return void
933 */
HAL_CDE_Exp(int8_t input_scale,int32_t input_val,int8_t * output_scale,int32_t * output_val)934 __STATIC_INLINE void HAL_CDE_Exp(int8_t input_scale, int32_t input_val, int8_t *output_scale, int32_t *output_val)
935 {
936 volatile uint64_t result;
937 uint32_t mask;
938
939 mask = HAL_MATH_DisableInterrupt();
940 result = __arm_cx2da(CP_MATH_CDE, input_scale, input_val,
941 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_EXP, 12));
942 HAL_MATH_EnableInterrupt(mask);
943
944 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, output_val);
945
946 return;
947 }
948
949 /**
950 * @brief Ln of the given value
951 * formula: output_val*(2^output_scale)=ln(input_val*(2^input_scale)),
952 input_val*(2^input_scale) range is (0.107, 9.35)
953 *
954 * @param[in] input_scale input scaling factor in Q8.0 format, range is [0, 4]
955 * @param[in] input_val input value in Q1.31 format
956 * @param[out] output_scale ponits to output scaling factor in Q8.0 format, range is [1, 2]
957 * @param[out] output_val points to output value in Q1.31 format
958 *
959 * @return void
960 */
HAL_CDE_Ln(int8_t input_scale,int32_t input_val,int8_t * output_scale,int32_t * output_val)961 __STATIC_INLINE void HAL_CDE_Ln(int8_t input_scale, int32_t input_val, int8_t *output_scale, int32_t *output_val)
962 {
963 volatile uint64_t result;
964 uint32_t mask;
965
966 mask = HAL_MATH_DisableInterrupt();
967 result = __arm_cx2da(CP_MATH_CDE, input_scale, input_val,
968 CP_MATH_CDE_MAKE_CTRL_CODE(CP_MATH_CDE_MODE_LN, 12));
969 HAL_MATH_EnableInterrupt(mask);
970
971 CP_MATH_GET_SCALE_AND_VAL(result, output_scale, output_val);
972
973 return;
974 }
975
976 #endif /* HAL_MATH_CDE_SUPPORT */
977
978 #ifdef __cplusplus
979 }
980 #endif
981
982 ///@} MATH
983
984 #endif /* __BF0_HAL_MATH_H */
