IIR coefficients and difference equation implementation in C language
/**********************************************************************
*                                                                     *
*                                                                     *
*                                                                     *
*                        Copyright (c) 2005                           *
*                                                                     *
*                      Robert Bristow-Johnson                         *
*                                                                     *
*                     rbj@audioimagination.com                        *
*                                                                     *
*                                                                     *
*                                 EQ.c                                *
*                                                                     *
*                                                                     *
*   Utilities for computing EQ filter coefficients, plotting          *
*   frequency response, and EQ filtering sampled audio data.          *
*                                                                     *
*                                                                     *
**********************************************************************/






#include &lt;stdio.h&gt;
#include "__functions.h"

enum EQ_type
{
LPF,
HPF,
BPF,
BPF0,
notch,
APF,
peakingEQ,
lowShelf,
highShelf,
lowShelf1,
highShelf1
};


typedef struct
{
float Fs;
enum EQ_type type;
float f0;
float dBgain;
float Q;
float filterCoefficients[5];    /* b0, b1, b2, a1, a2   (a0 is normallized to 1) */
float graphCoefficients[6];     /* B2, B1, B0, A2, A1, A0 */
} sectionData;



int main(void);
int computeSectionCofficients(float Fs, enum EQ_type type, float f0, float dBgain, float Q, sectionData *data);
int clearArray(long N, float *array);
int plotSectionFrequencyResponse(int Npoints, float Fmin, float Fmax, float *frequencyValues, float *dBvalues, sectionData *data);
int filterBuffer(long Nsamples, int Nsections, float *input, float *output, float *filterStates, sectionData **sections);



#define BUFFER_SIZE 1024
#define GRAPH_SIZE 589
#define NUM_SECTIONS 4
#define FS 44100.0
#define FMIN 19.99955848414893
#define FMAX 19999.55848414893

#define F1 50.0
#define G1 0.0
#define Q1 1.0

#define F2 200.0
#define G2 0.0
#define Q2 2.0

#define F3 1000.0
#define G3 10.0
#define Q3 2.0

#define F4 5000.0
#define G4 0.0
#define Q4 1.0

sectionData section1, section2, section3, section4;         /* this has to be global */

void plotTheFrequencyResponse(void);
void runTheFilter(void);

int main(void)
{
computeSectionCofficients(FS, lowShelf1,  F1, G1, Q1, &amp;section1);
computeSectionCofficients(FS, peakingEQ,  F2, G2, Q2, &amp;section2);
computeSectionCofficients(FS, peakingEQ,  F3, G3, Q3, &amp;section3);
computeSectionCofficients(FS, highShelf1, F4, G4, Q4, &amp;section4);

plotTheFrequencyResponse();
runTheFilter();
}


void plotTheFrequencyResponse(void)
{
float freq[GRAPH_SIZE];
float graph[GRAPH_SIZE];

clearArray(GRAPH_SIZE, graph);
plotSectionFrequencyResponse(GRAPH_SIZE, FMIN, FMAX, freq, graph, &amp;section1);
plotSectionFrequencyResponse(GRAPH_SIZE, FMIN, FMAX, freq, graph, &amp;section2);
plotSectionFrequencyResponse(GRAPH_SIZE, FMIN, FMAX, freq, graph, &amp;section3);
plotSectionFrequencyResponse(GRAPH_SIZE, FMIN, FMAX, freq, graph, &amp;section4);
}



void runTheFilter(void)
{
float aFilterBuffer[BUFFER_SIZE];
float aFilterStates[(2*NUM_SECTIONS+2)];
sectionData *sectionPtrArray[NUM_SECTIONS];

sectionPtrArray[0] = &amp;section1;
sectionPtrArray[1] = &amp;section2;
sectionPtrArray[2] = &amp;section3;
sectionPtrArray[3] = &amp;section4;

clearArray(2*NUM_SECTIONS+2, aFilterStates);
clearArray(BUFFER_SIZE, aFilterBuffer);
aFilterBuffer[0] = 1.0;                         /* input an impulse to get impulse response */
filterBuffer(BUFFER_SIZE, NUM_SECTIONS, aFilterBuffer, aFilterBuffer, aFilterStates, sectionPtrArray);
}





int computeSectionCofficients(float Fs, enum EQ_type type, float f0, float dBgain, float Q, sectionData *data)
{
register float temp, *coef_ptr;
float A = __exp2(0.08304820237218*dBgain);
float A2 = A*A;
float Ap1, Am1;
float cos_w0 = __cos_pi(2.0*f0/Fs);
float sin_w0 = __sin_pi(2.0*f0/Fs);
float alpha = 0.5*sin_w0/Q;
float a0, a1, a2, b0, b1, b2;

switch (type)
    {
    case LPF:
        b0 =  0.5*(1.0 - cos_w0)*A2;
        b1 =  2.0*b0;
        b2 =      b0;
        a0 =   1.0 + alpha;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha;
        break;

    case HPF:
        b0 =  0.5*(1.0 + cos_w0)*A2;
        b1 = -2.0*b0;
        b2 =      b0;
        a0 =   1.0 + alpha;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha;
        break;

    case BPF:
        b0 =   0.5*sin_w0*A2;
        b1 =   0.0;
        b2 =  -b0;
        a0 =   1.0 + alpha;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha;
        break;

    case BPF0:
        b0 =   alpha*A2;
        b1 =   0.0;
        b2 =  -b0;
        a0 =   1.0 + alpha;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha;
        break;

    case notch:
        b0 =   A2;
        b1 =  -2.0*cos_w0*A2;
        b2 =   A2;
        a0 =   1.0 + alpha;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha;
        break;

    case APF:
        b0 =  (1.0 - alpha)*A2;
        b1 =  -2.0*cos_w0*A2;
        b2 =  (1.0 + alpha)*A2;
        a0 =   1.0 + alpha;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha;
        break;

    case peakingEQ:
        if (A &gt; 1.0)
            {
            alpha *= A;
            }
         else
            {
            alpha /= A;
            }
        b0 =   1.0 + alpha*A;
        b1 =  -2.0*cos_w0;
        b2 =   1.0 - alpha*A;
        a0 =   1.0 + alpha/A;
        a1 =  -2.0*cos_w0;
        a2 =   1.0 - alpha/A;
        break;

    case lowShelf:
        temp = 2.0*__sqrt(A)*alpha;
        Ap1  = A + 1.0;
        Am1  = A - 1.0;
        b0 =  A*(Ap1 - Am1*cos_w0 + temp);
        b1 =  2.0*A*(Am1 - Ap1*cos_w0);
        b2 =  A*(Ap1 - Am1*cos_w0 - temp);
        a0 =  Ap1 + Am1*cos_w0 + temp;
        a1 =  -2.0*( Am1 + Ap1*cos_w0);
        a2 =  Ap1 + Am1*cos_w0 - temp;
        break;

    case highShelf:
        temp = 2.0*__sqrt(A)*alpha;
        Ap1  = A + 1.0;
        Am1  = A - 1.0;
        b0 =  A*(Ap1 + Am1*cos_w0 + temp);
        b1 = -2.0*A*(Am1 + Ap1*cos_w0);
        b2 =  A*(Ap1 + Am1*cos_w0 - temp);
        a0 =  Ap1 - Am1*cos_w0 + temp;
        a1 =  2.0*( Am1 - Ap1*cos_w0);
        a2 =  Ap1 - Am1*cos_w0 - temp;
        break;

    case lowShelf1:
        temp = cos_w0 + 1.0;
        b0 =  A2*sin_w0 +    temp;
        b1 =  A2*sin_w0 -    temp;
        b2 =  0.0;
        a0 =     sin_w0 +    temp;
        a1 =     sin_w0 -    temp;
        a2 =  0.0;
        break;

    case highShelf1:
        temp = cos_w0 + 1.0;
        b0 =     sin_w0 + A2*temp;
        b1 =     sin_w0 - A2*temp;
        b2 =  0.0;
        a0 =     sin_w0 +    temp;
        a1 =     sin_w0 -    temp;
        a2 =  0.0;
        break;
    }

coef_ptr = &amp;(data-&gt;filterCoefficients[0]);
temp = 1/a0;
*coef_ptr++ = temp*b0;                      /* b0/a0 */
*coef_ptr++ = temp*b1;                      /* b1/a0 */
*coef_ptr++ = temp*b2;                      /* b2/a0 */
*coef_ptr++ = temp*a1;                      /* a1/a0 */
*coef_ptr   = temp*a2;                      /* a2/a0 */

coef_ptr = &amp;(data-&gt;graphCoefficients[0]);
temp = b0*b2;
*coef_ptr++ = temp;                         /* B2 */
*coef_ptr++ = -0.25*b1*(b0 + b2) - temp;    /* B1 */
temp = b0 + b1 + b2;
temp = 0.0625*temp*temp;
*coef_ptr++ = temp;                         /* B0 */
temp = a0*a2;
*coef_ptr++ = temp;                         /* A2 */
*coef_ptr++ = -0.25*a1*(a0 + a2) - temp;    /* A1 */
temp = a0 + a1 + a2;
temp = 0.0625*temp*temp;
*coef_ptr   = temp;                         /* A0 */

data-&gt;Fs = Fs;
data-&gt;type = type;
data-&gt;f0 = f0;
data-&gt;dBgain = dBgain;
data-&gt;Q = Q;

return 0;
}


int clearArray(long N, float *array)
{
register long n;

for (n=0; n&lt;N; n++)
    {
    array[n] = 0.0;
    }

return 0;
}


int plotSectionFrequencyResponse(int Npoints, float Fmin, float Fmax, float *frequencyValues, float *dBvalues, sectionData *data)
{
register int n;
float logFreqStep = __log2(Fmax/Fmin)/(float)(Npoints-1);
register float phi, tau = 1.0/(data-&gt;Fs);
float *coef_ptr = &amp;(data-&gt;graphCoefficients[0]);
register float B2, B1, B0, A2, A1, A0;

B2 = *coef_ptr++;
B1 = *coef_ptr++;
B0 = *coef_ptr++;
A2 = *coef_ptr++;
A1 = *coef_ptr++;
A0 = *coef_ptr;

for (n=0; n&lt;Npoints; n++)
    {
    phi = Fmin*__exp2((float)n*logFreqStep);
    frequencyValues[n] = phi;
    phi = __sin_pi(tau*phi);
    phi = phi*phi;                                                                              /* sin(w/2)^2 */
    dBvalues[n] += DB_LOG2_ENERGY*(__log2((B2*phi + B1)*phi + B0) - __log2((A2*phi + A1)*phi + A0));
    }

return 0;
}


int filterBuffer(long Nsamples, int Nsections, float *input, float *output, float *filterStates, sectionData **sections)
{
register long n;
register int i;
register sectionData **section_ptr;
register float sampleValue, state1, state2, *state_ptr, *coef_ptr;

for (n=0; n&lt;Nsamples; n++)
    {
    section_ptr = sections;                         /* reset section pointer */
    state_ptr = filterStates;                       /* reset state pointer */

    sampleValue = input[n];                         /* get input sample */

        state2 = *state_ptr++;                      /*   x[n-2]            */
        state1 = *state_ptr--;                      /*   x[n-1]            */
        *state_ptr++ = state1;                      /*   x[n-1] -&gt; x[n-2]  */
        *state_ptr++ = sampleValue;                 /*   x[n]   -&gt; x[n-1]  */

    i = Nsections;
    while (--i &gt;= 0)
        {
        coef_ptr = &amp;((*section_ptr++)-&gt;filterCoefficients[0]);  /* point to section filter coefficients */

        sampleValue =  *coef_ptr++ * sampleValue;   /*   b0*x[n]           */
        sampleValue += *coef_ptr++ * state1;        /*   b1*x[n-1]         */
        sampleValue += *coef_ptr++ * state2;        /*   b2*x[n-2]         */

        state2 = *state_ptr++;                      /*   y[n-2]            */
        state1 = *state_ptr--;                      /*   y[n-1]            */
        *state_ptr++ = state1;                      /*   y[n-1] -&gt; y[n-2]  */

        sampleValue -= *coef_ptr++ * state1;        /*  -a1*y[n-1]         */
        sampleValue -= *coef_ptr++ * state2;        /*  -a2*y[n-2]         */

        *state_ptr++ = sampleValue;                 /*   y[n]   -&gt; y[n-1]  */
/*
*      at this point sampleValue is the output of the section just completed 
*     and will be the input to the section about to be computed unless this 
*     is the last section, in which case sampleValue is the input.
*     state1 and state2 are the output states of the section just completed
*     and will be the input states of of the section about to be computed.
*/
        }

    output[n] = sampleValue;                        /* store output sample  */
    }

return 0;
}