amf_graphicequalizer.c

ADI SHARC DSP 音频算法标准模块库

// Copyright(c) 2007 Analog Devices, Inc. All Rights Reserved.
// This software is proprietary and confidential to Analog Devices, Inc. and its licensors.

// File    : $Id: AMF_GraphicEqualizer.c 230 2007-07-13 13:25:18Z asripada $ 
// Part of : VisualAudio V2.5.0 
// Updated : $Date: 2007-07-13 18:55:18 +0530 (Fri, 13 Jul 2007) $ by $Author: asripada $


#include <math.h>
#include "AMF_GraphicEqualizer.h"


#if (AMF_SIGNALS_ARE_FLOATING_POINT==0)
typedef unsigned long long U64;
typedef long long I64;


#define Q31_TO_FLOAT_SCALE 0x7FFFFFFF
#define Q30_TO_FLOAT_SCALE 0x3FFFFFFF

#define Q31_TO_FLOAT(x) ((float)(((float)x)/Q31_TO_FLOAT_SCALE))
#define FLOAT_TO_Q31(x) ((AMF_Signal)(x*Q31_TO_FLOAT_SCALE))
#define Q30_TO_FLOAT(x) ((float)(((float)x)/Q30_TO_FLOAT_SCALE))
#define FLOAT_TO_Q30(x) ((AMF_Signal)(x*Q30_TO_FLOAT_SCALE))
#define MULT_Q31(x,y)  ((AMF_Signal)(((I64)x * (I64)y)>>31))
#define MULT_Q30(x,y)  ((AMF_Signal)(((I64)x * (I64)y)>>30))
#define DOUBLE(x) (x<<1)


#define STATE_TO_Q31(x) ({if(x>8388607.0) x = 8388607.0;if(x<-8388608.0) x = -8388608;x<<8;})
#define Q31_TO_STATE(x) x>>8

#define STATE_TO_SIGNAL(x) STATE_TO_Q31(x)
#define SIGNAL_TO_STATE(x) Q31_TO_STATE(x)


#define COEFF0_TO_FLOAT(x) Q31_TO_FLOAT(x)
#define COEFF1_TO_FLOAT(x) Q30_TO_FLOAT(x)
#define FLOAT_TO_COEFF0(x) FLOAT_TO_Q31(x)
#define FLOAT_TO_COEFF1(x) FLOAT_TO_Q30(x)


#define MULT_COEFF0(x,y)  MULT_Q31(x,y)
#define MULT_COEFF1(x,y)  MULT_Q30(x,y)

#else

#define DOUBLE(x) (x*2)
#define COEFF0_TO_FLOAT(x) x
#define COEFF1_TO_FLOAT(x) x
#define FLOAT_TO_COEFF0(x) x
#define FLOAT_TO_COEFF1(x) x

#define STATE_TO_SIGNAL(x) x
#define SIGNAL_TO_STATE(x) x

#define MULT_COEFF0(x,y) x*y
#define MULT_COEFF1(x,y) x*y

#endif


#define FILTER_ORDER 4



SEG_MOD_SLOW_CONST const AMF_ModuleClass AMFClassGraphicEqualizer = {
    
    /* Flags */
    0,
     
    /* Render function */
    (AMF_RenderFunction)AMF_GraphicEqualizer_Render,  // render function 
    
    /* Default bypass */
    (void *)0,

    /* Input descriptor - 1 input, and it is mono. */
    1, AMF_MonoPin(0),

    /* Output descriptor - 1 output, and it is mono. */
    1, AMF_MonoPin(0),
};





// NOTE: This function is the C reference of the equalizer, it 
//       details the general algorithm that the module uses.

#if 0
SEG_MOD_FAST_CODE
 void AMF_GraphicEqualizer_Render(AMF_GraphicEqualizer *  restrict instance, AMF_Signal *restrict *buffers,int tickSize)
{
    int numBands = instance->numBands;
    int numStages = instance->numStages;
    int band, stage ,i;
    AMF_Signal *input = buffers[0];
    AMF_Signal *output = buffers[1];
    AMF_Signal c0,  V, K;
    AMF_Signal s[4],c, a0Inv;
	AMF_Signal x4,x5,x6,x7,x8,x9;
	AMF_Signal *pC0= (AMF_Signal *)instance->pC0;
	AMF_Signal *pC = (AMF_Signal *)instance->pC;
	AMF_Signal *pA0Inv= (AMF_Signal *)instance->pA0Inv;    
	AMF_Signal *pStates= (AMF_Signal *)instance->pStates;
	AMF_Signal *pK= (AMF_Signal *)instance->pK;
	AMF_Signal *pV= (AMF_Signal *)instance->pV;
#ifdef ENA_TUNE_IN_DSP
    AMF_GraphicEqualizer_Tune(instance,buffers,tickSize);
#endif
    for(i=0;i<tickSize;i++)
    {
        output[i] = SIGNAL_TO_STATE(input[i]);
    }

	for (band = 0; band < numBands; band++)
	{
		c0 = pC0[band];
		V = pV[band];
		K = pK[band];
            

		for (stage = 0; stage < numStages; stage++)
		{

			c    = pC[stage];
			a0Inv = pA0Inv[band*numStages + stage];
			s[0] = pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 0];
			s[1] = pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 1];
			s[2] = pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 2];
			s[3] = pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 3];


			for(i=0;i<tickSize;i++)
			{
                x4 =  s[0] - s[1];
				x4 =  MULT_COEFF0(c0,x4);
				x6 = s[1] - x4;
				s[1] = s[0]	- x4;

				x4 =  s[2] - s[3];
				x4 =  MULT_COEFF0(c0,x4);
				x7 = s[3] - x4;
				s[3] = s[2]	- x4;
				s[2] = -x6;
				
				x6 = DOUBLE(x6);
				x8 = x7 + x6;


				x9 = -DOUBLE(x7);
				x4 =  MULT_COEFF1(K ,x8);
				x4 += MULT_COEFF0(x9,c); 
				x4 = MULT_COEFF1(K ,x4);
				x4 -= x6 ;
				x4 += x7;
				x4 -= MULT_COEFF1(K,output[i]); 
				x9 = MULT_COEFF0(a0Inv,x4); //-x9

				x5 = x8-x9;
				x5 = MULT_COEFF1(K ,x5);
				s[0] = x9;


				x4 = x7 + x9;
				x4 = MULT_COEFF0(c ,x4);
				x4 = x5 - x4;
				x4 = DOUBLE(x4);
				x4 += MULT_COEFF0(V,x5);
				x4 = MULT_COEFF0(V,x4);
				x4 +=  output[i];
                x4 = SIGNAL_TO_STATE(STATE_TO_SIGNAL(x4));
				output[i] = x4;
			}

            pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 0] = s[0];
            pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 1] = s[1];
            pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 2] = s[2];
            pStates[band * numStages*FILTER_ORDER + stage * FILTER_ORDER + 3] = s[3];            
					
		}
	}
    for(i=0;i<tickSize;i++)
    {
        output[i] = STATE_TO_SIGNAL(output[i]);
    }
}
#endif


#ifdef ENA_TUNE_IN_DSP



#define _PI_ (3.1415926535897932384626433832795)
#define MAX_UPPER_FREQ_RAD 0.9*_PI_

 void  EQ_setGain(int, float, AMF_GraphicEqualizer *);
 void  EQ_initFilters(AMF_GraphicEqualizer *);
 void  EQ_updateFilters(AMF_GraphicEqualizer *); 

SEG_MOD_FAST_CODE
void AMF_GraphicEqualizer_Tune(AMF_GraphicEqualizer *instance,AMF_Signal **buffers , int tickSize)
{
    if (instance->initFlag==0)
    {
    	EQ_initFilters(instance);
    } 

    /* Update filter coeffs based on gain values */
	EQ_updateFilters(instance);
    
}


 void EQ_setGain(int band, float gain, AMF_GraphicEqualizer* instance )
{
    int stage;
	int numStages = instance->numStages;
    float k,v, a0Inv, kBase,c;
    float *pKBase = (float *)instance->pKBase;
	AMF_Signal *pC = (AMF_Signal *)instance->pC;
	AMF_Signal *pA0Inv = (AMF_Signal *)instance->pA0Inv;
	AMF_Signal *pK= (AMF_Signal *)instance->pK;
	AMF_Signal *pV= (AMF_Signal *)instance->pV;

    kBase = pKBase[band];
    
	k = pow(gain, -1.0 / (4 * numStages)) * kBase ;
	//v = sqrt(gain) - 1;
	v = pow(gain,.25) - 1;
	for (stage = 0; stage < numStages; stage++)
    {
        c = COEFF0_TO_FLOAT(pC[stage]);
        a0Inv = 1 / (1 + 2 * k * c + k * k);
        pA0Inv[band*numStages + stage] = FLOAT_TO_COEFF0(a0Inv);
    }

	pV[band] = FLOAT_TO_COEFF0(v);
    pK[band] = FLOAT_TO_COEFF1(k);

}

 void EQ_initFilters(AMF_GraphicEqualizer * instance)
{
    int stage, band;
    
    float c;
	float *pCenterFreq = (float *)instance->pCenterFreq;
    float *pPrevGain =(float *) instance->pPrevGain;
	float *pGain = (float *)instance->pGain;
	AMF_Signal *pC = (AMF_Signal  *)instance->pC;
	float firstCenterFreq;
	int numStages = instance->numStages;
	int numBands = instance->numBands;
	float fB, wB, wU, wL, wM; 
	float currQ,c0;
	float fL, fU;
	float centerFreq;
	float *pKBase = (float *)instance->pKBase;
	AMF_Signal *pC0 = instance->pC0;
	int samplingRate = instance->samplingRate;

    firstCenterFreq = instance->firstCenterFreq;


	

	for (stage = 0; stage < numStages; stage++) 
    {
        c = cos((.5 - (2.0 * stage + 1) / (4 * numStages)) * _PI_);
		pC[stage] = FLOAT_TO_COEFF0(c);
    }
    
	for (band = 0; band < numBands; band++)
	{
		pCenterFreq[band] = firstCenterFreq * pow(2, band*10.0/numBands);
#if 1
			currQ = pow(2,10.0/numBands);
            centerFreq = pCenterFreq[band];
			fL  = centerFreq / sqrt(currQ);
			fU = centerFreq * sqrt(currQ);


			wU = 2 * _PI_ / samplingRate * fU;
			wL = 2 * _PI_ / samplingRate * fL;


			wU = (wU > MAX_UPPER_FREQ_RAD)	? MAX_UPPER_FREQ_RAD: wU;
			wL = (wL > MAX_UPPER_FREQ_RAD)	? MAX_UPPER_FREQ_RAD: wL;

			wB = wU - wL;
			wM = 2 * atan(sqrt(tan(wU / 2) * tan(wL / 2)));
			pKBase[band] = tan(wB / 2);
			c0 = cos(wM);
			pC0[band] = FLOAT_TO_COEFF0(c0);
#endif
		pPrevGain[band]= -pGain[band];
		if(pPrevGain[band]==0)	
			pPrevGain[band]=-1;	/* any negative number  */
	}

	instance->initFlag = 1;
}


 void EQ_updateFilters(AMF_GraphicEqualizer * instance)
{
	float fB, wB, wU, wL, wM, scale; 
	float currQ, prevQ,c0;
	float fL, fU;
	float gain, prevGain;
	int band;
	int numStages = instance->numStages;
	int numBands = instance->numBands;
	int samplingRate = instance->samplingRate;
    
	float *pKBase = (float *)instance->pKBase;
    float *pGain = (float *)instance->pGain;
    float *pCenterFreq = (float *)instance->pCenterFreq;
    float *pPrevGain = (float *)instance->pPrevGain;
	float centerFreq;

	AMF_Signal *pC0 = instance->pC0;


	for(band=0;band<numBands;band++)	
	{

		gain = pGain[band];
		prevGain = pPrevGain[band];
#if 0
		if (gain >= 0.0)
			currQ = 1.9;
		else
			currQ = 2.1;

		if (prevGain >= 0.0)
			prevQ = 1.9;
		else	
			prevQ = 2.1;




		if (currQ != prevQ)
		{	
			currQ = pow(currQ,10.0/numBands);
            centerFreq = pCenterFreq[band];
			fL  = centerFreq / sqrt(currQ);
			fU = centerFreq * sqrt(currQ);


			wU = 2 * _PI_ / samplingRate * fU;
			wL = 2 * _PI_ / samplingRate * fL;


			wU = (wU > MAX_UPPER_FREQ_RAD)	? MAX_UPPER_FREQ_RAD: wU;
			wL = (wL > MAX_UPPER_FREQ_RAD)	? MAX_UPPER_FREQ_RAD: wL;

			wB = wU - wL;
			wM = 2 * atan(sqrt(tan(wU / 2) * tan(wL / 2)));
			pKBase[band] = tan(wB / 2);
			c0 = cos(wM);
			pC0[band] = FLOAT_TO_COEFF0(c0);
		}
#endif
	
		if(gain != prevGain)
		{
			scale = pow(10, gain/20.0); 
			EQ_setGain(band, scale, instance);
			pPrevGain[band] = gain;
		}	

	}

}

#endif