Embedded_SDK
/
MCUXpresso_LPC55S69
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
MCUXpresso_LPC55S69/devices/LPC55S69/drivers/fsl_powerquad_filter.c

429 lines
11 KiB
C
Raw Permalink Blame History

/*
* Copyright 2018-2022 NXP
* All rights reserved.
*
* SPDX-License-Identifier: BSD-3-Clause
*/
#include "fsl_powerquad.h"
/*******************************************************************************
* Definitions
******************************************************************************/
/* Component ID definition, used by tools. */
#ifndef FSL_COMPONENT_ID
#define FSL_COMPONENT_ID "platform.drivers.powerquad_filter"
#endif
/*******************************************************************************
* Code
******************************************************************************/
void PQ_VectorBiquadDf2F32(float *pSrc, float *pDst, int32_t length)
{
int32_t remainderBy8 = length % 8;
pq_float_t val;
if (remainderBy8 > 0)
{
length -= remainderBy8;
while ((remainderBy8--) > 0)
{
val.floatX = *pSrc++;
_pq_biquad0(val.integerX);
val.integerX = _pq_readAdd0();
*pDst++ = val.floatX;
}
}
if (length > 0)
{
PQ_StartVector(pSrc, pDst, length);
PQ_Vector8BiquadDf2F32();
PQ_EndVector();
}
}
void PQ_VectorBiquadDf2Fixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
{
int32_t remainderBy8 = length % 8;
if (remainderBy8 > 0)
{
length -= remainderBy8;
while ((remainderBy8--) > 0)
{
_pq_biquad0_fx(*pSrc++);
*pDst++ = (int32_t)_pq_readAdd0_fx();
}
}
if (length > 0)
{
PQ_StartVector(pSrc, pDst, length);
PQ_Vector8BiquadDf2Fixed32();
PQ_EndVector();
}
}
void PQ_VectorBiquadDf2Fixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
{
int32_t remainderBy8 = length % 8;
if (remainderBy8 > 0)
{
length -= remainderBy8;
while ((remainderBy8--) > 0)
{
_pq_biquad0_fx(*pSrc++);
*pDst++ = (int16_t)_pq_readAdd0_fx();
}
}
if (length > 0)
{
PQ_StartVectorFixed16(pSrc, pDst, length);
PQ_Vector8BiquadDf2Fixed16();
PQ_EndVector();
}
}
void PQ_VectorBiquadCascadeDf2F32(float *pSrc, float *pDst, int32_t length)
{
int32_t remainderBy8 = length % 8;
pq_float_t val;
if (remainderBy8 > 0)
{
length -= remainderBy8;
PQ_Biquad1F32(&pSrc[0], &pDst[0]);
for (int i = 1; i < remainderBy8; i++)
{
val.floatX = pSrc[i - 1];
_pq_biquad0(val.integerX);
val.floatX = pSrc[i];
_pq_biquad1(val.integerX);
val.integerX = _pq_readAdd0();
pDst[i - 1] = val.floatX;
val.integerX = _pq_readAdd1();
pDst[i] = val.floatX;
}
PQ_BiquadF32(&pSrc[remainderBy8 - 1], &pDst[remainderBy8 - 1]);
}
if (length > 0)
{
PQ_StartVector(&pSrc[remainderBy8], &pDst[remainderBy8], length);
PQ_Vector8BiquadDf2CascadeF32();
PQ_EndVector();
}
}
void PQ_VectorBiquadCascadeDf2Fixed32(int32_t *pSrc, int32_t *pDst, int32_t length)
{
int32_t remainderBy8 = length % 8;
if (remainderBy8 > 0)
{
length -= remainderBy8;
_pq_biquad1_fx(pSrc[0]);
pDst[0] = (int32_t)_pq_readAdd1_fx();
for (int i = 1; i < remainderBy8; i++)
{
_pq_biquad0_fx(pSrc[i - 1]);
_pq_biquad1_fx(pSrc[i]);
pDst[i - 1] = (int32_t)_pq_readAdd0_fx();
pDst[i] = (int32_t)_pq_readAdd1_fx();
}
_pq_biquad0_fx(pSrc[remainderBy8 - 1]);
pDst[remainderBy8 - 1] = (int32_t)_pq_readAdd0_fx();
}
if (length > 0)
{
PQ_StartVector(&pSrc[remainderBy8], &pDst[remainderBy8], length);
PQ_Vector8BiquadDf2CascadeFixed32();
PQ_EndVector();
}
}
void PQ_VectorBiquadCascadeDf2Fixed16(int16_t *pSrc, int16_t *pDst, int32_t length)
{
int32_t remainderBy8 = length % 8;
if (remainderBy8 > 0)
{
length -= remainderBy8;
_pq_biquad1_fx(pSrc[0]);
pDst[0] = (int16_t)_pq_readAdd1_fx();
for (int i = 1; i < remainderBy8; i++)
{
_pq_biquad0_fx(pSrc[i - 1]);
_pq_biquad1_fx(pSrc[i]);
pDst[i - 1] = (int16_t)_pq_readAdd0_fx();
pDst[i] = (int16_t)_pq_readAdd1_fx();
}
_pq_biquad0_fx(pSrc[remainderBy8 - 1]);
pDst[remainderBy8 - 1] = (int16_t)_pq_readAdd0_fx();
}
if (length > 0)
{
PQ_StartVectorFixed16(&pSrc[remainderBy8], &pDst[remainderBy8], length);
PQ_Vector8BiquadDf2CascadeFixed16();
PQ_EndVector();
}
}
void PQ_BiquadBackUpInternalState(POWERQUAD_Type *base, int32_t biquad_num, pq_biquad_state_t *state)
{
pq_float_t v_n_1;
pq_float_t v_n;
#if defined(__GNUC__)
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Wstrict-aliasing"
#endif
if (0 == biquad_num)
{
v_n_1.integerX = base->GPREG[0];
v_n.integerX = base->GPREG[1];
state->param.v_n_1 = v_n_1.floatX;
state->param.v_n = v_n.floatX;
state->compreg = base->COMPREG[1];
}
else
{
v_n_1.integerX = base->GPREG[8];
v_n.integerX = base->GPREG[9];
state->param.v_n_1 = v_n_1.floatX;
state->param.v_n = v_n.floatX;
state->compreg = base->COMPREG[3];
}
#if defined(__GNUC__)
#pragma GCC diagnostic pop
#endif
}
void PQ_BiquadRestoreInternalState(POWERQUAD_Type *base, int32_t biquad_num, pq_biquad_state_t *state)
{
pq_float_t v_n_1;
pq_float_t v_n;
pq_float_t a_1;
pq_float_t a_2;
pq_float_t b_0;
pq_float_t b_1;
pq_float_t b_2;
if (0 == biquad_num)
{
v_n_1.floatX = state->param.v_n_1;
v_n.floatX = state->param.v_n;
a_1.floatX = state->param.a_1;
a_2.floatX = state->param.a_2;
b_0.floatX = state->param.b_0;
b_1.floatX = state->param.b_1;
b_2.floatX = state->param.b_2;
base->GPREG[0] = v_n_1.integerX;
base->GPREG[1] = v_n.integerX;
base->GPREG[2] = a_1.integerX;
base->GPREG[3] = a_2.integerX;
base->GPREG[4] = b_0.integerX;
base->GPREG[5] = b_1.integerX;
base->GPREG[6] = b_2.integerX;
base->COMPREG[1] = state->compreg;
}
else
{
v_n_1.floatX = state->param.v_n_1;
v_n.floatX = state->param.v_n;
a_1.floatX = state->param.a_1;
a_2.floatX = state->param.a_2;
b_0.floatX = state->param.b_0;
b_1.floatX = state->param.b_1;
b_2.floatX = state->param.b_2;
base->GPREG[8] = v_n_1.integerX;
base->GPREG[9] = v_n.integerX;
base->GPREG[10] = a_1.integerX;
base->GPREG[11] = a_2.integerX;
base->GPREG[12] = b_0.integerX;
base->GPREG[13] = b_1.integerX;
base->GPREG[14] = b_2.integerX;
base->COMPREG[3] = state->compreg;
}
}
void PQ_FIR(POWERQUAD_Type *base,
const void *pAData,
int32_t ALength,
const void *pBData,
int32_t BLength,
void *pResult,
uint32_t opType)
{
assert(NULL != pAData);
assert(NULL != pBData);
assert(NULL != pResult);
base->INABASE = (uint32_t)(const uint32_t *)pAData;
base->INBBASE = (uint32_t)(const uint32_t *)pBData;
base->LENGTH = ((uint32_t)BLength << 16U) + (uint32_t)ALength;
base->OUTBASE = (uint32_t)(uint32_t *)pResult;
base->CONTROL = (CP_FIR << 4U) | opType;
}
void PQ_FIRIncrement(POWERQUAD_Type *base, int32_t ALength, int32_t BLength, int32_t xOffset)
{
base->MISC = (uint32_t)xOffset;
base->LENGTH = ((uint32_t)BLength << 16U) + (uint32_t)ALength;
base->CONTROL = (CP_FIR << 4U) | PQ_FIR_INCREMENTAL;
}
void PQ_BiquadCascadeDf2Init(pq_biquad_cascade_df2_instance *S, uint8_t numStages, pq_biquad_state_t *pState)
{
S->numStages = numStages;
S->pState = pState;
}
void PQ_BiquadCascadeDf2F32(const pq_biquad_cascade_df2_instance *S, float *pSrc, float *pDst, uint32_t blockSize)
{
uint32_t stage = S->numStages;
pq_biquad_state_t *states = S->pState;
if (pDst != pSrc)
{
(void)memcpy(pDst, pSrc, 4U * blockSize);
}
if ((stage & 0x01U) == 0x01U)
{
PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
PQ_VectorBiquadDf2F32(pSrc, pDst, (int32_t)blockSize);
PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
states++;
stage--;
}
while (stage > 0U)
{
PQ_BiquadRestoreInternalState(POWERQUAD, 1, states);
states++;
PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
PQ_VectorBiquadCascadeDf2F32(pDst, pDst, (int32_t)blockSize);
states--;
PQ_BiquadBackUpInternalState(POWERQUAD, 1, states);
states++;
PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
states++;
stage -= 2U;
}
}
void PQ_BiquadCascadeDf2Fixed32(const pq_biquad_cascade_df2_instance *S,
int32_t *pSrc,
int32_t *pDst,
uint32_t blockSize)
{
uint32_t stage = S->numStages;
pq_biquad_state_t *states = S->pState;
if (pDst != pSrc)
{
(void)memcpy(pDst, pSrc, 4U * blockSize);
}
if ((stage & 0x01U) == 0x01U)
{
PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
PQ_VectorBiquadDf2Fixed32(pSrc, pDst, (int32_t)blockSize);
PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
states++;
stage--;
}
while (stage > 0U)
{
PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
states++;
PQ_BiquadRestoreInternalState(POWERQUAD, 1, states);
PQ_VectorBiquadCascadeDf2Fixed32(pDst, pDst, (int32_t)blockSize);
states--;
PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
states++;
PQ_BiquadBackUpInternalState(POWERQUAD, 1, states);
states++;
stage -= 2U;
}
}
void PQ_BiquadCascadeDf2Fixed16(const pq_biquad_cascade_df2_instance *S,
int16_t *pSrc,
int16_t *pDst,
uint32_t blockSize)
{
uint32_t stage = S->numStages;
pq_biquad_state_t *states = S->pState;
if (pDst != pSrc)
{
(void)memcpy(pDst, pSrc, 2U * blockSize);
}
if ((stage & 0x01U) == 0x01U)
{
PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
PQ_VectorBiquadDf2Fixed16(pSrc, pDst, (int32_t)blockSize);
PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
states++;
stage--;
}
while (stage > 0U)
{
PQ_BiquadRestoreInternalState(POWERQUAD, 0, states);
states++;
PQ_BiquadRestoreInternalState(POWERQUAD, 1, states);
PQ_VectorBiquadCascadeDf2Fixed16(pDst, pDst, (int32_t)blockSize);
states--;
PQ_BiquadBackUpInternalState(POWERQUAD, 0, states);
states++;
PQ_BiquadBackUpInternalState(POWERQUAD, 1, states);
states++;
stage -= 2U;
}
}
Reference in New Issue View Git Blame Copy Permalink