moved into qmmp dir

git-svn-id: http://svn.code.sf.net/p/qmmp-dev/code/trunk/qmmp@12 90c681e8-e032-0410-971d-27865f9a5e38

6 files changed, 694 insertions, 0 deletions

diff --git a/lib/equ/iir.c b/lib/equ/iir.c
new file mode 100644
index 000000000..9d826b86c
--- /dev/null
+++ b/lib/equ/iir.c
@@ -0,0 +1,85 @@
+/*
+ *   PCM time-domain equalizer
+ *
+ *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users sourceforge net>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *   $Id: iir.c,v 1.15 2005/10/17 01:57:59 liebremx Exp $
+ */
+
+#include <math.h>
+#include "iir.h"
+
+/* Coefficients */
+sIIRCoefficients *iir_cf;
+
+/* Volume gain 
+ * values should be between 0.0 and 1.0 
+ * Use the preamp from XMMS for now
+ * */
+float preamp[EQ_CHANNELS];
+
+#ifdef BENCHMARK
+#include "benchmark.h"
+double timex = 0.0;
+int count = 0;
+unsigned int blength = 0;
+#endif
+
+/* 
+ * Global vars
+ */
+int rate;
+int band_count;
+
+void set_preamp(int chn, float val)
+{
+  preamp[chn] = val;
+}
+
+/* Init the filters */
+void init_iir()
+{
+  calc_coeffs();
+#if 0
+  band_count = cfg.band_num;
+#endif
+
+  band_count = 10;
+
+  rate = 44100;
+
+  iir_cf = get_coeffs(&band_count, rate);
+  clean_history();
+}
+
+#ifdef ARCH_X86
+/* Round function provided by Frank Klemm which saves around 100K
+ * CPU cycles in my PIII for each call to the IIR function with 4K samples
+ */
+__inline__ int round_trick(float floatvalue_to_round)
+{
+  float   floattmp ;
+  int     rounded_value ;
+
+  floattmp      = (int) 0x00FD8000L + (floatvalue_to_round);
+  rounded_value = *(int*)(&floattmp) - (int)0x4B7D8000L;
+
+  if ( rounded_value != (short) rounded_value )
+    rounded_value = ( rounded_value >> 31 ) ^ 0x7FFF;
+  return rounded_value;
+}
+#endif
diff --git a/lib/equ/iir.h b/lib/equ/iir.h
new file mode 100644
index 000000000..e7ea5ef1a
--- /dev/null
+++ b/lib/equ/iir.h
@@ -0,0 +1,84 @@
+/*
+ *   PCM time-domain equalizer
+ *
+ *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users.sourceforge.net>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *   $Id: iir.h,v 1.12 2005/10/17 01:57:59 liebremx Exp $
+ */
+#ifndef IIR_H
+#define IIR_H
+
+//#include <glib.h>
+//#include "main.h"
+#include "iir_cfs.h"
+
+/*
+ * Flush-to-zero to avoid flooding the CPU with underflow exceptions 
+ */
+#ifdef SSE_MATH
+#define FTZ 0x8000
+#define FTZ_ON { \
+    unsigned int mxcsr; \
+  __asm__  __volatile__ ("stmxcsr %0" : "=m" (*&mxcsr)); \
+  mxcsr |= FTZ; \
+  __asm__  __volatile__ ("ldmxcsr %0" : : "m" (*&mxcsr)); \
+}
+#define FTZ_OFF { \
+    unsigned int mxcsr; \
+  __asm__  __volatile__ ("stmxcsr %0" : "=m" (*&mxcsr)); \
+  mxcsr &= ~FTZ; \
+  __asm__  __volatile__ ("ldmxcsr %0" : : "m" (*&mxcsr)); \
+}
+#else
+#define FTZ_ON
+#define FTZ_OFF
+#endif
+
+/*
+ * Function prototypes
+ */
+void init_iir();
+void clean_history();
+void set_gain(int index, int chn, float val);
+void set_preamp(int chn, float val);
+
+
+ int iir(void * d, int length, int nch);
+
+#ifdef ARCH_X86
+__inline__ int round_trick(float floatvalue_to_round);
+#endif
+#ifdef ARCH_PPC
+__inline__ int round_ppc(float x);
+#endif
+
+#define EQ_CHANNELS 2
+#define EQ_MAX_BANDS 10
+
+extern float preamp[EQ_CHANNELS];
+extern sIIRCoefficients *iir_cf;
+extern int rate;
+extern int band_count;
+
+#ifdef BENCHMARK
+extern double timex;
+extern int count;
+extern unsigned int blength;
+#endif
+
+#endif /* #define IIR_H */
+
diff --git a/lib/equ/iir_cfs.c b/lib/equ/iir_cfs.c
new file mode 100644
index 000000000..f8e6f88a6
--- /dev/null
+++ b/lib/equ/iir_cfs.c
@@ -0,0 +1,237 @@
+/*
+ *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users.sourceforge.net>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *
+ *   Coefficient stuff
+ *
+ *   $Id: iir_cfs.c,v 1.1 2005/10/17 01:57:59 liebremx Exp $
+ */
+
+#include "iir_cfs.h"
+#include <stdio.h>
+#include <math.h>
+
+/*************************** 
+ * IIR filter coefficients *
+ ***************************/ 
+static sIIRCoefficients iir_cf10_11k_11025[10] __attribute__((aligned));
+static sIIRCoefficients iir_cf10_22k_22050[10] __attribute__((aligned));
+static sIIRCoefficients iir_cforiginal10_44100[10] __attribute__((aligned));
+static sIIRCoefficients iir_cforiginal10_48000[10] __attribute__((aligned));
+static sIIRCoefficients iir_cf10_44100[10] __attribute__((aligned));
+static sIIRCoefficients iir_cf10_48000[10] __attribute__((aligned));
+static sIIRCoefficients iir_cf15_44100[15] __attribute__((aligned));
+static sIIRCoefficients iir_cf15_48000[15] __attribute__((aligned));
+static sIIRCoefficients iir_cf25_44100[25] __attribute__((aligned));
+static sIIRCoefficients iir_cf25_48000[25] __attribute__((aligned));
+static sIIRCoefficients iir_cf31_44100[31] __attribute__((aligned));
+static sIIRCoefficients iir_cf31_48000[31] __attribute__((aligned));
+
+/****************************************************************** 
+ * Definitions and data structures to calculate the coefficients
+ ******************************************************************/
+static const double band_f011k[] =
+{ 31, 62, 125, 250, 500, 1000, 2000, 3000, 4000, 5500 
+};
+static const double band_f022k[] =
+{ 31, 62, 125, 250, 500, 1000, 2000, 4000, 8000, 11000 
+};
+static const double band_f010[] =
+{ 31, 62, 125, 250, 500, 1000, 2000, 4000, 8000, 16000 
+};
+static const double band_original_f010[] =
+{ 60, 170, 310, 600, 1000, 3000, 6000, 12000, 14000, 16000 
+};
+static const double band_f015[] =
+{ 25,40,63,100,160,250,400,630,1000,1600,2500,4000,6300,10000,16000
+};
+static const double band_f025[] =
+{ 20,31.5,40,50,80,100,125,160,250,315,400,500,800,
+  1000,1250,1600,2500,3150,4000,5000,8000,10000,12500,16000,20000
+};
+static const double band_f031[] =
+{ 20,25,31.5,40,50,63,80,100,125,160,200,250,315,400,500,630,800,
+  1000,1250,1600,2000,2500,3150,4000,5000,6300,8000,10000,12500,16000,20000
+};
+
+#define GAIN_F0 1.0
+#define GAIN_F1 GAIN_F0 / M_SQRT2
+
+#define SAMPLING_FREQ 44100.0
+#define TETA(f) (2*M_PI*(double)f/bands[n].sfreq)
+#define TWOPOWER(value) (value * value)
+
+#define BETA2(tf0, tf) \
+(TWOPOWER(GAIN_F1)*TWOPOWER(cos(tf0)) \
+ - 2.0 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0) \
+ + TWOPOWER(GAIN_F1) \
+ - TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)))
+#define BETA1(tf0, tf) \
+    (2.0 * TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf)) \
+     + TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf0)) \
+     - 2.0 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0) \
+     - TWOPOWER(GAIN_F1) + TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)))
+#define BETA0(tf0, tf) \
+    (0.25 * TWOPOWER(GAIN_F1) * TWOPOWER(cos(tf0)) \
+     - 0.5 * TWOPOWER(GAIN_F1) * cos(tf) * cos(tf0) \
+     + 0.25 * TWOPOWER(GAIN_F1) \
+     - 0.25 * TWOPOWER(GAIN_F0) * TWOPOWER(sin(tf)))
+
+#define GAMMA(beta, tf0) ((0.5 + beta) * cos(tf0))
+#define ALPHA(beta) ((0.5 - beta)/2.0)
+
+struct {
+    sIIRCoefficients *coeffs;
+    const double *cfs;
+    double octave;
+    int band_count;
+    double sfreq;
+} bands[] = {
+  { iir_cf10_11k_11025,     band_f011k,         1.0,     10, 11025.0 },
+  { iir_cf10_22k_22050,     band_f022k,         1.0,     10, 22050.0 },
+  { iir_cforiginal10_44100, band_original_f010, 1.0,     10, 44100.0 },
+  { iir_cforiginal10_48000, band_original_f010, 1.0,     10, 48000.0 },
+  { iir_cf10_44100,         band_f010,          1.0,     10, 44100.0 },
+  { iir_cf10_48000,         band_f010,          1.0,     10, 48000.0 },
+  { iir_cf15_44100,         band_f015,          2.0/3.0, 15, 44100.0 },
+  { iir_cf15_48000,         band_f015,          2.0/3.0, 15, 48000.0 },
+  { iir_cf25_44100,         band_f025,          1.0/3.0, 25, 44100.0 },
+  { iir_cf25_48000,         band_f025,          1.0/3.0, 25, 48000.0 },
+  { iir_cf31_44100,         band_f031,          1.0/3.0, 31, 44100.0 },
+  { iir_cf31_48000,         band_f031,          1.0/3.0, 31, 48000.0 },
+  { 0, 0, 0, 0, 0 }
+};
+
+/*************
+ * Functions *
+ *************/
+
+/* Get the coeffs for a given number of bands and sampling frequency */
+sIIRCoefficients* get_coeffs(int *bands, int sfreq)
+{
+  sIIRCoefficients *iir_cf = 0;
+  switch(sfreq)
+  {
+    case 11025: iir_cf = iir_cf10_11k_11025;
+                *bands = 10;
+                break;
+    case 22050: iir_cf = iir_cf10_22k_22050;
+                *bands = 10;
+                break;
+    case 48000: 
+                switch(*bands)
+                {
+                  case 31: iir_cf = iir_cf31_48000; break;
+                  case 25: iir_cf = iir_cf25_48000; break;
+                  case 15: iir_cf = iir_cf15_48000; break;
+                  default:
+                           /*iir_cf = use_xmms_original_freqs ? 
+                             iir_cforiginal10_48000 :
+                             iir_cf10_48000;*/
+                             iir_cf = iir_cforiginal10_48000;
+                           break;
+                }
+                break;
+    default:
+                switch(*bands)
+                {
+                  case 31: iir_cf = iir_cf31_44100; break;
+                  case 25: iir_cf = iir_cf25_44100; break;
+                  case 15: iir_cf = iir_cf15_44100; break;
+                  default:
+                           /*iir_cf = use_xmms_original_freqs ? 
+                             iir_cforiginal10_44100 :
+                             iir_cf10_44100;*/
+                             iir_cf = iir_cforiginal10_44100;
+                           break;
+                }
+                break;
+  }
+  return iir_cf;
+}
+
+/* Get the freqs at both sides of F0. These will be cut at -3dB */
+static void find_f1_and_f2(double f0, double octave_percent, double *f1, double *f2)
+{
+    double octave_factor = pow(2.0, octave_percent/2.0);
+    *f1 = f0/octave_factor;
+    *f2 = f0*octave_factor;
+}
+
+/* Find the quadratic root
+ * Always return the smallest root */
+static int find_root(double a, double b, double c, double *x0) {
+  double k = c-((b*b)/(4.*a));
+  double h = -(b/(2.*a));
+  double x1 = 0.;
+  if (-(k/a) < 0.)
+    return -1; 
+  *x0 = h - sqrt(-(k/a));
+  x1 = h + sqrt(-(k/a));
+  if (x1 < *x0)
+    *x0 = x1;
+  return 0;
+}
+
+/* Calculate all the coefficients as specified in the bands[] array */
+void calc_coeffs()
+{
+  int i, n;
+  double f1, f2;
+  double x0;
+
+  n = 0;
+  for (; bands[n].cfs; n++) {
+    double *freqs = (double *)bands[n].cfs;
+    for (i=0; i<bands[n].band_count; i++)
+    {
+
+      /* Find -3dB frequencies for the center freq */
+      find_f1_and_f2(freqs[i], bands[n].octave, &f1, &f2);
+      /* Find Beta */
+      if ( find_root(
+            BETA2(TETA(freqs[i]), TETA(f1)), 
+            BETA1(TETA(freqs[i]), TETA(f1)), 
+            BETA0(TETA(freqs[i]), TETA(f1)), 
+            &x0) == 0)
+      {
+        /* Got a solution, now calculate the rest of the factors */
+        /* Take the smallest root always (find_root returns the smallest one)
+         *
+         * NOTE: The IIR equation is
+         *	y[n] = 2 * (alpha*(x[n]-x[n-2]) + gamma*y[n-1] - beta*y[n-2])
+         *  Now the 2 factor has been distributed in the coefficients
+         */
+        /* Now store the coefficients */
+        bands[n].coeffs[i].beta = 2.0 * x0;
+        bands[n].coeffs[i].alpha = 2.0 * ALPHA(x0);
+        bands[n].coeffs[i].gamma = 2.0 * GAMMA(x0, TETA(freqs[i]));
+#ifdef DEBUG
+        printf("Freq[%d]: %f. Beta: %.10e Alpha: %.10e Gamma %.10e\n",
+            i, freqs[i], bands[n].coeffs[i].beta,
+            bands[n].coeffs[i].alpha, bands[n].coeffs[i].gamma);
+#endif
+      } else {
+        /* Shouldn't happen */
+        bands[n].coeffs[i].beta = 0.;
+        bands[n].coeffs[i].alpha = 0.;
+        bands[n].coeffs[i].gamma = 0.;
+        printf("  **** Where are the roots?\n");
+      }
+    }// for i
+  }//for n
+}
diff --git a/lib/equ/iir_cfs.h b/lib/equ/iir_cfs.h
new file mode 100644
index 000000000..c4cc4a0fd
--- /dev/null
+++ b/lib/equ/iir_cfs.h
@@ -0,0 +1,39 @@
+/*
+ *   PCM time-domain equalizer
+ *
+ *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users.sourceforge.net>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *   $Id: iir_cfs.h,v 1.1 2005/10/17 01:57:59 liebremx Exp $
+ */
+#ifndef IIR_CFS_H
+#define IIR_CFS_H
+
+//#include <glib.h>
+
+/* Coefficients entry */
+typedef struct 
+{
+    float beta;
+    float alpha; 
+    float gamma;
+    float dummy; // Word alignment
+}sIIRCoefficients;
+
+sIIRCoefficients* get_coeffs(int *bands, int sfreq); //, bool use_xmms_original_freqs);
+void calc_coeffs();
+
+#endif
diff --git a/lib/equ/iir_fpu.c b/lib/equ/iir_fpu.c
new file mode 100644
index 000000000..ae0051fdf
--- /dev/null
+++ b/lib/equ/iir_fpu.c
@@ -0,0 +1,210 @@
+/*
+ *   PCM time-domain equalizer
+ *
+ *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users sourceforge net>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *   $Id: iir_fpu.c,v 1.3 2005/11/13 20:02:58 lisanet Exp $
+ */
+
+#include <strings.h>
+#include <stdlib.h>
+//#include <glib.h>
+#include "iir.h"
+#include "iir_fpu.h"
+
+static sXYData data_history[EQ_MAX_BANDS][EQ_CHANNELS] __attribute__((aligned));
+static sXYData data_history2[EQ_MAX_BANDS][EQ_CHANNELS] __attribute__((aligned));
+float gain[EQ_MAX_BANDS][EQ_CHANNELS] __attribute__((aligned));
+/* random noise */
+sample_t dither[256];
+int di;
+
+void set_gain(int index, int chn, float val)
+{
+  gain[index][chn] = val;
+}
+
+void clean_history()
+{
+  int n;
+  /* Zero the history arrays */
+  bzero(data_history, sizeof(sXYData) * EQ_MAX_BANDS * EQ_CHANNELS);
+  bzero(data_history2, sizeof(sXYData) * EQ_MAX_BANDS * EQ_CHANNELS);
+  /* this is only needed if we use fpu code and there's no other place for
+  the moment to init the dither array*/
+  for (n = 0; n < 256; n++) {
+      dither[n] = (rand() % 4) - 2;
+  }
+  di = 0;
+}
+
+__inline__ int iir(void * d, int length, int nch)
+{
+/*  FTZ_ON; */
+  short *data = (short *) d;
+  /* Indexes for the history arrays
+   * These have to be kept between calls to this function
+   * hence they are static */
+  static int i = 2, j = 1, k = 0;	
+
+  int index, band, channel;
+  int tempgint, halflength;
+  sample_t out[EQ_CHANNELS], pcm[EQ_CHANNELS];
+
+#if 0
+  /* Load the correct filter table according to the sampling rate if needed */
+  if (srate != rate)
+  {
+    band_count = eqcfg.band_num;
+    rate = srate;
+    iir_cf = get_coeffs(&band_count, rate, eqcfg.use_xmms_original_freqs);
+    clean_history();
+  }
+#endif
+
+#ifdef BENCHMARK
+  start_counter();
+#endif /* BENCHMARK */
+
+  /**
+   * IIR filter equation is
+   * y[n] = 2 * (alpha*(x[n]-x[n-2]) + gamma*y[n-1] - beta*y[n-2])
+   *
+   * NOTE: The 2 factor was introduced in the coefficients to save
+   * 			a multiplication
+   *
+   * This algorithm cascades two filters to get nice filtering
+   * at the expense of extra CPU cycles
+   */
+  /* 16bit, 2 bytes per sample, so divide by two the length of
+   * the buffer (length is in bytes)
+   */
+  halflength = (length >> 1);
+  for (index = 0; index < halflength; index+=nch)
+  {
+    /* For each channel */
+    for (channel = 0; channel < nch; channel++)
+    {
+      pcm[channel] = data[index+channel] * 4;
+      /* Preamp gain */
+      pcm[channel] *= (preamp[channel] / 2);
+      
+      /* add random noise */
+      pcm[channel] += dither[di];
+
+      out[channel] = 0.0;
+      /* For each band */
+      for (band = 0; band < band_count; band++)
+      {
+        /* Store Xi(n) */
+        data_history[band][channel].x[i] = pcm[channel];
+        /* Calculate and store Yi(n) */
+        data_history[band][channel].y[i] = 
+          (
+           /* 		= alpha * [x(n)-x(n-2)] */
+           iir_cf[band].alpha * ( data_history[band][channel].x[i]
+             -  data_history[band][channel].x[k])
+           /* 		+ gamma * y(n-1) */
+           + iir_cf[band].gamma * data_history[band][channel].y[j]
+           /* 		- beta * y(n-2) */
+           - iir_cf[band].beta * data_history[band][channel].y[k]
+          );
+        /* 
+         * The multiplication by 2.0 was 'moved' into the coefficients to save
+         * CPU cycles here */
+        /* Apply the gain  */
+        out[channel] += data_history[band][channel].y[i]*gain[band][channel]; /* * 2.0; */
+      } /* For each band */
+
+      //if (cfg.eq_extra_filtering)
+      {
+        /* Filter the sample again */
+        for (band = 0; band < band_count; band++)
+        {
+          /* Store Xi(n) */
+          data_history2[band][channel].x[i] = out[channel];
+          /* Calculate and store Yi(n) */
+          data_history2[band][channel].y[i] = 
+            (
+             /* y(n) = alpha * [x(n)-x(n-2)] */
+             iir_cf[band].alpha * (data_history2[band][channel].x[i]
+               -  data_history2[band][channel].x[k])
+             /* 	    + gamma * y(n-1) */
+             + iir_cf[band].gamma * data_history2[band][channel].y[j]
+             /* 		- beta * y(n-2) */
+             - iir_cf[band].beta * data_history2[band][channel].y[k]
+            );
+          /* Apply the gain */
+          out[channel] +=  data_history2[band][channel].y[i]*gain[band][channel];
+        } /* For each band */
+      }
+
+      /* Volume stuff
+         Scale down original PCM sample and add it to the filters 
+         output. This substitutes the multiplication by 0.25
+         Go back to use the floating point multiplication before the
+         conversion to give more dynamic range
+         */
+      out[channel] += pcm[channel]*0.25;
+      
+      /* remove random noise */
+      out[channel] -= dither[di]*0.25;
+
+      /* Round and convert to integer */
+#ifdef ARCH_PPC
+      tempgint = round_ppc(out[channel]);
+#else
+#ifdef ARCH_X86
+      tempgint = round_trick(out[channel]);
+#else
+      tempgint = (int)out[channel];
+#endif
+#endif
+
+      /* Limit the output */
+      if (tempgint < -32768)
+        data[index+channel] = -32768;
+      else if (tempgint > 32767)
+        data[index+channel] = 32767;
+      else 
+        data[index+channel] = tempgint;
+    } /* For each channel */
+    
+    /* Wrap around the indexes */
+    i = (i+1)%3;
+    j = (j+1)%3;
+    k = (k+1)%3;
+    /* random noise index */
+    di = (di + 1) % 256;
+
+  }/* For each pair of samples */
+
+#ifdef BENCHMARK
+  timex += get_counter();
+  blength += length;
+  if (count++ == 1024)
+  {
+    printf("FLOATING POINT: %f %d\n",timex/1024.0, blength/1024);
+    blength = 0;
+    timex = 0.;
+    count = 0;
+  }
+#endif /* BENCHMARK */
+
+/*  FTZ_OFF; */
+  return length;
+}
diff --git a/lib/equ/iir_fpu.h b/lib/equ/iir_fpu.h
new file mode 100644
index 000000000..990eebf97
--- /dev/null
+++ b/lib/equ/iir_fpu.h
@@ -0,0 +1,39 @@
+/*
+ *   PCM time-domain equalizer
+ *
+ *   Copyright (C) 2002-2005  Felipe Rivera <liebremx at users.sourceforge.net>
+ *
+ *   This program is free software; you can redistribute it and/or modify
+ *   it under the terms of the GNU General Public License as published by
+ *   the Free Software Foundation; either version 2 of the License, or
+ *   (at your option) any later version.
+ *
+ *   This program is distributed in the hope that it will be useful,
+ *   but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *   GNU General Public License for more details.
+ *
+ *   You should have received a copy of the GNU General Public License
+ *   along with this program; if not, write to the Free Software
+ *   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
+ *
+ *   $Id: iir_fpu.h,v 1.2 2005/11/01 15:59:20 lisanet Exp $$
+ */
+#ifndef IIR_FPU_H
+#define IIR_FPU_H
+
+#define sample_t double
+
+/*
+ * Normal FPU implementation data structures
+ */
+/* Coefficient history for the IIR filter */
+typedef struct
+{
+    sample_t x[3]; /* x[n], x[n-1], x[n-2] */
+    sample_t y[3]; /* y[n], y[n-1], y[n-2] */
+    sample_t dummy1; // Word alignment
+    sample_t dummy2;
+}sXYData;
+
+#endif