Adding equalizer plugin

git-svn-id: svn://svn.mplayerhq.hu/mplayer/trunk@6431 b3059339-0415-0410-9bf9-f77b7e298cf2

4 files changed, 245 insertions, 3 deletions

diff --git a/libao2/Makefile b/libao2/Makefile
index 118f455b54..d30aeb1fe1 100644
--- a/libao2/Makefile
+++ b/libao2/Makefile
@@ -4,7 +4,7 @@ include config.mak
 LIBNAME = libao2.a
 
 # TODO: moveout ao_sdl.c so it's only used when SDL is detected
-SRCS=afmt.c audio_out.c ao_mpegpes.c ao_null.c ao_pcm.c ao_plugin.c pl_delay.c pl_format.c pl_surround.c remez.c pl_resample.c pl_volume.c pl_extrastereo.c pl_volnorm.c $(OPTIONAL_SRCS)
+SRCS=afmt.c audio_out.c ao_mpegpes.c ao_null.c ao_pcm.c ao_plugin.c pl_delay.c pl_format.c pl_surround.c remez.c pl_resample.c pl_volume.c pl_extrastereo.c pl_volnorm.c pl_eq.c $(OPTIONAL_SRCS)
 
 OBJS=$(SRCS:.c=.o)
 
diff --git a/libao2/audio_plugin.h b/libao2/audio_plugin.h
index 2ff06591b5..084be9cc50 100644
--- a/libao2/audio_plugin.h
+++ b/libao2/audio_plugin.h
@@ -56,7 +56,7 @@ extern ao_plugin_cfg_t ao_plugin_cfg;
 // This block should not be available in the pl_xxxx files
 // due to compilation issues
 #ifndef PLUGIN
-#define NPL 7+1 // Number of PLugins ( +1 list ends with NULL )
+#define NPL 8+1 // Number of PLugins ( +1 list ends with NULL )
 // List of plugins 
 extern ao_plugin_functions_t audio_plugin_delay;
 extern ao_plugin_functions_t audio_plugin_format; 
@@ -65,7 +65,7 @@ extern ao_plugin_functions_t audio_plugin_resample;
 extern ao_plugin_functions_t audio_plugin_volume;
 extern ao_plugin_functions_t audio_plugin_extrastereo;
 extern ao_plugin_functions_t audio_plugin_volnorm;
-
+extern ao_plugin_functions_t audio_plugin_eq;
 
 #define AO_PLUGINS { \
    &audio_plugin_delay, \
@@ -75,6 +75,7 @@ extern ao_plugin_functions_t audio_plugin_volnorm;
    &audio_plugin_volume, \
    &audio_plugin_extrastereo, \
    &audio_plugin_volnorm, \
+   &audio_plugin_eq, \
    NULL \
 }
 #endif /* PLUGIN */
diff --git a/libao2/eq.h b/libao2/eq.h
new file mode 100644
index 0000000000..dab6e96d65
--- /dev/null
+++ b/libao2/eq.h
@@ -0,0 +1,36 @@
+/*=============================================================================
+//	
+//  This software has been released under the terms of the GNU Public
+//  license. See http://www.gnu.org/copyleft/gpl.html for details.
+//
+//  Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
+//
+//=============================================================================
+*/
+
+/* Equalizer plugin header file defines struct used for setting or
+   getting the gain of a specific channel and frequency */
+
+typedef struct equalizer_s
+{
+  float gain;   	// Gain in db  -15 - 15 
+  int	channel; 	// Channel number 0 - 5 
+  int 	band;		// Frequency band 0 - 9
+}equalizer_t;
+
+/* The different frequency bands are:
+nr.    	center frequency
+0  	31.25 Hz
+1 	62.50 Hz
+2	125.0 Hz
+3	250.0 Hz
+4	500.0 Hz
+5	1.000 kHz
+6	2.000 kHz
+7	4.000 kHz
+8	8.000 kHz
+9       16.00 kHz
+*/
+
+#define AOCONTROL_PLUGIN_EQ_SET_GAIN 2  // Use this to set the gain
+#define AOCONTROL_PLUGIN_EQ_GET_GAIN 3  // Use this to get the gain
diff --git a/libao2/pl_eq.c b/libao2/pl_eq.c
new file mode 100644
index 0000000000..11874520ce
--- /dev/null
+++ b/libao2/pl_eq.c
@@ -0,0 +1,205 @@
+/*=============================================================================
+//	
+//  This software has been released under the terms of the GNU Public
+//  license. See http://www.gnu.org/copyleft/gpl.html for details.
+//
+//  Copyright 2001 Anders Johansson ajh@atri.curtin.edu.au
+//
+//=============================================================================
+*/
+
+/* Equalizer plugin, implementation of a 10 band time domain graphic
+   equalizer using IIR filters. The IIR filters are implemented using a
+   Direct Form II approach. But has been modified (b1 == 0 always) to
+   save computation.
+*/
+#define PLUGIN
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <inttypes.h>
+#include <math.h>
+
+#include "audio_out.h"
+#include "audio_plugin.h"
+#include "audio_plugin_internal.h"
+#include "afmt.h"
+#include "eq.h"
+
+static ao_info_t info =
+{
+        "Equalizer audio plugin",
+        "eq",
+        "Anders",
+        ""
+};
+
+LIBAO_PLUGIN_EXTERN(eq)
+
+
+#define CH  6     // Max number of channels
+#define L   2     // Storage for filter taps
+#define KM  10    // Max number of octaves
+
+#define Q   1.2247 /* Q value for band-pass filters 1.2247=(3/2)^(1/2)
+		      gives 4db suppression @ Fc*2 and Fc/2 */
+
+// Center frequencies for band-pass filters
+#define CF  {31.25,62.5,125,250,500,1000,2000,4000,8000,16000}
+
+// local data
+typedef struct pl_eq_s
+{
+  int16_t   a[KM][L];        // A weights
+  int16_t   b[KM][L];        // B weights
+  int16_t   wq[CH][KM][L];   // Circular buffer for W data
+  int16_t   g[CH][KM];       // Gain factor for each channel and band
+  int16_t   K; 		     // Number of used eq bands
+  int       channels;        // Number of channels
+} pl_eq_t;
+
+static pl_eq_t pl_eq;
+
+// to set/get/query special features/parameters
+static int control(int cmd,int arg){
+  switch(cmd){
+  case AOCONTROL_PLUGIN_SET_LEN:
+    return CONTROL_OK;
+  case AOCONTROL_PLUGIN_EQ_SET_GAIN:{
+    float gain = ((equalizer_t*)arg)->gain;
+    int ch     =((equalizer_t*)arg)->channel;
+    int band   =((equalizer_t*)arg)->band;
+    if(ch > CH || ch < 0 || band > KM || band < 0)
+      return CONTROL_ERROR;
+    
+    pl_eq.g[ch][band]=(int16_t) 4096 * (pow(10.0,gain/20.0)-1.0);
+    return CONTROL_OK;
+  }
+  case AOCONTROL_PLUGIN_EQ_GET_GAIN:{
+    int ch     =((equalizer_t*)arg)->channel;
+    int band   =((equalizer_t*)arg)->band;
+    if(ch > CH || ch < 0 || band > KM || band < 0)
+      return CONTROL_ERROR;
+    
+    ((equalizer_t*)arg)->gain = log10((float)pl_eq.g[ch][band]/4096.0+1) * 20.0;
+    return CONTROL_OK;
+  }
+  }
+  return CONTROL_UNKNOWN;
+}
+
+// 2nd order Band-pass Filter design
+void bp2(int16_t* a, int16_t* b, float fc, float q){
+  double th=2*3.141592654*fc;
+  double C=(1 - tan(th*q/2))/(1 + tan(th*q/2));
+  
+  a[0] = (int16_t)lround( 16383.0 * (1 + C) * cos(th));
+  a[1] = (int16_t)lround(-16383.0 * C);
+  
+  b[0] = (int16_t)lround(-16383.0 * (C - 1)/2);
+  b[1] = (int16_t)lround(-16383.0 * 1.0050);
+}
+
+// empty buffers
+static void reset(){
+  int k,l,c;
+  for(c=0;c<pl_eq.channels;c++)
+    for(k=0;k<pl_eq.K;k++)
+      for(l=0;l<L*2;l++)
+	pl_eq.wq[c][k][l]=0;
+}
+
+// open & setup audio device
+// return: 1=success 0=fail
+static int init(){
+  int   c,k   = 0;
+  float F[KM] = CF;
+  
+  // Check input format
+  if(ao_plugin_data.format != AFMT_S16_LE){
+    fprintf(stderr,"[pl_eq] Input audio format not yet supported. \n");
+    return 0;
+  }
+
+  // Check number of channels
+  if(ao_plugin_data.channels>CH){
+     fprintf(stderr,"[pl_eq] Too many channels, max is 6.\n");
+    return 0;
+  }
+  pl_eq.channels=ao_plugin_data.channels;
+
+  // Calculate number of active filters
+  pl_eq.K=KM;
+  while(F[pl_eq.K-1] > (float)ao_plugin_data.rate/2)
+    pl_eq.K--;
+
+  // Generate filter taps
+  for(k=0;k<pl_eq.K;k++)
+    bp2(pl_eq.a[k],pl_eq.b[k],F[k]/((float)ao_plugin_data.rate),Q);
+
+  // Reset buffers
+  reset();
+
+  // Reset gain factors
+  for(c=0;c<pl_eq.channels;c++)
+    for(k=0;k<pl_eq.K;k++)
+      pl_eq.g[c][k]=0;
+
+  // Tell ao_plugin how much this plugin adds to the overall time delay
+  ao_plugin_data.delay_fix-=2/((float)pl_eq.channels*(float)ao_plugin_data.rate);
+  // Print some cool remark of what the plugin does
+  printf("[pl_eq] Equalizer in use.\n");
+  return 1;
+}
+
+// close plugin
+static void uninit(){
+}
+
+// processes 'ao_plugin_data.len' bytes of 'data'
+// called for every block of data
+static int play(){
+  uint16_t  	ci    	= pl_eq.channels; 	// Index for channels
+  uint16_t	nch   	= pl_eq.channels;   	// Number of channels
+
+  while(ci--){
+    int16_t*	g	= pl_eq.g[ci]; 	// Gain factor 
+    int16_t*	in    	= ((int16_t*)ao_plugin_data.data)+ci;
+    int16_t*	out   	= ((int16_t*)ao_plugin_data.data)+ci;
+    int16_t* 	end   	= in+ao_plugin_data.len/2; // Block loop end
+
+    while(in < end){
+      register int16_t k   = 0;	   	// Frequency band index
+      register int32_t yt  = 0;    	// Total output from filters
+      register int16_t x   = *in; 	/* Current input sample scale
+					   to prevent overflow in wq */
+      in+=nch;
+
+      // Run the filters 
+      for(;k<pl_eq.K;k++){
+	// Pointer to circular buffer wq
+	register int16_t* wq = pl_eq.wq[ci][k];
+	// Calculate output from AR part of current filter
+	register int32_t xt = (x*pl_eq.b[k][0]) >> 4;
+	register int32_t w = xt + wq[0]*pl_eq.a[k][0] + wq[1]*pl_eq.a[k][1];
+	// Calculate output form MA part of current filter
+	yt+=(((w + wq[1]*pl_eq.b[k][1]) >> 10)*g[k]) >> 12;
+	// Update circular buffer
+	wq[1] = wq[0]; wq[0] = w >> 14;
+      }	
+
+      // Calculate output 
+      *out=(int16_t)(yt+x);
+      out+=nch;
+    }
+  }
+  return 1;
+}
+
+
+
+
+
+
+