void FO_1I_D32F32C31_TRC_WRA_01( Biquad_Instance_t *pInstance,
LVM_INT32 *pDataIn,
LVM_INT32 *pDataOut,
LVM_INT16 NrSamples)
{
LVM_INT32 ynL,templ;
LVM_INT16 ii;
PFilter_State pBiquadState = (PFilter_State) pInstance;
for (ii = NrSamples; ii != 0; ii--)
{
/**************************************************************************
PROCESSING OF THE LEFT CHANNEL
***************************************************************************/
// ynL=A1 (Q31) * x(n-1)L (Q0) >>31 in Q0
MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[0],ynL,31)
// ynL+=A0 (Q31) * x(n)L (Q0) >> 31 in Q0
MUL32x32INTO32(pBiquadState->coefs[1],*pDataIn,templ,31)
ynL+=templ;
// ynL+= (-B1 (Q31) * y(n-1)L (Q0) ) >> 31 in Q0
MUL32x32INTO32(pBiquadState->coefs[2],pBiquadState->pDelays[1],templ,31)
ynL+=templ;
/**************************************************************************
UPDATING THE DELAYS
***************************************************************************/
pBiquadState->pDelays[1]=ynL; // Update y(n-1)L in Q0
pBiquadState->pDelays[0]=(*pDataIn++); // Update x(n-1)L in Q0
/**************************************************************************
WRITING THE OUTPUT
***************************************************************************/
*pDataOut++=(LVM_INT32)ynL; // Write Left output in Q0
}
}
LVPSA_RETURN LVPSA_Init ( pLVPSA_Handle_t *phInstance,
LVPSA_InitParams_t *pInitParams,
LVPSA_ControlParams_t *pControlParams,
LVPSA_MemTab_t *pMemoryTable )
{
LVPSA_InstancePr_t *pLVPSA_Inst;
LVPSA_RETURN errorCode = LVPSA_OK;
LVM_UINT32 ii;
extern LVM_INT16 LVPSA_GainTable[];
LVM_UINT32 BufferLength = 0;
/* Ints_Alloc instances, needed for memory alignment management */
INST_ALLOC Instance;
INST_ALLOC Scratch;
INST_ALLOC Data;
INST_ALLOC Coef;
/* Check parameters */
if((phInstance == LVM_NULL) || (pInitParams == LVM_NULL) || (pControlParams == LVM_NULL) || (pMemoryTable == LVM_NULL))
{
return(LVPSA_ERROR_NULLADDRESS);
}
if( (pInitParams->SpectralDataBufferDuration > LVPSA_MAXBUFFERDURATION) ||
(pInitParams->SpectralDataBufferDuration == 0) ||
(pInitParams->MaxInputBlockSize > LVPSA_MAXINPUTBLOCKSIZE) ||
(pInitParams->MaxInputBlockSize == 0) ||
(pInitParams->nBands < LVPSA_NBANDSMIN) ||
(pInitParams->nBands > LVPSA_NBANDSMAX) ||
(pInitParams->pFiltersParams == 0))
{
return(LVPSA_ERROR_INVALIDPARAM);
}
for(ii = 0; ii < pInitParams->nBands; ii++)
{
if((pInitParams->pFiltersParams[ii].CenterFrequency > LVPSA_MAXCENTERFREQ) ||
(pInitParams->pFiltersParams[ii].PostGain > LVPSA_MAXPOSTGAIN) ||
(pInitParams->pFiltersParams[ii].PostGain < LVPSA_MINPOSTGAIN) ||
(pInitParams->pFiltersParams[ii].QFactor < LVPSA_MINQFACTOR) ||
(pInitParams->pFiltersParams[ii].QFactor > LVPSA_MAXQFACTOR))
{
return(LVPSA_ERROR_INVALIDPARAM);
}
}
/*Inst_Alloc instances initialization */
InstAlloc_Init( &Instance , pMemoryTable->Region[LVPSA_MEMREGION_INSTANCE].pBaseAddress);
InstAlloc_Init( &Scratch , pMemoryTable->Region[LVPSA_MEMREGION_SCRATCH].pBaseAddress);
InstAlloc_Init( &Data , pMemoryTable->Region[LVPSA_MEMREGION_PERSISTENT_DATA].pBaseAddress);
InstAlloc_Init( &Coef , pMemoryTable->Region[LVPSA_MEMREGION_PERSISTENT_COEF].pBaseAddress);
/* Set the instance handle if not already initialised */
if (*phInstance == LVM_NULL)
{
*phInstance = InstAlloc_AddMember( &Instance, sizeof(LVPSA_InstancePr_t) );
}
pLVPSA_Inst =(LVPSA_InstancePr_t*)*phInstance;
/* Check the memory table for NULL pointers */
for (ii = 0; ii < LVPSA_NR_MEMORY_REGIONS; ii++)
{
if (pMemoryTable->Region[ii].Size!=0)
{
if (pMemoryTable->Region[ii].pBaseAddress==LVM_NULL)
{
return(LVPSA_ERROR_NULLADDRESS);
}
pLVPSA_Inst->MemoryTable.Region[ii] = pMemoryTable->Region[ii];
}
}
/* Initialize module's internal parameters */
pLVPSA_Inst->bControlPending = LVM_FALSE;
pLVPSA_Inst->nBands = pInitParams->nBands;
pLVPSA_Inst->MaxInputBlockSize = pInitParams->MaxInputBlockSize;
pLVPSA_Inst->SpectralDataBufferDuration = pInitParams->SpectralDataBufferDuration;
pLVPSA_Inst->CurrentParams.Fs = LVM_FS_DUMMY;
pLVPSA_Inst->CurrentParams.LevelDetectionSpeed = LVPSA_SPEED_DUMMY;
{ /* for avoiding QAC warnings */
LVM_INT32 SDBD=(LVM_INT32)pLVPSA_Inst->SpectralDataBufferDuration;
LVM_INT32 IRTI=(LVM_INT32)LVPSA_InternalRefreshTimeInv;
LVM_INT32 BL;
MUL32x32INTO32(SDBD,IRTI,BL,LVPSA_InternalRefreshTimeShift)
BufferLength=(LVM_UINT32)BL;
}
if((BufferLength * LVPSA_InternalRefreshTime) != pLVPSA_Inst->SpectralDataBufferDuration)
{
pLVPSA_Inst->SpectralDataBufferLength = BufferLength + 1;
}
else
{
pLVPSA_Inst->SpectralDataBufferLength = BufferLength;
}
/* Assign the pointers */
pLVPSA_Inst->pPostGains = InstAlloc_AddMember( &Instance, pInitParams->nBands * sizeof(LVM_UINT16) );
pLVPSA_Inst->pFiltersParams = InstAlloc_AddMember( &Instance, pInitParams->nBands * sizeof(LVPSA_FilterParam_t) );
pLVPSA_Inst->pSpectralDataBufferStart = InstAlloc_AddMember( &Instance, pInitParams->nBands * pLVPSA_Inst->SpectralDataBufferLength * sizeof(LVM_UINT8) );
pLVPSA_Inst->pPreviousPeaks = InstAlloc_AddMember( &Instance, pInitParams->nBands * sizeof(LVM_UINT8) );
pLVPSA_Inst->pBPFiltersPrecision = InstAlloc_AddMember( &Instance, pInitParams->nBands * sizeof(LVPSA_BPFilterPrecision_en) );
pLVPSA_Inst->pBP_Instances = InstAlloc_AddMember( &Coef, pInitParams->nBands * sizeof(Biquad_Instance_t) );
pLVPSA_Inst->pQPD_States = InstAlloc_AddMember( &Coef, pInitParams->nBands * sizeof(QPD_State_t) );
pLVPSA_Inst->pBP_Taps = InstAlloc_AddMember( &Data, pInitParams->nBands * sizeof(Biquad_1I_Order2_Taps_t) );
pLVPSA_Inst->pQPD_Taps = InstAlloc_AddMember( &Data, pInitParams->nBands * sizeof(QPD_Taps_t) );
/* Copy filters parameters in the private instance */
for(ii = 0; ii < pLVPSA_Inst->nBands; ii++)
{
pLVPSA_Inst->pFiltersParams[ii] = pInitParams->pFiltersParams[ii];
}
/* Set Post filters gains*/
for(ii = 0; ii < pLVPSA_Inst->nBands; ii++)
{
pLVPSA_Inst->pPostGains[ii] =(LVM_UINT16) LVPSA_GainTable[pInitParams->pFiltersParams[ii].PostGain + 15];
}
pLVPSA_Inst->pSpectralDataBufferWritePointer = pLVPSA_Inst->pSpectralDataBufferStart;
/* Initialize control dependant internal parameters */
errorCode = LVPSA_Control (*phInstance, pControlParams);
if(errorCode!=0)
{
return errorCode;
}
errorCode = LVPSA_ApplyNewSettings (pLVPSA_Inst);
if(errorCode!=0)
{
return errorCode;
}
return(errorCode);
}
void LVPSA_QPD_Process ( void *hInstance,
LVM_INT16 *pInSamps,
LVM_INT16 numSamples,
LVM_INT16 BandIndex)
{
/******************************************************************************
PARAMETERS
*******************************************************************************/
LVPSA_InstancePr_t *pLVPSA_Inst = (LVPSA_InstancePr_t*)hInstance;
QPD_State_t *pQPDState = (QPD_State_t*)&pLVPSA_Inst->pQPD_States[BandIndex];
/* Pointer to taps */
LVM_INT32* pDelay = pQPDState->pDelay;
/* Parameters needed during quasi peak calculations */
LVM_INT32 X0;
LVM_INT32 temp,temp2;
LVM_INT32 accu;
LVM_INT16 Xg0;
LVM_INT16 D0;
LVM_INT16 V0 = (LVM_INT16)(*pDelay);
/* Filter's coef */
LVM_INT32 Kp = pQPDState->Coefs[0];
LVM_INT32 Km = pQPDState->Coefs[1];
LVM_INT16 ii = numSamples;
LVM_UINT8 *pWrite = pLVPSA_Inst->pSpectralDataBufferWritePointer;
LVM_INT32 BufferUpdateSamplesCount = pLVPSA_Inst->BufferUpdateSamplesCount;
LVM_UINT16 DownSamplingFactor = pLVPSA_Inst->DownSamplingFactor;
/******************************************************************************
INITIALIZATION
*******************************************************************************/
/* Correct the pointer to take the first down sampled signal sample */
pInSamps += pLVPSA_Inst->DownSamplingCount;
/* Correct also the number of samples */
ii = (LVM_INT16)(ii - (LVM_INT16)pLVPSA_Inst->DownSamplingCount);
while (ii > 0)
{
/* Apply post gain */
X0 = ((*pInSamps) * pLVPSA_Inst->pPostGains[BandIndex]) >> (LVPSA_GAINSHIFT-1); /* - 1 to compensate scaling in process function*/
pInSamps = pInSamps + DownSamplingFactor;
/* Saturate and take absolute value */
if(X0 < 0)
X0 = -X0;
if (X0 > 0x7FFF)
Xg0 = 0x7FFF;
else
Xg0 = (LVM_INT16)(X0);
/* Quasi peak filter calculation */
D0 = (LVM_INT16)(Xg0 - V0);
temp2 = (LVM_INT32)D0;
MUL32x32INTO32(temp2,Kp,accu,31);
D0 = (LVM_INT16)(D0>>1);
if (D0 < 0){
D0 = (LVM_INT16)(-D0);
}
temp2 = (LVM_INT32)D0;
MUL32x32INTO32((LVM_INT32)D0,Km,temp,31);
accu +=temp + Xg0;
if (accu > 0x7FFF)
accu = 0x7FFF;
else if(accu < 0)
accu = 0x0000;
V0 = (LVM_INT16)accu;
if(((pLVPSA_Inst->nSamplesBufferUpdate - BufferUpdateSamplesCount) < DownSamplingFactor))
{
LVPSA_QPD_WritePeak( pLVPSA_Inst,
&pWrite,
BandIndex,
V0);
BufferUpdateSamplesCount -= pLVPSA_Inst->nSamplesBufferUpdate;
pLVPSA_Inst->LocalSamplesCount = (LVM_UINT16)(numSamples - ii);
}
BufferUpdateSamplesCount+=DownSamplingFactor;
ii = (LVM_INT16)(ii-DownSamplingFactor);
}
/* Store last taps in memory */
*pDelay = (LVM_INT32)(V0);
/* If this is the last call to the function after last band processing,
update the parameters. */
if(BandIndex == (pLVPSA_Inst->nRelevantFilters-1))
{
pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite;
/* Adjustment for 11025Hz input, 220,5 is normally
the exact number of samples for 20ms.*/
if((pLVPSA_Inst->pSpectralDataBufferWritePointer != pWrite)&&(pLVPSA_Inst->CurrentParams.Fs == LVM_FS_11025))
{
if(pLVPSA_Inst->nSamplesBufferUpdate == 220)
{
pLVPSA_Inst->nSamplesBufferUpdate = 221;
}
else
{
pLVPSA_Inst->nSamplesBufferUpdate = 220;
}
}
pLVPSA_Inst->pSpectralDataBufferWritePointer = pWrite;
pLVPSA_Inst->BufferUpdateSamplesCount = BufferUpdateSamplesCount;
pLVPSA_Inst->DownSamplingCount = (LVM_UINT16)(-ii);
}
}
void BQ_2I_D32F32C30_TRC_WRA_01 ( Biquad_Instance_t *pInstance,
LVM_INT32 *pDataIn,
LVM_INT32 *pDataOut,
LVM_INT16 NrSamples)
{
#if !(defined __ARM_HAVE_NEON)
LVM_INT32 ynL,ynR,templ,tempd;
LVM_INT16 ii;
PFilter_State pBiquadState = (PFilter_State) pInstance;
for (ii = NrSamples; ii != 0; ii--)
{
/**************************************************************************
PROCESSING OF THE LEFT CHANNEL
***************************************************************************/
/* ynL= ( A2 (Q30) * x(n-2)L (Q0) ) >>30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[2],ynL,30)
/* ynL+= ( A1 (Q30) * x(n-1)L (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[0],templ,30)
ynL+=templ;
/* ynL+= ( A0 (Q30) * x(n)L (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[2],*pDataIn,templ,30)
ynL+=templ;
/* ynL+= (-B2 (Q30) * y(n-2)L (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[6],templ,30)
ynL+=templ;
/* ynL+= (-B1 (Q30) * y(n-1)L (Q0) ) >> 30 in Q0 */
MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[4],templ,30)
ynL+=templ;
/**************************************************************************
PROCESSING OF THE RIGHT CHANNEL
***************************************************************************/
/* ynR= ( A2 (Q30) * x(n-2)R (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[3],ynR,30)
/* ynR+= ( A1 (Q30) * x(n-1)R (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[1],templ,30)
ynR+=templ;
/* ynR+= ( A0 (Q30) * x(n)R (Q0) ) >> 30 in Q0*/
tempd=*(pDataIn+1);
MUL32x32INTO32(pBiquadState->coefs[2],tempd,templ,30)
ynR+=templ;
/* ynR+= (-B2 (Q30) * y(n-2)R (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[7],templ,30)
ynR+=templ;
/* ynR+= (-B1 (Q30) * y(n-1)R (Q0) ) >> 30 in Q0 */
MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[5],templ,30)
ynR+=templ;
/**************************************************************************
UPDATING THE DELAYS
***************************************************************************/
pBiquadState->pDelays[7]=pBiquadState->pDelays[5]; /* y(n-2)R=y(n-1)R*/
pBiquadState->pDelays[6]=pBiquadState->pDelays[4]; /* y(n-2)L=y(n-1)L*/
pBiquadState->pDelays[3]=pBiquadState->pDelays[1]; /* x(n-2)R=x(n-1)R*/
pBiquadState->pDelays[2]=pBiquadState->pDelays[0]; /* x(n-2)L=x(n-1)L*/
pBiquadState->pDelays[5]=(LVM_INT32)ynR; /* Update y(n-1)R in Q0*/
pBiquadState->pDelays[4]=(LVM_INT32)ynL; /* Update y(n-1)L in Q0*/
pBiquadState->pDelays[0]=(*pDataIn); /* Update x(n-1)L in Q0*/
pDataIn++;
pBiquadState->pDelays[1]=(*pDataIn); /* Update x(n-1)R in Q0*/
pDataIn++;
/**************************************************************************
WRITING THE OUTPUT
***************************************************************************/
*pDataOut=(LVM_INT32)ynL; /* Write Left output in Q0*/
pDataOut++;
*pDataOut=(LVM_INT32)ynR; /* Write Right ouput in Q0*/
pDataOut++;
}
#else
LVM_INT16 ii=0;
PFilter_State pBiquadState = (PFilter_State) pInstance;
int32x2_t A2 = vdup_n_s32(pBiquadState->coefs[0]);
int32x2_t A1 = vdup_n_s32(pBiquadState->coefs[1]);
int32x2_t A0 = vdup_n_s32(pBiquadState->coefs[2]);
int32x2_t B2 = vdup_n_s32(pBiquadState->coefs[3]);
int32x2_t B1 = vdup_n_s32(pBiquadState->coefs[4]);
int32x2_t X_2 = vld1_s32(&pBiquadState->pDelays[2]);
int32x2_t X_1 = vld1_s32(&pBiquadState->pDelays[0]);
int32x2_t Y_2 = vld1_s32(&pBiquadState->pDelays[6]);
int32x2_t Y_1 = vld1_s32(&pBiquadState->pDelays[4]);
for(ii=0; ii<NrSamples; ii++){
int32x2_t s = vld1_s32(pDataIn);
int64x2_t r = vmull_s32(A2, X_2);
r = vmlal_s32(r, A1, X_1);
r = vmlal_s32(r, A0, s);
r = vmlal_s32(r, B2, Y_2);
r = vmlal_s32(r, B1, Y_1);
int32_t ll =(int32_t)( vgetq_lane_s64(r, 0) >> 30);
int32_t rr =(int32_t)( vgetq_lane_s64(r, 1) >> 30);
pDataIn += 2;
*pDataOut ++ = ll;
*pDataOut ++ = rr;
int32_t tmp1, tmp2;
tmp1 = vget_lane_s32(X_1, 0);
tmp2 = vget_lane_s32(X_1, 1);
vset_lane_s32(tmp1, X_2, 0);
vset_lane_s32(tmp2, X_2, 1);
tmp1 = vget_lane_s32(Y_1, 0);
tmp2 = vget_lane_s32(Y_1, 1);
vset_lane_s32(tmp1, Y_2, 0);
vset_lane_s32(tmp2, Y_2, 1);
vset_lane_s32(ll, Y_1, 0);
vset_lane_s32(rr, Y_1, 1);
tmp1 = vget_lane_s32(s, 0);
tmp2 = vget_lane_s32(s, 1);
vset_lane_s32(tmp1, X_1, 0);
vset_lane_s32(tmp2, X_1, 1);
}
vst1_s32(&pBiquadState->pDelays[2], X_2);
vst1_s32(&pBiquadState->pDelays[0], X_1);
vst1_s32(&pBiquadState->pDelays[6], Y_2);
vst1_s32(&pBiquadState->pDelays[4], Y_1);
#endif
}
void BQ_2I_D32F32C30_TRC_WRA_01 ( Biquad_Instance_t *pInstance,
LVM_INT32 *pDataIn,
LVM_INT32 *pDataOut,
LVM_INT16 NrSamples)
{
LVM_INT32 ynL,ynR,templ,tempd;
LVM_INT16 ii;
PFilter_State pBiquadState = (PFilter_State) pInstance;
for (ii = NrSamples; ii != 0; ii--)
{
/**************************************************************************
PROCESSING OF THE LEFT CHANNEL
***************************************************************************/
/* ynL= ( A2 (Q30) * x(n-2)L (Q0) ) >>30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[2],ynL,30)
/* ynL+= ( A1 (Q30) * x(n-1)L (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[0],templ,30)
ynL+=templ;
/* ynL+= ( A0 (Q30) * x(n)L (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[2],*pDataIn,templ,30)
ynL+=templ;
/* ynL+= (-B2 (Q30) * y(n-2)L (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[6],templ,30)
ynL+=templ;
/* ynL+= (-B1 (Q30) * y(n-1)L (Q0) ) >> 30 in Q0 */
MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[4],templ,30)
ynL+=templ;
/**************************************************************************
PROCESSING OF THE RIGHT CHANNEL
***************************************************************************/
/* ynR= ( A2 (Q30) * x(n-2)R (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[0],pBiquadState->pDelays[3],ynR,30)
/* ynR+= ( A1 (Q30) * x(n-1)R (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[1],pBiquadState->pDelays[1],templ,30)
ynR+=templ;
/* ynR+= ( A0 (Q30) * x(n)R (Q0) ) >> 30 in Q0*/
tempd=*(pDataIn+1);
MUL32x32INTO32(pBiquadState->coefs[2],tempd,templ,30)
ynR+=templ;
/* ynR+= (-B2 (Q30) * y(n-2)R (Q0) ) >> 30 in Q0*/
MUL32x32INTO32(pBiquadState->coefs[3],pBiquadState->pDelays[7],templ,30)
ynR+=templ;
/* ynR+= (-B1 (Q30) * y(n-1)R (Q0) ) >> 30 in Q0 */
MUL32x32INTO32(pBiquadState->coefs[4],pBiquadState->pDelays[5],templ,30)
ynR+=templ;
/**************************************************************************
UPDATING THE DELAYS
***************************************************************************/
pBiquadState->pDelays[7]=pBiquadState->pDelays[5]; /* y(n-2)R=y(n-1)R*/
pBiquadState->pDelays[6]=pBiquadState->pDelays[4]; /* y(n-2)L=y(n-1)L*/
pBiquadState->pDelays[3]=pBiquadState->pDelays[1]; /* x(n-2)R=x(n-1)R*/
pBiquadState->pDelays[2]=pBiquadState->pDelays[0]; /* x(n-2)L=x(n-1)L*/
pBiquadState->pDelays[5]=(LVM_INT32)ynR; /* Update y(n-1)R in Q0*/
pBiquadState->pDelays[4]=(LVM_INT32)ynL; /* Update y(n-1)L in Q0*/
pBiquadState->pDelays[0]=(*pDataIn); /* Update x(n-1)L in Q0*/
pDataIn++;
pBiquadState->pDelays[1]=(*pDataIn); /* Update x(n-1)R in Q0*/
pDataIn++;
/**************************************************************************
WRITING THE OUTPUT
***************************************************************************/
*pDataOut=(LVM_INT32)ynL; /* Write Left output in Q0*/
pDataOut++;
*pDataOut=(LVM_INT32)ynR; /* Write Right ouput in Q0*/
pDataOut++;
}
}
