1 /*
2 * Copyright 2018-2022 NXP
3 * All rights reserved.
4 *
5 * SPDX-License-Identifier: BSD-3-Clause
6 */
7
8 #include "fsl_powerquad.h"
9
10 /*******************************************************************************
11 * Definitions
12 ******************************************************************************/
13
14 /* Component ID definition, used by tools. */
15 #ifndef FSL_COMPONENT_ID
16 #define FSL_COMPONENT_ID "platform.drivers.powerquad_math"
17 #endif
18
19 /*******************************************************************************
20 * Code
21 ******************************************************************************/
PQ_VectorLnF32(float * pSrc,float * pDst,int32_t length)22 void PQ_VectorLnF32(float *pSrc, float *pDst, int32_t length)
23 {
24 int32_t remainderBy8 = length % 8;
25 pq_float_t val;
26
27 if (remainderBy8 > 0)
28 {
29 length -= remainderBy8;
30 while ((remainderBy8--) > 0)
31 {
32 val.floatX = *pSrc++;
33 _pq_ln0(val.integerX);
34 val.integerX = _pq_readAdd0();
35 *pDst++ = val.floatX;
36 }
37 }
38
39 if (length > 0)
40 {
41 PQ_StartVector(pSrc, pDst, length);
42 PQ_Vector8F32(PQ_LN, 1, PQ_TRANS);
43 PQ_EndVector();
44 }
45 }
46
PQ_VectorInvF32(float * pSrc,float * pDst,int32_t length)47 void PQ_VectorInvF32(float *pSrc, float *pDst, int32_t length)
48 {
49 int32_t remainderBy8 = length % 8;
50 pq_float_t val;
51
52 if (remainderBy8 > 0)
53 {
54 length -= remainderBy8;
55 while ((remainderBy8--) > 0)
56 {
57 val.floatX = *pSrc++;
58 _pq_inv0(val.integerX);
59 val.integerX = _pq_readMult0();
60 *pDst++ = val.floatX;
61 }
62 }
63
64 if (length > 0)
65 {
66 PQ_StartVector(pSrc, pDst, length);
67 PQ_Vector8F32(PQ_INV, 0, PQ_TRANS);
68 PQ_EndVector();
69 }
70 }
71
PQ_VectorSqrtF32(float * pSrc,float * pDst,int32_t length)72 void PQ_VectorSqrtF32(float *pSrc, float *pDst, int32_t length)
73 {
74 int32_t remainderBy8 = length % 8;
75 pq_float_t val;
76
77 if (remainderBy8 > 0)
78 {
79 length -= remainderBy8;
80 while ((remainderBy8--) > 0)
81 {
82 val.floatX = *pSrc++;
83 _pq_sqrt0(val.integerX);
84 val.integerX = _pq_readMult0();
85 *pDst++ = val.floatX;
86 }
87 }
88
89 if (length > 0)
90 {
91 PQ_StartVector(pSrc, pDst, length);
92 PQ_Vector8F32(PQ_SQRT, 0, PQ_TRANS);
93 PQ_EndVector();
94 }
95 }
96
PQ_VectorInvSqrtF32(float * pSrc,float * pDst,int32_t length)97 void PQ_VectorInvSqrtF32(float *pSrc, float *pDst, int32_t length)
98 {
99 int32_t remainderBy8 = length % 8;
100 pq_float_t val;
101
102 if (remainderBy8 > 0)
103 {
104 length -= remainderBy8;
105 while ((remainderBy8--) > 0)
106 {
107 val.floatX = *pSrc++;
108 _pq_invsqrt0(val.integerX);
109 val.integerX = _pq_readMult0();
110 *pDst++ = val.floatX;
111 }
112 }
113
114 if (length > 0)
115 {
116 PQ_StartVector(pSrc, pDst, length);
117 PQ_Vector8F32(PQ_INVSQRT, 0, PQ_TRANS);
118 PQ_EndVector();
119 }
120 }
121
PQ_VectorEtoxF32(float * pSrc,float * pDst,int32_t length)122 void PQ_VectorEtoxF32(float *pSrc, float *pDst, int32_t length)
123 {
124 int32_t remainderBy8 = length % 8;
125 pq_float_t val;
126
127 if (remainderBy8 > 0)
128 {
129 length -= remainderBy8;
130 while ((remainderBy8--) > 0)
131 {
132 val.floatX = *pSrc++;
133 _pq_etox0(val.integerX);
134 val.integerX = _pq_readMult0();
135 *pDst++ = val.floatX;
136 }
137 }
138
139 if (length > 0)
140 {
141 PQ_StartVector(pSrc, pDst, length);
142 PQ_Vector8F32(PQ_ETOX, 0, PQ_TRANS);
143 PQ_EndVector();
144 }
145 }
146
PQ_VectorEtonxF32(float * pSrc,float * pDst,int32_t length)147 void PQ_VectorEtonxF32(float *pSrc, float *pDst, int32_t length)
148 {
149 int32_t remainderBy8 = length % 8;
150 pq_float_t val;
151
152 if (remainderBy8 > 0)
153 {
154 length -= remainderBy8;
155 while ((remainderBy8--) > 0)
156 {
157 val.floatX = *pSrc++;
158 _pq_etonx0(val.integerX);
159 val.integerX = _pq_readMult0();
160 *pDst++ = val.floatX;
161 }
162 }
163
164 if (length > 0)
165 {
166 PQ_StartVector(pSrc, pDst, length);
167 PQ_Vector8F32(PQ_ETONX, 0, PQ_TRANS);
168 PQ_EndVector();
169 }
170 }
171
PQ_VectorSinF32(float * pSrc,float * pDst,int32_t length)172 void PQ_VectorSinF32(float *pSrc, float *pDst, int32_t length)
173 {
174 int32_t remainderBy8 = length % 8;
175 pq_float_t val;
176
177 if (remainderBy8 > 0)
178 {
179 length -= remainderBy8;
180 while ((remainderBy8--) > 0)
181 {
182 val.floatX = *pSrc++;
183 _pq_sin0(val.integerX);
184 val.integerX = _pq_readAdd0();
185 *pDst++ = val.floatX;
186 }
187 }
188
189 if (length > 0)
190 {
191 PQ_StartVector(pSrc, pDst, length);
192 PQ_Vector8F32(PQ_SIN, 1, PQ_TRIG);
193 PQ_EndVector();
194 }
195 }
196
PQ_VectorCosF32(float * pSrc,float * pDst,int32_t length)197 void PQ_VectorCosF32(float *pSrc, float *pDst, int32_t length)
198 {
199 int32_t remainderBy8 = length % 8;
200 pq_float_t val;
201
202 if (remainderBy8 > 0)
203 {
204 length -= remainderBy8;
205 while ((remainderBy8--) > 0)
206 {
207 val.floatX = *pSrc++;
208 _pq_cos0(val.integerX);
209 val.integerX = _pq_readAdd0();
210 *pDst++ = val.floatX;
211 }
212 }
213
214 if (length > 0)
215 {
216 PQ_StartVector(pSrc, pDst, length);
217 PQ_Vector8F32(PQ_COS, 1, PQ_TRIG);
218 PQ_EndVector();
219 }
220 }
221
PQ_VectorLnFixed32(int32_t * pSrc,int32_t * pDst,int32_t length)222 void PQ_VectorLnFixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
223 {
224 int32_t remainderBy8 = length % 8;
225
226 if (remainderBy8 > 0)
227 {
228 length -= remainderBy8;
229 while ((remainderBy8--) > 0)
230 {
231 _pq_ln_fx0(*pSrc++);
232 *pDst++ = (int32_t)_pq_readAdd0_fx();
233 }
234 }
235
236 if (length > 0)
237 {
238 PQ_StartVector(pSrc, pDst, length);
239 PQ_Vector8Fixed32(PQ_LN, 1, PQ_TRANS_FIXED);
240 PQ_EndVector();
241 }
242 }
243
PQ_VectorInvFixed32(int32_t * pSrc,int32_t * pDst,int32_t length)244 void PQ_VectorInvFixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
245 {
246 int32_t remainderBy8 = length % 8;
247
248 if (remainderBy8 > 0)
249 {
250 length -= remainderBy8;
251 while ((remainderBy8--) > 0)
252 {
253 _pq_inv_fx0(*pSrc++);
254 *pDst++ = (int32_t)_pq_readMult0_fx();
255 }
256 }
257
258 if (length > 0)
259 {
260 PQ_StartVector(pSrc, pDst, length);
261 PQ_Vector8Fixed32(PQ_INV, 0, PQ_TRANS_FIXED);
262 PQ_EndVector();
263 }
264 }
265
PQ_VectorSqrtFixed32(int32_t * pSrc,int32_t * pDst,int32_t length)266 void PQ_VectorSqrtFixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
267 {
268 int32_t remainderBy8 = length % 8;
269
270 if (remainderBy8 > 0)
271 {
272 length -= remainderBy8;
273 while ((remainderBy8--) > 0)
274 {
275 _pq_sqrt_fx0(*pSrc++);
276 *pDst++ = (int32_t)_pq_readMult0_fx();
277 }
278 }
279
280 if (length > 0)
281 {
282 PQ_StartVector(pSrc, pDst, length);
283 PQ_Vector8Fixed32(PQ_SQRT, 0, PQ_TRANS_FIXED);
284 PQ_EndVector();
285 }
286 }
287
PQ_VectorInvSqrtFixed32(int32_t * pSrc,int32_t * pDst,int32_t length)288 void PQ_VectorInvSqrtFixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
289 {
290 int32_t remainderBy8 = length % 8;
291
292 if (remainderBy8 > 0)
293 {
294 length -= remainderBy8;
295 while ((remainderBy8--) > 0)
296 {
297 _pq_invsqrt_fx0(*pSrc++);
298 *pDst++ = (int32_t)_pq_readMult0_fx();
299 }
300 }
301
302 if (length > 0)
303 {
304 PQ_StartVector(pSrc, pDst, length);
305 PQ_Vector8Fixed32(PQ_INVSQRT, 0, PQ_TRANS_FIXED);
306 PQ_EndVector();
307 }
308 }
309
PQ_VectorEtoxFixed32(int32_t * pSrc,int32_t * pDst,int32_t length)310 void PQ_VectorEtoxFixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
311 {
312 int32_t remainderBy8 = length % 8;
313
314 if (remainderBy8 > 0)
315 {
316 length -= remainderBy8;
317 while ((remainderBy8--) > 0)
318 {
319 _pq_etox_fx0(*pSrc++);
320 *pDst++ = (int32_t)_pq_readMult0_fx();
321 }
322 }
323
324 if (length > 0)
325 {
326 PQ_StartVector(pSrc, pDst, length);
327 PQ_Vector8Fixed32(PQ_ETOX, 0, PQ_TRANS_FIXED);
328 PQ_EndVector();
329 }
330 }
331
PQ_VectorEtonxFixed32(int32_t * pSrc,int32_t * pDst,int32_t length)332 void PQ_VectorEtonxFixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
333 {
334 int32_t remainderBy8 = length % 8;
335
336 if (remainderBy8 > 0)
337 {
338 length -= remainderBy8;
339 while ((remainderBy8--) > 0)
340 {
341 _pq_etonx_fx0(*pSrc++);
342 *pDst++ = (int32_t)_pq_readMult0_fx();
343 }
344 }
345
346 if (length > 0)
347 {
348 PQ_StartVector(pSrc, pDst, length);
349 PQ_Vector8Fixed32(PQ_ETONX, 0, PQ_TRANS_FIXED);
350 PQ_EndVector();
351 }
352 }
353
PQ_VectorSinQ31(int32_t * pSrc,int32_t * pDst,int32_t length)354 void PQ_VectorSinQ31(int32_t *pSrc, int32_t *pDst, int32_t length)
355 {
356 int32_t remainderBy8 = length % 8;
357
358 uint32_t cppre;
359 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
360 pq_float_t magic;
361 pq_float_t valFloat;
362
363 magic.integerX = 0x30c90fdbU;
364 #endif
365
366 cppre = POWERQUAD->CPPRE;
367 POWERQUAD->CPPRE = POWERQUAD_CPPRE_CPPRE_OUT(31);
368
369 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
370 if (remainderBy8 > 0)
371 {
372 length -= remainderBy8;
373 while ((remainderBy8--) > 0)
374 {
375 valFloat.floatX = magic.floatX * (float)(*pSrc++);
376 _pq_sin0(valFloat.integerX);
377 (void)_pq_readAdd0();
378 *pDst++ = (int32_t)_pq_readAdd0_fx();
379 }
380 }
381
382 while (length > 0)
383 {
384 valFloat.floatX = magic.floatX * (float)(*pSrc++);
385 _pq_sin0(valFloat.integerX);
386 (void)_pq_readAdd0();
387 *pDst++ = (int32_t)_pq_readAdd0_fx();
388
389 valFloat.floatX = magic.floatX * (float)(*pSrc++);
390 _pq_sin0(valFloat.integerX);
391 (void)_pq_readAdd0();
392 *pDst++ = (int32_t)_pq_readAdd0_fx();
393
394 valFloat.floatX = magic.floatX * (float)(*pSrc++);
395 _pq_sin0(valFloat.integerX);
396 (void)_pq_readAdd0();
397 *pDst++ = (int32_t)_pq_readAdd0_fx();
398
399 valFloat.floatX = magic.floatX * (float)(*pSrc++);
400 _pq_sin0(valFloat.integerX);
401 (void)_pq_readAdd0();
402 *pDst++ = (int32_t)_pq_readAdd0_fx();
403
404 valFloat.floatX = magic.floatX * (float)(*pSrc++);
405 _pq_sin0(valFloat.integerX);
406 (void)_pq_readAdd0();
407 *pDst++ = (int32_t)_pq_readAdd0_fx();
408
409 valFloat.floatX = magic.floatX * (float)(*pSrc++);
410 _pq_sin0(valFloat.integerX);
411 (void)_pq_readAdd0();
412 *pDst++ = (int32_t)_pq_readAdd0_fx();
413
414 valFloat.floatX = magic.floatX * (float)(*pSrc++);
415 _pq_sin0(valFloat.integerX);
416 (void)_pq_readAdd0();
417 *pDst++ = (int32_t)_pq_readAdd0_fx();
418
419 valFloat.floatX = magic.floatX * (float)(*pSrc++);
420 _pq_sin0(valFloat.integerX);
421 (void)_pq_readAdd0();
422 *pDst++ = (int32_t)_pq_readAdd0_fx();
423
424 length -= 8;
425 }
426
427 #else
428 if (remainderBy8 > 0)
429 {
430 length -= remainderBy8;
431 while ((remainderBy8--) > 0)
432 {
433 _pq_sin_fx0(*pSrc++);
434 *pDst++ = (int32_t)_pq_readAdd0_fx();
435 }
436 }
437
438 if (length > 0)
439 {
440 PQ_StartVector(pSrc, pDst, length);
441 PQ_Vector8Fixed32(PQ_SIN, 1, PQ_TRIG_FIXED);
442 PQ_EndVector();
443 }
444 #endif
445
446 POWERQUAD->CPPRE = cppre;
447 }
448
PQ_VectorCosQ31(int32_t * pSrc,int32_t * pDst,int32_t length)449 void PQ_VectorCosQ31(int32_t *pSrc, int32_t *pDst, int32_t length)
450 {
451 int32_t remainderBy8 = length % 8;
452
453 uint32_t cppre;
454 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
455 pq_float_t magic;
456 pq_float_t valFloat;
457
458 magic.integerX = 0x30c90fdb;
459 #endif
460
461 cppre = POWERQUAD->CPPRE;
462 POWERQUAD->CPPRE = POWERQUAD_CPPRE_CPPRE_OUT(31);
463
464 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
465 if (remainderBy8 > 0)
466 {
467 length -= remainderBy8;
468 while ((remainderBy8--) > 0)
469 {
470 valFloat.floatX = magic.floatX * (float)(*pSrc++);
471 _pq_cos0(valFloat.integerX);
472 (void)_pq_readAdd0();
473 *pDst++ = (int32_t)_pq_readAdd0_fx();
474 }
475 }
476
477 while (length > 0)
478 {
479 valFloat.floatX = magic.floatX * (float)(*pSrc++);
480 _pq_cos0(valFloat.integerX);
481 (void)_pq_readAdd0();
482 *pDst++ = (int32_t)_pq_readAdd0_fx();
483
484 valFloat.floatX = magic.floatX * (float)(*pSrc++);
485 _pq_cos0(valFloat.integerX);
486 (void)_pq_readAdd0();
487 *pDst++ = (int32_t)_pq_readAdd0_fx();
488
489 valFloat.floatX = magic.floatX * (float)(*pSrc++);
490 _pq_cos0(valFloat.integerX);
491 (void)_pq_readAdd0();
492 *pDst++ = (int32_t)_pq_readAdd0_fx();
493
494 valFloat.floatX = magic.floatX * (float)(*pSrc++);
495 _pq_cos0(valFloat.integerX);
496 (void)_pq_readAdd0();
497 *pDst++ = (int32_t)_pq_readAdd0_fx();
498
499 valFloat.floatX = magic.floatX * (float)(*pSrc++);
500 _pq_cos0(valFloat.integerX);
501 (void)_pq_readAdd0();
502 *pDst++ = (int32_t)_pq_readAdd0_fx();
503
504 valFloat.floatX = magic.floatX * (float)(*pSrc++);
505 _pq_cos0(valFloat.integerX);
506 (void)_pq_readAdd0();
507 *pDst++ = (int32_t)_pq_readAdd0_fx();
508
509 valFloat.floatX = magic.floatX * (float)(*pSrc++);
510 _pq_cos0(valFloat.integerX);
511 (void)_pq_readAdd0();
512 *pDst++ = (int32_t)_pq_readAdd0_fx();
513
514 valFloat.floatX = magic.floatX * (float)(*pSrc++);
515 _pq_cos0(valFloat.integerX);
516 (void)_pq_readAdd0();
517 *pDst++ = (int32_t)_pq_readAdd0_fx();
518
519 length -= 8;
520 }
521
522 #else
523 if (remainderBy8 > 0)
524 {
525 length -= remainderBy8;
526 while ((remainderBy8--) > 0)
527 {
528 _pq_cos_fx0(*pSrc++);
529 *pDst++ = (int32_t)_pq_readAdd0_fx();
530 }
531 }
532
533 if (length > 0)
534 {
535 PQ_StartVector(pSrc, pDst, length);
536 PQ_Vector8Fixed32(PQ_COS, 1, PQ_TRIG_FIXED);
537 PQ_EndVector();
538 }
539 #endif
540
541 POWERQUAD->CPPRE = cppre;
542 }
543
PQ_VectorLnFixed16(int16_t * pSrc,int16_t * pDst,int32_t length)544 void PQ_VectorLnFixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
545 {
546 int32_t remainderBy8 = length % 8;
547
548 if (remainderBy8 > 0)
549 {
550 length -= remainderBy8;
551 while ((remainderBy8--) > 0)
552 {
553 _pq_ln_fx0(*pSrc++);
554 *pDst++ = (int16_t)_pq_readAdd0_fx();
555 }
556 }
557
558 if (length > 0)
559 {
560 PQ_StartVectorFixed16(pSrc, pDst, length);
561 PQ_Vector8Fixed16(PQ_LN, 1, PQ_TRANS_FIXED);
562 PQ_EndVector();
563 }
564 }
565
PQ_VectorInvFixed16(int16_t * pSrc,int16_t * pDst,int32_t length)566 void PQ_VectorInvFixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
567 {
568 int32_t remainderBy8 = length % 8;
569
570 if (remainderBy8 > 0)
571 {
572 length -= remainderBy8;
573 while ((remainderBy8--) > 0)
574 {
575 _pq_inv_fx0(*pSrc++);
576 *pDst++ = (int16_t)_pq_readMult0_fx();
577 }
578 }
579
580 if (length > 0)
581 {
582 PQ_StartVectorFixed16(pSrc, pDst, length);
583 PQ_Vector8Fixed16(PQ_INV, 0, PQ_TRANS_FIXED);
584 PQ_EndVector();
585 }
586 }
587
PQ_VectorSqrtFixed16(int16_t * pSrc,int16_t * pDst,int32_t length)588 void PQ_VectorSqrtFixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
589 {
590 int32_t remainderBy8 = length % 8;
591
592 if (remainderBy8 > 0)
593 {
594 length -= remainderBy8;
595 while ((remainderBy8--) > 0)
596 {
597 _pq_sqrt_fx0(*pSrc++);
598 *pDst++ = (int16_t)_pq_readMult0_fx();
599 }
600 }
601
602 if (length > 0)
603 {
604 PQ_StartVectorFixed16(pSrc, pDst, length);
605 PQ_Vector8Fixed16(PQ_SQRT, 0, PQ_TRANS_FIXED);
606 PQ_EndVector();
607 }
608 }
609
PQ_VectorInvSqrtFixed16(int16_t * pSrc,int16_t * pDst,int32_t length)610 void PQ_VectorInvSqrtFixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
611 {
612 int32_t remainderBy8 = length % 8;
613
614 if (remainderBy8 > 0)
615 {
616 length -= remainderBy8;
617 while ((remainderBy8--) > 0)
618 {
619 _pq_invsqrt_fx0(*pSrc++);
620 *pDst++ = (int16_t)_pq_readMult0_fx();
621 }
622 }
623
624 if (length > 0)
625 {
626 PQ_StartVectorFixed16(pSrc, pDst, length);
627 PQ_Vector8Fixed16(PQ_INVSQRT, 0, PQ_TRANS_FIXED);
628 PQ_EndVector();
629 }
630 }
631
PQ_VectorEtoxFixed16(int16_t * pSrc,int16_t * pDst,int32_t length)632 void PQ_VectorEtoxFixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
633 {
634 int32_t remainderBy8 = length % 8;
635
636 if (remainderBy8 > 0)
637 {
638 length -= remainderBy8;
639 while ((remainderBy8--) > 0)
640 {
641 _pq_etox_fx0(*pSrc++);
642 *pDst++ = (int16_t)_pq_readMult0_fx();
643 }
644 }
645
646 if (length > 0)
647 {
648 PQ_StartVectorFixed16(pSrc, pDst, length);
649 PQ_Vector8Fixed16(PQ_ETOX, 0, PQ_TRANS_FIXED);
650 PQ_EndVector();
651 }
652 }
653
PQ_VectorEtonxFixed16(int16_t * pSrc,int16_t * pDst,int32_t length)654 void PQ_VectorEtonxFixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
655 {
656 int32_t remainderBy8 = length % 8;
657
658 if (remainderBy8 > 0)
659 {
660 length -= remainderBy8;
661 while ((remainderBy8--) > 0)
662 {
663 _pq_etonx_fx0(*pSrc++);
664 *pDst++ = (int16_t)_pq_readMult0_fx();
665 }
666 }
667
668 if (length > 0)
669 {
670 PQ_StartVectorFixed16(pSrc, pDst, length);
671 PQ_Vector8Fixed16(PQ_ETONX, 0, PQ_TRANS_FIXED);
672 PQ_EndVector();
673 }
674 }
675
PQ_VectorSinQ15(int16_t * pSrc,int16_t * pDst,int32_t length)676 void PQ_VectorSinQ15(int16_t *pSrc, int16_t *pDst, int32_t length)
677 {
678 uint32_t cppre;
679 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
680 pq_float_t magic;
681 pq_float_t valFloat;
682
683 magic.integerX = 0x30c90fdb;
684 #endif
685
686 cppre = POWERQUAD->CPPRE;
687 POWERQUAD->CPPRE = POWERQUAD_CPPRE_CPPRE_OUT(31);
688
689 int32_t remainderBy8 = length % 8;
690
691 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
692 if (remainderBy8 > 0)
693 {
694 length -= remainderBy8;
695 while ((remainderBy8--) > 0)
696 {
697 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
698 _pq_sin0(valFloat.integerX);
699 (void)_pq_readAdd0();
700 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
701 }
702 }
703
704 while (length > 0)
705 {
706 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
707 _pq_sin0(valFloat.integerX);
708 (void)_pq_readAdd0();
709 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
710
711 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
712 _pq_sin0(valFloat.integerX);
713 (void)_pq_readAdd0();
714 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
715
716 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
717 _pq_sin0(valFloat.integerX);
718 (void)_pq_readAdd0();
719 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
720
721 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
722 _pq_sin0(valFloat.integerX);
723 (void)_pq_readAdd0();
724 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
725
726 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
727 _pq_sin0(valFloat.integerX);
728 (void)_pq_readAdd0();
729 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
730
731 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
732 _pq_sin0(valFloat.integerX);
733 (void)_pq_readAdd0();
734 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
735
736 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
737 _pq_sin0(valFloat.integerX);
738 (void)_pq_readAdd0();
739 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
740
741 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
742 _pq_sin0(valFloat.integerX);
743 (void)_pq_readAdd0();
744 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
745
746 length -= 8;
747 }
748
749 #else
750
751 if (remainderBy8 > 0)
752 {
753 length -= remainderBy8;
754 while ((remainderBy8--) > 0)
755 {
756 _pq_sin_fx0((uint32_t)(*pSrc++) << 16U);
757 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
758 }
759 }
760
761 if (length > 0)
762 {
763 PQ_StartVectorQ15(pSrc, pDst, length);
764 PQ_Vector8Q15(PQ_SIN, 1, PQ_TRIG_FIXED);
765 PQ_EndVector();
766 }
767 #endif
768
769 POWERQUAD->CPPRE = cppre;
770 }
771
PQ_VectorCosQ15(int16_t * pSrc,int16_t * pDst,int32_t length)772 void PQ_VectorCosQ15(int16_t *pSrc, int16_t *pDst, int32_t length)
773 {
774 uint32_t cppre;
775 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
776 pq_float_t magic;
777 pq_float_t valFloat;
778
779 magic.integerX = 0x30c90fdbU;
780 #endif
781
782 cppre = POWERQUAD->CPPRE;
783 POWERQUAD->CPPRE = POWERQUAD_CPPRE_CPPRE_OUT(31);
784
785 int32_t remainderBy8 = length % 8;
786
787 #if defined(FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA) && FSL_FEATURE_POWERQUAD_SIN_COS_FIX_ERRATA
788 if (remainderBy8 > 0)
789 {
790 length -= remainderBy8;
791 while ((remainderBy8--) > 0)
792 {
793 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
794 _pq_cos0(valFloat.integerX);
795 (void)_pq_readAdd0();
796 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
797 }
798 }
799
800 while (length > 0)
801 {
802 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
803 _pq_cos0(valFloat.integerX);
804 (void)_pq_readAdd0();
805 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
806
807 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
808 _pq_cos0(valFloat.integerX);
809 (void)_pq_readAdd0();
810 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
811
812 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
813 _pq_cos0(valFloat.integerX);
814 (void)_pq_readAdd0();
815 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
816
817 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
818 _pq_cos0(valFloat.integerX);
819 (void)_pq_readAdd0();
820 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
821
822 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
823 _pq_cos0(valFloat.integerX);
824 (void)_pq_readAdd0();
825 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
826
827 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
828 _pq_cos0(valFloat.integerX);
829 (void)_pq_readAdd0();
830 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
831
832 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
833 _pq_cos0(valFloat.integerX);
834 (void)_pq_readAdd0();
835 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
836
837 valFloat.floatX = magic.floatX * (float)(uint32_t)((uint32_t)(*pSrc++) << 16U);
838 _pq_cos0(valFloat.integerX);
839 (void)_pq_readAdd0();
840 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
841
842 length -= 8;
843 }
844
845 #else
846
847 if (remainderBy8 > 0)
848 {
849 length -= remainderBy8;
850 while ((remainderBy8--) > 0)
851 {
852 _pq_cos_fx0((uint32_t)(*pSrc++) << 16U);
853 *pDst++ = (int16_t)(uint32_t)((_pq_readAdd0_fx()) >> 16U);
854 }
855 }
856
857 if (length > 0)
858 {
859 PQ_StartVectorQ15(pSrc, pDst, length);
860 PQ_Vector8Q15(PQ_COS, 1, PQ_TRIG_FIXED);
861 PQ_EndVector();
862 }
863 #endif
864
865 POWERQUAD->CPPRE = cppre;
866 }
867
PQ_ArctanFixed(POWERQUAD_Type * base,int32_t x,int32_t y,pq_cordic_iter_t iteration)868 int32_t PQ_ArctanFixed(POWERQUAD_Type *base, int32_t x, int32_t y, pq_cordic_iter_t iteration)
869 {
870 base->CORDIC_X = (uint32_t)x;
871 base->CORDIC_Y = (uint32_t)y;
872 base->CORDIC_Z = 0U;
873 base->CONTROL = (CP_CORDIC << 4U) | CORDIC_ARCTAN | CORDIC_ITER(iteration);
874
875 PQ_WaitDone(base);
876 return (int32_t)base->CORDIC_Z;
877 }
878
PQ_ArctanhFixed(POWERQUAD_Type * base,int32_t x,int32_t y,pq_cordic_iter_t iteration)879 int32_t PQ_ArctanhFixed(POWERQUAD_Type *base, int32_t x, int32_t y, pq_cordic_iter_t iteration)
880 {
881 base->CORDIC_X = (uint32_t)x;
882 base->CORDIC_Y = (uint32_t)y;
883 base->CORDIC_Z = 0U;
884 base->CONTROL = (CP_CORDIC << 4U) | CORDIC_ARCTANH | CORDIC_ITER(iteration);
885
886 PQ_WaitDone(base);
887 return (int32_t)base->CORDIC_Z;
888 }
889
