amf_graphicequalizer_ds.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_DS.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_DS.h"

#define NUM_CHANNELS 2

#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_DS = {
    
    /* Flags */
    0,
     
    /* Render function */
    (AMF_RenderFunction)AMF_GraphicEqualizer_DS_Render,  // render function 
    
    /* Default bypass */
    (void *)0,

    /* Input descriptor - 1 input, and it is stereo. */
    1,  AMF_StereoPin(0),

    /* Output descriptor - 1 output, and it is stereo. */
    1,  AMF_StereoPin(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_DS_Render(AMF_GraphicEqualizer_DS *  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[8],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;
	int channel_count;

#ifdef ENA_TUNE_IN_DSP
    AMF_GraphicEqualizer_DS_Tune(instance,buffers,tickSize);
#endif
    for(i=0;i<2*tickSize;i++)
    {
        output[i] = SIGNAL_TO_STATE(input[i]);
    }

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


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

            {

                c    = pC[stage];
                a0Inv = pA0Inv[NUM_CHANNELS*(band*numStages + stage) + channel_count];

                s[0] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 0];
                s[2] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 2];
                s[4] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 4];
                s[6] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 6];
                /*** For the second Channel ********/
                s[1] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 1];
                s[3] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 3];
                s[5] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 5];
                s[7] = pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 7];

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

                    x4 =  s[4 + channel_count] - s[6 + channel_count];
                    x4 =  MULT_COEFF0(c0,x4);
                    x7 = s[6 + channel_count] - x4;
                    s[6 + channel_count] = s[4 + channel_count]	- x4;
                    s[4 + channel_count] = -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[2*i + channel_count]); 
                    x9 = MULT_COEFF0(a0Inv,x4); //-x9

                    x5 = x8-x9;
                    x5 = MULT_COEFF1(K ,x5);
                    s[0 + channel_count] = 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[2*i + channel_count];
                    x4 = SIGNAL_TO_STATE(STATE_TO_SIGNAL(x4));
                    output[2*i + channel_count] = x4;
                }



                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 0] = s[0];
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 1] = s[1];
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 2] = s[2];
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 3] = s[3];            
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 4] = s[4];
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 5] = s[5];
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 6] = s[6];
                pStates[(band * numStages*FILTER_ORDER + stage * FILTER_ORDER)*2 + 7] = s[7];            

            }



        }
    }
    for(i=0;i<2*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_DS(int, float, AMF_GraphicEqualizer_DS *);
 void  EQ_initFilters_DS(AMF_GraphicEqualizer_DS *);
 void  EQ_updateFilters_DS(AMF_GraphicEqualizer_DS *); 

void AMF_GraphicEqualizer_DS_Tune(AMF_GraphicEqualizer_DS *instance,AMF_Signal **buffers , int tickSize)
{
    if (instance->initFlag==0)
    {
    	EQ_initFilters_DS(instance);
    } 

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


 void EQ_setGain_DS(int band, float gain, AMF_GraphicEqualizer_DS* 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);

        if(band%2) // Second Channel 
            pA0Inv[(band-1)*numStages + 1 + 2*stage] = FLOAT_TO_COEFF0(a0Inv);
        else //First Channel 
            pA0Inv[band*numStages + 2*stage] = FLOAT_TO_COEFF0(a0Inv);

    }

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

}

 void EQ_initFilters_DS(AMF_GraphicEqualizer_DS * 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>>1];
			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[2*band]= -pGain[2*band];
		if(pPrevGain[2*band]==0)	
			pPrevGain[2*band]=-1;	/* any negative number  */

		pPrevGain[2*band+1]= -pGain[2*band+1];
		if(pPrevGain[2*band+1]==0)	
			pPrevGain[2*band+1]=-1;	/* any negative number  */

	}

	instance->initFlag = 1;
}


 void EQ_updateFilters_DS(AMF_GraphicEqualizer_DS * 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<2*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>>1];
			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_DS(band, scale, instance);
			pPrevGain[band] = gain;
		}	

	}

}

#endif