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_filter"
17 #endif
18
19 /*******************************************************************************
20 * Code
21 ******************************************************************************/
PQ_VectorBiquadDf2F32(float * pSrc,float * pDst,int32_t length)22 void PQ_VectorBiquadDf2F32(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_biquad0(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_Vector8BiquadDf2F32();
43 PQ_EndVector();
44 }
45 }
46
PQ_VectorBiquadDf2Fixed32(int32_t * pSrc,int32_t * pDst,int32_t length)47 void PQ_VectorBiquadDf2Fixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
48 {
49 int32_t remainderBy8 = length % 8;
50
51 if (remainderBy8 > 0)
52 {
53 length -= remainderBy8;
54 while ((remainderBy8--) > 0)
55 {
56 _pq_biquad0_fx(*pSrc++);
57 *pDst++ = (int32_t)_pq_readAdd0_fx();
58 }
59 }
60
61 if (length > 0)
62 {
63 PQ_StartVector(pSrc, pDst, length);
64 PQ_Vector8BiquadDf2Fixed32();
65 PQ_EndVector();
66 }
67 }
68
PQ_VectorBiquadDf2Fixed16(int16_t * pSrc,int16_t * pDst,int32_t length)69 void PQ_VectorBiquadDf2Fixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
70 {
71 int32_t remainderBy8 = length % 8;
72
73 if (remainderBy8 > 0)
74 {
75 length -= remainderBy8;
76 while ((remainderBy8--) > 0)
77 {
78 _pq_biquad0_fx(*pSrc++);
79 *pDst++ = (int16_t)_pq_readAdd0_fx();
80 }
81 }
82
83 if (length > 0)
84 {
85 PQ_StartVectorFixed16(pSrc, pDst, length);
86 PQ_Vector8BiquadDf2Fixed16();
87 PQ_EndVector();
88 }
89 }
90
PQ_VectorBiquadCascadeDf2F32(float * pSrc,float * pDst,int32_t length)91 void PQ_VectorBiquadCascadeDf2F32(float *pSrc, float *pDst, int32_t length)
92 {
93 int32_t remainderBy8 = length % 8;
94 pq_float_t val;
95
96 if (remainderBy8 > 0)
97 {
98 length -= remainderBy8;
99
100 PQ_Biquad1F32(&pSrc[0], &pDst[0]);
101
102 for (int i = 1; i < remainderBy8; i++)
103 {
104 val.floatX = pSrc[i - 1];
105 _pq_biquad0(val.integerX);
106
107 val.floatX = pSrc[i];
108 _pq_biquad1(val.integerX);
109
110 val.integerX = _pq_readAdd0();
111 pDst[i - 1] = val.floatX;
112
113 val.integerX = _pq_readAdd1();
114 pDst[i] = val.floatX;
115 }
116
117 PQ_BiquadF32(&pSrc[remainderBy8 - 1], &pDst[remainderBy8 - 1]);
118 }
119
120 if (length > 0)
121 {
122 PQ_StartVector(&pSrc[remainderBy8], &pDst[remainderBy8], length);
123 PQ_Vector8BiquadDf2CascadeF32();
124 PQ_EndVector();
125 }
126 }
127
PQ_VectorBiquadCascadeDf2Fixed32(int32_t * pSrc,int32_t * pDst,int32_t length)128 void PQ_VectorBiquadCascadeDf2Fixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
129 {
130 int32_t remainderBy8 = length % 8;
131
132 if (remainderBy8 > 0)
133 {
134 length -= remainderBy8;
135
136 _pq_biquad1_fx(pSrc[0]);
137 pDst[0] = (int32_t)_pq_readAdd1_fx();
138
139 for (int i = 1; i < remainderBy8; i++)
140 {
141 _pq_biquad0_fx(pSrc[i - 1]);
142 _pq_biquad1_fx(pSrc[i]);
143 pDst[i - 1] = (int32_t)_pq_readAdd0_fx();
144 pDst[i] = (int32_t)_pq_readAdd1_fx();
145 }
146
147 _pq_biquad0_fx(pSrc[remainderBy8 - 1]);
148 pDst[remainderBy8 - 1] = (int32_t)_pq_readAdd0_fx();
149 }
150
151 if (length > 0)
152 {
153 PQ_StartVector(&pSrc[remainderBy8], &pDst[remainderBy8], length);
154 PQ_Vector8BiquadDf2CascadeFixed32();
155 PQ_EndVector();
156 }
157 }
158
PQ_VectorBiquadCascadeDf2Fixed16(int16_t * pSrc,int16_t * pDst,int32_t length)159 void PQ_VectorBiquadCascadeDf2Fixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
160 {
161 int32_t remainderBy8 = length % 8;
162
163 if (remainderBy8 > 0)
164 {
165 length -= remainderBy8;
166
167 _pq_biquad1_fx(pSrc[0]);
168 pDst[0] = (int16_t)_pq_readAdd1_fx();
169
170 for (int i = 1; i < remainderBy8; i++)
171 {
172 _pq_biquad0_fx(pSrc[i - 1]);
173 _pq_biquad1_fx(pSrc[i]);
174 pDst[i - 1] = (int16_t)_pq_readAdd0_fx();
175 pDst[i] = (int16_t)_pq_readAdd1_fx();
176 }
177
178 _pq_biquad0_fx(pSrc[remainderBy8 - 1]);
179 pDst[remainderBy8 - 1] = (int16_t)_pq_readAdd0_fx();
180 }
181
182 if (length > 0)
183 {
184 PQ_StartVectorFixed16(&pSrc[remainderBy8], &pDst[remainderBy8], length);
185 PQ_Vector8BiquadDf2CascadeFixed16();
186 PQ_EndVector();
187 }
188 }
189
PQ_BiquadBackUpInternalState(POWERQUAD_Type * base,int32_t biquad_num,pq_biquad_state_t * state)190 void PQ_BiquadBackUpInternalState(POWERQUAD_Type *base, int32_t biquad_num, pq_biquad_state_t *state)
191 {
192 pq_float_t v_n_1;
193 pq_float_t v_n;
194
195 #if defined(__GNUC__)
196 #pragma GCC diagnostic push
197 #pragma GCC diagnostic ignored "-Wstrict-aliasing"
198 #endif
199 if (0 == biquad_num)
200 {
201 v_n_1.integerX = base->GPREG[0];
202 v_n.integerX = base->GPREG[1];
203
204 state->param.v_n_1 = v_n_1.floatX;
205 state->param.v_n = v_n.floatX;
206 state->compreg = base->COMPREG[1];
207 }
208 else
209 {
210 v_n_1.integerX = base->GPREG[8];
211 v_n.integerX = base->GPREG[9];
212
213 state->param.v_n_1 = v_n_1.floatX;
214 state->param.v_n = v_n.floatX;
215 state->compreg = base->COMPREG[3];
216 }
217 #if defined(__GNUC__)
218 #pragma GCC diagnostic pop
219 #endif
220 }
221
PQ_BiquadRestoreInternalState(POWERQUAD_Type * base,int32_t biquad_num,pq_biquad_state_t * state)222 void PQ_BiquadRestoreInternalState(POWERQUAD_Type *base, int32_t biquad_num, pq_biquad_state_t *state)
223 {
224 pq_float_t v_n_1;
225 pq_float_t v_n;
226 pq_float_t a_1;
227 pq_float_t a_2;
228 pq_float_t b_0;
229 pq_float_t b_1;
230 pq_float_t b_2;
231
232 if (0 == biquad_num)
233 {
234 v_n_1.floatX = state->param.v_n_1;
235 v_n.floatX = state->param.v_n;
236 a_1.floatX = state->param.a_1;
237 a_2.floatX = state->param.a_2;
238 b_0.floatX = state->param.b_0;
239 b_1.floatX = state->param.b_1;
240 b_2.floatX = state->param.b_2;
241
242 base->GPREG[0] = v_n_1.integerX;
243 base->GPREG[1] = v_n.integerX;
244 base->GPREG[2] = a_1.integerX;
245 base->GPREG[3] = a_2.integerX;
246 base->GPREG[4] = b_0.integerX;
247 base->GPREG[5] = b_1.integerX;
248 base->GPREG[6] = b_2.integerX;
249 base->COMPREG[1] = state->compreg;
250 }
251 else
252 {
253 v_n_1.floatX = state->param.v_n_1;
254 v_n.floatX = state->param.v_n;
255 a_1.floatX = state->param.a_1;
256 a_2.floatX = state->param.a_2;
257 b_0.floatX = state->param.b_0;
258 b_1.floatX = state->param.b_1;
259 b_2.floatX = state->param.b_2;
260
261 base->GPREG[8] = v_n_1.integerX;
262 base->GPREG[9] = v_n.integerX;
263 base->GPREG[10] = a_1.integerX;
264 base->GPREG[11] = a_2.integerX;
265 base->GPREG[12] = b_0.integerX;
266 base->GPREG[13] = b_1.integerX;
267 base->GPREG[14] = b_2.integerX;
268 base->COMPREG[3] = state->compreg;
269 }
270 }
271
PQ_FIR(POWERQUAD_Type * base,const void * pAData,int32_t ALength,const void * pBData,int32_t BLength,void * pResult,uint32_t opType)272 void PQ_FIR(POWERQUAD_Type *base,
273 const void *pAData,
274 int32_t ALength,
275 const void *pBData,
276 int32_t BLength,
277 void *pResult,
278 uint32_t opType)
279 {
280 assert(NULL != pAData);
281 assert(NULL != pBData);
282 assert(NULL != pResult);
283
284 base->INABASE = (uint32_t)(const uint32_t *)pAData;
285 base->INBBASE = (uint32_t)(const uint32_t *)pBData;
286 base->LENGTH = ((uint32_t)BLength << 16U) + (uint32_t)ALength;
287 base->OUTBASE = (uint32_t)(uint32_t *)pResult;
288 base->CONTROL = (CP_FIR << 4U) | opType;
289 }
290
PQ_FIRIncrement(POWERQUAD_Type * base,int32_t ALength,int32_t BLength,int32_t xOffset)291 void PQ_FIRIncrement(POWERQUAD_Type *base, int32_t ALength, int32_t BLength, int32_t xOffset)
292 {
293 base->MISC = (uint32_t)xOffset;
294 base->LENGTH = ((uint32_t)BLength << 16U) + (uint32_t)ALength;
295 base->CONTROL = (CP_FIR << 4U) | PQ_FIR_INCREMENTAL;
296 }
297
PQ_BiquadCascadeDf2Init(pq_biquad_cascade_df2_instance * S,uint8_t numStages,pq_biquad_state_t * pState)298 void PQ_BiquadCascadeDf2Init(pq_biquad_cascade_df2_instance *S, uint8_t numStages, pq_biquad_state_t *pState)
299 {
300 S->numStages = numStages;
301 S->pState = pState;
302 }
303
PQ_BiquadCascadeDf2F32(const pq_biquad_cascade_df2_instance * S,float * pSrc,float * pDst,uint32_t blockSize)304 void PQ_BiquadCascadeDf2F32(const pq_biquad_cascade_df2_instance *S, float *pSrc, float *pDst, uint32_t blockSize)
305 {
306 uint32_t stage = S->numStages;
307 pq_biquad_state_t *states = S->pState;
308
309 if (pDst != pSrc)
310 {
311 (void)memcpy(pDst, pSrc, 4U * blockSize);
312 }
313
314 if ((stage & 0x01U) == 0x01U)
315 {
316 PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
317
318 PQ_VectorBiquadDf2F32(pSrc, pDst, (int32_t)blockSize);
319
320 PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
321
322 states++;
323 stage--;
324 }
325
326 while (stage > 0U)
327 {
328 PQ_BiquadRestoreInternalState(POWERQUAD, 1, states);
329 states++;
330 PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
331
332 PQ_VectorBiquadCascadeDf2F32(pDst, pDst, (int32_t)blockSize);
333
334 states--;
335 PQ_BiquadBackUpInternalState(POWERQUAD, 1, states);
336 states++;
337 PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
338
339 states++;
340 stage -= 2U;
341 }
342 }
343
PQ_BiquadCascadeDf2Fixed32(const pq_biquad_cascade_df2_instance * S,int32_t * pSrc,int32_t * pDst,uint32_t blockSize)344 void PQ_BiquadCascadeDf2Fixed32(const pq_biquad_cascade_df2_instance *S,
345 int32_t *pSrc,
346 int32_t *pDst,
347 uint32_t blockSize)
348 {
349 uint32_t stage = S->numStages;
350 pq_biquad_state_t *states = S->pState;
351
352 if (pDst != pSrc)
353 {
354 (void)memcpy(pDst, pSrc, 4U * blockSize);
355 }
356
357 if ((stage & 0x01U) == 0x01U)
358 {
359 PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
360
361 PQ_VectorBiquadDf2Fixed32(pSrc, pDst, (int32_t)blockSize);
362
363 PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
364
365 states++;
366 stage--;
367 }
368
369 while (stage > 0U)
370 {
371 PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
372 states++;
373 PQ_BiquadRestoreInternalState(POWERQUAD, 1, states);
374
375 PQ_VectorBiquadCascadeDf2Fixed32(pDst, pDst, (int32_t)blockSize);
376
377 states--;
378 PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
379 states++;
380 PQ_BiquadBackUpInternalState(POWERQUAD, 1, states);
381
382 states++;
383 stage -= 2U;
384 }
385 }
386
PQ_BiquadCascadeDf2Fixed16(const pq_biquad_cascade_df2_instance * S,int16_t * pSrc,int16_t * pDst,uint32_t blockSize)387 void PQ_BiquadCascadeDf2Fixed16(const pq_biquad_cascade_df2_instance *S,
388 int16_t *pSrc,
389 int16_t *pDst,
390 uint32_t blockSize)
391 {
392 uint32_t stage = S->numStages;
393 pq_biquad_state_t *states = S->pState;
394
395 if (pDst != pSrc)
396 {
397 (void)memcpy(pDst, pSrc, 2U * blockSize);
398 }
399
400 if ((stage & 0x01U) == 0x01U)
401 {
402 PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
403
404 PQ_VectorBiquadDf2Fixed16(pSrc, pDst, (int32_t)blockSize);
405
406 PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
407
408 states++;
409 stage--;
410 }
411
412 while (stage > 0U)
413 {
414 PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
415 states++;
416 PQ_BiquadRestoreInternalState(POWERQUAD, 1, states);
417
418 PQ_VectorBiquadCascadeDf2Fixed16(pDst, pDst, (int32_t)blockSize);
419
420 states--;
421 PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
422 states++;
423 PQ_BiquadBackUpInternalState(POWERQUAD, 1, states);
424
425 states++;
426 stage -= 2U;
427 }
428 }
429
