Adding sub-woofer filter, use this filter to add a sub channel to the audio stream

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

6 files changed, 374 insertions, 2 deletions

diff --git a/libaf/Makefile b/libaf/Makefile
index 79ee144c76..2f716e22c2 100644
--- a/libaf/Makefile
+++ b/libaf/Makefile
@@ -2,7 +2,7 @@ include ../config.mak
 
 LIBNAME = libaf.a
 
-SRCS=af.c af_mp.c af_dummy.c af_delay.c af_channels.c af_format.c af_resample.c window.c filter.c af_volume.c af_equalizer.c af_tools.c af_comp.c af_gate.c af_pan.c af_surround.c
+SRCS=af.c af_mp.c af_dummy.c af_delay.c af_channels.c af_format.c af_resample.c window.c filter.c af_volume.c af_equalizer.c af_tools.c af_comp.c af_gate.c af_pan.c af_surround.c af_sub.c
 
 OBJS=$(SRCS:.c=.o)
 
diff --git a/libaf/af.c b/libaf/af.c
index f2bded6cb5..ca2dc7a75f 100644
--- a/libaf/af.c
+++ b/libaf/af.c
@@ -20,6 +20,7 @@ extern af_info_t af_info_gate;
 extern af_info_t af_info_comp;
 extern af_info_t af_info_pan;
 extern af_info_t af_info_surround;
+extern af_info_t af_info_sub;
 
 static af_info_t* filter_list[]={ \
    &af_info_dummy,\
@@ -33,6 +34,7 @@ static af_info_t* filter_list[]={ \
    &af_info_comp,\
    &af_info_pan,\
    &af_info_surround,\
+   &af_info_sub,\
    NULL \
 };
 
diff --git a/libaf/af_sub.c b/libaf/af_sub.c
new file mode 100644
index 0000000000..f64c68c532
--- /dev/null
+++ b/libaf/af_sub.c
@@ -0,0 +1,181 @@
+/*=============================================================================
+//	
+//  This software has been released under the terms of the GNU Public
+//  license. See http://www.gnu.org/copyleft/gpl.html for details.
+//
+//  Copyright 2002 Anders Johansson ajh@watri.uwa.edu.au
+//
+//=============================================================================
+*/
+
+/* This filter adds a sub-woofer channels to the audio stream by
+   averaging the left and right channel and low-pass filter them. The
+   low-pass filter is implemented as a 4th order IIR Butterworth
+   filter, with a variable cutoff frequency between 10 and 300 Hz. The
+   filter gives 24dB/octave attenuation. There are two runtime
+   controls one for setting which channel to insert the sub-audio into
+   called AF_CONTROL_SUB_CH and one for setting the cutoff frequency
+   called AF_CONTROL_SUB_FC.
+*/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h> 
+
+#include "af.h"
+#include "dsp.h"
+
+// Q value for low-pass filter
+#define Q 1.0
+
+// Analog domain biquad section 
+typedef struct{
+  float a[3];		// Numerator coefficients
+  float b[3];		// Denominator coefficients
+} biquad_t;
+
+// S-parameters for designing 4th order Butterworth filter
+static biquad_t sp[2] = {{{1.0,0.0,0.0},{1.0,0.765367,1.0}},
+			 {{1.0,0.0,0.0},{1.0,1.847759,1.0}}};
+
+// Data for specific instances of this filter
+typedef struct af_sub_s
+{
+  float w[2][4];	// Filter taps for low-pass filter
+  float q[2][2];	// Circular queues
+  float	fc;		// Cutoff frequency [Hz] for low-pass filter
+  float k;		// Filter gain;
+  int ch;		// Channel number which to insert the filtered data
+  
+}af_sub_t;
+
+// Initialization and runtime control
+static int control(struct af_instance_s* af, int cmd, void* arg)
+{
+  af_sub_t* s   = af->setup; 
+
+  switch(cmd){
+  case AF_CONTROL_REINIT:{
+    // Sanity check
+    if(!arg) return AF_ERROR;
+
+    af->data->rate   = ((af_data_t*)arg)->rate;
+    af->data->nch    = max(s->ch+1,((af_data_t*)arg)->nch);
+    af->data->format = AF_FORMAT_F | AF_FORMAT_NE;
+    af->data->bps    = 4;
+
+    // Design low-pass filter
+    s->k = 1.0;
+    if((-1 == szxform(sp[0].a, sp[0].b, Q, s->fc,
+       (float)af->data->rate, &s->k, s->w[0])) ||
+       (-1 == szxform(sp[1].a, sp[1].b, Q, s->fc,
+       (float)af->data->rate, &s->k, s->w[1])))
+      return AF_ERROR;
+    return af_test_output(af,(af_data_t*)arg);
+  }
+  case AF_CONTROL_COMMAND_LINE:{
+    int   ch=5;
+    float fc=60.0;
+    sscanf(arg,"%f:%i", &fc , &ch);
+    if(AF_OK != control(af,AF_CONTROL_SUB_CH | AF_CONTROL_SET, &ch))
+      return AF_ERROR;
+    return control(af,AF_CONTROL_SUB_FC | AF_CONTROL_SET, &fc);
+  }
+  case AF_CONTROL_SUB_CH | AF_CONTROL_SET: // Requires reinit
+    // Sanity check
+    if((*(int*)arg >= AF_NCH) || (*(int*)arg < 0)){
+      af_msg(AF_MSG_ERROR,"[sub] Subwoofer channel number must be between "
+	     " 0 and %i current value is %i\n", AF_NCH-1, *(int*)arg);
+      return AF_ERROR;
+    }
+    s->ch = *(int*)arg;
+    return AF_OK;
+  case AF_CONTROL_SUB_CH | AF_CONTROL_GET:
+    *(int*)arg = s->ch;
+    return AF_OK;
+  case AF_CONTROL_SUB_FC | AF_CONTROL_SET: // Requires reinit
+    // Sanity check
+    if((*(float*)arg > 300) || (*(float*)arg < 20)){
+      af_msg(AF_MSG_ERROR,"[sub] Cutoff frequency must be between 20Hz and"
+	     " 300Hz current value is %0.2f",*(float*)arg);
+      return AF_ERROR;
+    }
+    // Set cutoff frequency
+    s->fc = *(float*)arg;
+    return AF_OK;
+  case AF_CONTROL_SUB_FC | AF_CONTROL_GET:
+    *(float*)arg = s->fc;
+    return AF_OK;
+  }
+  return AF_UNKNOWN;
+}
+
+// Deallocate memory 
+static void uninit(struct af_instance_s* af)
+{
+  if(af->data)
+    free(af->data);
+  if(af->setup)
+    free(af->setup);
+}
+
+#ifndef IIR
+#define IIR(in,w,q,out) { \
+  float h0 = (q)[0]; \
+  float h1 = (q)[1]; \
+  float hn = (in) - h0 * (w)[0] - h1 * (w)[1];  \
+  out = hn + h0 * (w)[2] + h1 * (w)[3];	 \
+  (q)[1] = h0; \
+  (q)[0] = hn; \
+}
+#endif
+
+// Filter data through filter
+static af_data_t* play(struct af_instance_s* af, af_data_t* data)
+{
+  af_data_t*    c   = data;	 // Current working data
+  af_sub_t*  	s   = af->setup; // Setup for this instance
+  float*   	a   = c->audio;	 // Audio data
+  int		len = c->len/4;	 // Number of samples in current audio block 
+  int		nch = c->nch;	 // Number of channels
+  int		ch  = s->ch;	 // Channel in which to insert the sub audio
+  register int  i;
+
+  // Run filter
+  for(i=0;i<len;i+=nch){
+    // Average left and right
+    register float x = 0.5 * (a[i] + a[i+1]);
+    IIR(x * s->k, s->w[0], s->q[0], x);
+    IIR(x , s->w[1], s->q[1], a[i+ch]);
+  }
+
+  return c;
+}
+
+// Allocate memory and set function pointers
+static int open(af_instance_t* af){
+  af_sub_t* s;
+  af->control=control;
+  af->uninit=uninit;
+  af->play=play;
+  af->mul.n=1;
+  af->mul.d=1;
+  af->data=calloc(1,sizeof(af_data_t));
+  af->setup=s=calloc(1,sizeof(af_sub_t));
+  if(af->data == NULL || af->setup == NULL)
+    return AF_ERROR;
+  // Set default values
+  s->ch = 5;  	 // Channel nr 6
+  s->fc = 60; 	 // Cutoff frequency 60Hz
+  return AF_OK;
+}
+
+// Description of this filter
+af_info_t af_info_sub = {
+    "Audio filter for adding a sub-base channel",
+    "sub",
+    "Anders",
+    "",
+    AF_FLAGS_NOT_REENTRANT,
+    open
+};
diff --git a/libaf/control.h b/libaf/control.h
index c5468ad256..cc7a11486b 100644
--- a/libaf/control.h
+++ b/libaf/control.h
@@ -202,8 +202,17 @@ typedef struct af_control_ext_s{
 #define AF_CONTROL_EQUALIZER_GAIN 	0x00001C00 | AF_CONTROL_FILTER_SPECIFIC
 
 
-// Set delay length in seconds
+// Delay length in ms, arg is a control_ext with a float*
 #define AF_CONTROL_DELAY_LEN		0x00001D00 | AF_CONTROL_FILTER_SPECIFIC
 
 
+// Subwoofer
+
+// Channel number which to insert the filtered data, arg in int*
+#define AF_CONTROL_SUB_CH		0x00001E00 | AF_CONTROL_FILTER_SPECIFIC
+
+// Cutoff frequency [Hz] for lowpass filter, arg is float*
+#define AF_CONTROL_SUB_FC		0x00001F00 | AF_CONTROL_FILTER_SPECIFIC
+
+
 #endif /*__af_control_h */
diff --git a/libaf/filter.c b/libaf/filter.c
index 8d677f1e6d..526b00b244 100644
--- a/libaf/filter.c
+++ b/libaf/filter.c
@@ -14,6 +14,10 @@
 #include <math.h>
 #include "dsp.h"
 
+/******************************************************************************
+*  FIR filter implementations
+******************************************************************************/
+
 /* C implementation of FIR filter y=w*x
 
    n number of filter taps, where mod(n,4)==0
@@ -73,6 +77,9 @@ inline int updatepq(unsigned int n, unsigned int d, unsigned int xi, _ftype_t**
   return (++xi)&(n-1);
 }
 
+/******************************************************************************
+*  FIR filter design
+******************************************************************************/
 
 /* Design FIR filter using the Window method
 
@@ -255,3 +262,172 @@ int design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _fty
   }
   return -1;
 }
+
+/******************************************************************************
+*  IIR filter design
+******************************************************************************/
+
+/* Helper functions for the bilinear transform */
+
+/* Pre-warp the coefficients of a numerator or denominator.
+   Note that a0 is assumed to be 1, so there is no wrapping
+   of it.  
+*/
+void prewarp(_ftype_t* a, _ftype_t fc, _ftype_t fs)
+{
+  _ftype_t wp;
+  wp = 2.0 * fs * tan(M_PI * fc / fs);
+  a[2] = a[2]/(wp * wp);
+  a[1] = a[1]/wp;
+}
+
+/* Transform the numerator and denominator coefficients of s-domain
+   biquad section into corresponding z-domain coefficients.
+   
+   The transfer function for z-domain is:
+
+          1 + alpha1 * z^(-1) + alpha2 * z^(-2)
+   H(z) = -------------------------------------
+          1 + beta1 * z^(-1) + beta2 * z^(-2)
+
+   Store the 4 IIR coefficients in array pointed by coef in following
+   order:
+   beta1, beta2    (denominator)
+   alpha1, alpha2  (numerator)
+   
+   Arguments:
+   a       - s-domain numerator coefficients
+   b       - s-domain denominator coefficients
+   k 	   - filter gain factor. Initially set to 1 and modified by each
+             biquad section in such a way, as to make it the
+             coefficient by which to multiply the overall filter gain
+             in order to achieve a desired overall filter gain,
+             specified in initial value of k.  
+   fs 	   - sampling rate (Hz)
+   coef    - array of z-domain coefficients to be filled in.
+ 
+   Return: On return, set coef z-domain coefficients and k to the gain
+   required to maintain overall gain = 1.0;
+*/
+void bilinear(_ftype_t* a, _ftype_t* b, _ftype_t* k, _ftype_t fs, _ftype_t *coef)
+{
+  _ftype_t ad, bd;
+
+  /* alpha (Numerator in s-domain) */
+  ad = 4. * a[2] * fs * fs + 2. * a[1] * fs + a[0];
+  /* beta (Denominator in s-domain) */
+  bd = 4. * b[2] * fs * fs + 2. * b[1] * fs + b[0];
+
+  /* Update gain constant for this section */
+  *k *= ad/bd;
+
+  /* Denominator */
+  *coef++ = (2. * b[0] - 8. * b[2] * fs * fs)/bd; /* beta1 */
+  *coef++ = (4. * b[2] * fs * fs - 2. * b[1] * fs + b[0])/bd; /* beta2 */
+
+  /* Numerator */
+  *coef++ = (2. * a[0] - 8. * a[2] * fs * fs)/ad; /* alpha1 */
+  *coef   = (4. * a[2] * fs * fs - 2. * a[1] * fs + a[0])/ad;   /* alpha2 */
+}
+
+
+
+/* IIR filter design using bilinear transform and prewarp. Transforms
+   2nd order s domain analog filter into a digital IIR biquad link. To
+   create a filter fill in a, b, Q and fs and make space for coef and k.
+   
+
+   Example Butterworth design: 
+
+   Below are Butterworth polynomials, arranged as a series of 2nd
+   order sections:
+
+   Note: n is filter order.
+   
+   n  Polynomials
+   -------------------------------------------------------------------
+   2  s^2 + 1.4142s + 1
+   4  (s^2 + 0.765367s + 1) * (s^2 + 1.847759s + 1)
+   6  (s^2 + 0.5176387s + 1) * (s^2 + 1.414214 + 1) * (s^2 + 1.931852s + 1)
+   
+   For n=4 we have following equation for the filter transfer function:
+                       1                              1
+   T(s) = --------------------------- * ----------------------------
+          s^2 + (1/Q) * 0.765367s + 1   s^2 + (1/Q) * 1.847759s + 1
+   
+   The filter consists of two 2nd order sections since highest s power
+   is 2.  Now we can take the coefficients, or the numbers by which s
+   is multiplied and plug them into a standard formula to be used by
+   bilinear transform.
+
+   Our standard form for each 2nd order section is:
+
+          a2 * s^2 + a1 * s + a0
+   H(s) = ----------------------
+          b2 * s^2 + b1 * s + b0
+
+   Note that Butterworth numerator is 1 for all filter sections, which
+   means s^2 = 0 and s^1 = 0
+
+   Lets convert standard Butterworth polynomials into this form:
+
+             0 + 0 + 1                  0 + 0 + 1
+   --------------------------- * --------------------------
+   1 + ((1/Q) * 0.765367) + 1   1 + ((1/Q) * 1.847759) + 1
+
+   Section 1:
+   a2 = 0; a1 = 0; a0 = 1;
+   b2 = 1; b1 = 0.765367; b0 = 1;
+
+   Section 2:
+   a2 = 0; a1 = 0; a0 = 1;
+   b2 = 1; b1 = 1.847759; b0 = 1;
+
+   Q is filter quality factor or resonance, in the range of 1 to
+   1000. The overall filter Q is a product of all 2nd order stages.
+   For example, the 6th order filter (3 stages, or biquads) with
+   individual Q of 2 will have filter Q = 2 * 2 * 2 = 8.
+
+
+   Arguments:
+   a       - s-domain numerator coefficients, a[1] is always assumed to be 1.0
+   b       - s-domain denominator coefficients
+   Q	   - Q value for the filter
+   k 	   - filter gain factor. Initially set to 1 and modified by each
+             biquad section in such a way, as to make it the
+             coefficient by which to multiply the overall filter gain
+             in order to achieve a desired overall filter gain,
+             specified in initial value of k.  
+   fs 	   - sampling rate (Hz)
+   coef    - array of z-domain coefficients to be filled in.
+
+   Note: Upon return from each call, the k argument will be set to a
+   value, by which to multiply our actual signal in order for the gain
+   to be one. On second call to szxform() we provide k that was
+   changed by the previous section. During actual audio filtering
+   k can be used for gain compensation.
+
+   return -1 if fail 0 if success.
+*/
+int szxform(_ftype_t* a, _ftype_t* b, _ftype_t Q, _ftype_t fc, _ftype_t fs, _ftype_t *k, _ftype_t *coef)
+{
+  _ftype_t at[3];
+  _ftype_t bt[3];
+
+  if(!a || !b || !k || !coef || (Q>1000.0 || Q< 1.0)) 
+    return -1;
+
+  memcpy(at,a,3*sizeof(_ftype_t));
+  memcpy(bt,b,3*sizeof(_ftype_t));
+
+  bt[1]/=Q;
+
+  /* Calculate a and b and overwrite the original values */
+  prewarp(at, fc, fs);
+  prewarp(bt, fc, fs);
+  /* Execute bilinear transform */
+  bilinear(at, bt, k, fs, coef);
+
+  return 0;
+}
+
diff --git a/libaf/filter.h b/libaf/filter.h
index f36bc0bf2f..307760d856 100644
--- a/libaf/filter.h
+++ b/libaf/filter.h
@@ -45,14 +45,18 @@
 
 // Exported functions
 extern _ftype_t fir(unsigned int n, _ftype_t* w, _ftype_t* x);
+
 extern _ftype_t* pfir(unsigned int n, unsigned int k, unsigned int xi, _ftype_t** w, _ftype_t** x, _ftype_t* y, unsigned int s);
 
 extern int updateq(unsigned int n, unsigned int xi, _ftype_t* xq, _ftype_t* in);
 extern int updatepq(unsigned int n, unsigned int k, unsigned int xi, _ftype_t** xq, _ftype_t* in, unsigned int s);
 
 extern int design_fir(unsigned int n, _ftype_t* w, _ftype_t* fc, unsigned int flags, _ftype_t opt);
+
 extern int design_pfir(unsigned int n, unsigned int k, _ftype_t* w, _ftype_t** pw, _ftype_t g, unsigned int flags);
 
+extern int szxform(_ftype_t* a, _ftype_t* b, _ftype_t Q, _ftype_t fc, _ftype_t fs, _ftype_t *k, _ftype_t *coef);
+
 /* Add new data to circular queue designed to be used with a FIR
    filter. xq is the circular queue, in pointing at the new sample, xi
    current index for xq and n the length of the filter. xq must be n*2