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_matrix"
17 #endif
18
19 /*******************************************************************************
20 * Code
21 ******************************************************************************/
PQ_MatrixAddition(POWERQUAD_Type * base,uint32_t length,void * pAData,void * pBData,void * pResult)22 void PQ_MatrixAddition(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
23 {
24 assert(NULL != pAData);
25 assert(NULL != pBData);
26 assert(NULL != pResult);
27
28 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
29 base->INABASE = (uint32_t)(uint32_t *)pAData;
30 base->INBBASE = (uint32_t)(uint32_t *)pBData;
31 base->LENGTH = length;
32 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_ADD;
33 }
34
PQ_MatrixSubtraction(POWERQUAD_Type * base,uint32_t length,void * pAData,void * pBData,void * pResult)35 void PQ_MatrixSubtraction(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
36 {
37 assert(NULL != pAData);
38 assert(NULL != pBData);
39 assert(NULL != pResult);
40
41 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
42 base->INABASE = (uint32_t)(uint32_t *)pAData;
43 base->INBBASE = (uint32_t)(uint32_t *)pBData;
44 base->LENGTH = length;
45 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_SUB;
46 }
47
PQ_MatrixMultiplication(POWERQUAD_Type * base,uint32_t length,void * pAData,void * pBData,void * pResult)48 void PQ_MatrixMultiplication(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
49 {
50 assert(NULL != pAData);
51 assert(NULL != pBData);
52 assert(NULL != pResult);
53
54 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
55 base->INABASE = (uint32_t)(uint32_t *)pAData;
56 base->INBBASE = (uint32_t)(uint32_t *)pBData;
57 base->LENGTH = length;
58 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_MULT;
59 }
60
PQ_MatrixProduct(POWERQUAD_Type * base,uint32_t length,void * pAData,void * pBData,void * pResult)61 void PQ_MatrixProduct(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
62 {
63 assert(NULL != pAData);
64 assert(NULL != pBData);
65 assert(NULL != pResult);
66
67 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
68 base->INABASE = (uint32_t)(uint32_t *)pAData;
69 base->INBBASE = (uint32_t)(uint32_t *)pBData;
70 base->LENGTH = length;
71 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_PROD;
72 }
73
PQ_VectorDotProduct(POWERQUAD_Type * base,uint32_t length,void * pAData,void * pBData,void * pResult)74 void PQ_VectorDotProduct(POWERQUAD_Type *base, uint32_t length, void *pAData, void *pBData, void *pResult)
75 {
76 assert(NULL != pAData);
77 assert(NULL != pBData);
78 assert(NULL != pResult);
79
80 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
81 base->INABASE = (uint32_t)(uint32_t *)pAData;
82 base->INBBASE = (uint32_t)(uint32_t *)pBData;
83 base->LENGTH = length;
84 base->CONTROL = (CP_MTX << 4U) | PQ_VEC_DOTP;
85 }
86
PQ_MatrixInversion(POWERQUAD_Type * base,uint32_t length,void * pData,void * pTmpData,void * pResult)87 void PQ_MatrixInversion(POWERQUAD_Type *base, uint32_t length, void *pData, void *pTmpData, void *pResult)
88 {
89 assert(NULL != pData);
90 assert(NULL != pTmpData);
91 assert(NULL != pResult);
92
93 /* Workaround:
94 *
95 * Matrix inv depends on the coproc 1/x function, this puts coproc to right state.
96 */
97 _pq_inv0(1.0f);
98
99 base->INABASE = (uint32_t)(uint32_t *)pData;
100 base->TMPBASE = (uint32_t)(uint32_t *)pTmpData;
101 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
102 base->LENGTH = length;
103 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_INV;
104 }
105
PQ_MatrixTranspose(POWERQUAD_Type * base,uint32_t length,void * pData,void * pResult)106 void PQ_MatrixTranspose(POWERQUAD_Type *base, uint32_t length, void *pData, void *pResult)
107 {
108 assert(NULL != pData);
109 assert(NULL != pResult);
110
111 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
112 base->INABASE = (uint32_t)(uint32_t *)pData;
113 base->LENGTH = length;
114 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_TRAN;
115 }
116
PQ_MatrixScale(POWERQUAD_Type * base,uint32_t length,float misc,const void * pData,void * pResult)117 void PQ_MatrixScale(POWERQUAD_Type *base, uint32_t length, float misc, const void *pData, void *pResult)
118 {
119 assert(NULL != pData);
120 assert(NULL != pResult);
121 pq_float_t val;
122
123 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
124 base->INABASE = (uint32_t)(const uint32_t *)pData;
125 base->LENGTH = length;
126
127 val.floatX = misc;
128 base->MISC = val.integerX;
129
130 base->CONTROL = (CP_MTX << 4U) | PQ_MTX_SCALE;
131 }
132
