Port of pl_surround.c to af-layer.

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

3 files changed, 251 insertions, 1 deletions

diff --git a/libaf/Makefile b/libaf/Makefile
index d66aa8b4ee..79ee144c76 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
+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
 
 OBJS=$(SRCS:.c=.o)
 
diff --git a/libaf/af.c b/libaf/af.c
index 3f86ab9654..882750fd96 100644
--- a/libaf/af.c
+++ b/libaf/af.c
@@ -19,6 +19,7 @@ extern af_info_t af_info_equalizer;
 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;
 
 static af_info_t* filter_list[]={ \
    &af_info_dummy,\
@@ -31,6 +32,7 @@ static af_info_t* filter_list[]={ \
    &af_info_gate,\
    &af_info_comp,\
    &af_info_pan,\
+   &af_info_surround,\
    NULL \
 };
 
diff --git a/libaf/af_surround.c b/libaf/af_surround.c
new file mode 100644
index 0000000000..6eae623795
--- /dev/null
+++ b/libaf/af_surround.c
@@ -0,0 +1,248 @@
+/* 
+   This is an ao2 plugin to do simple decoding of matrixed surround
+   sound.  This will provide a (basic) surround-sound effect from
+   audio encoded for Dolby Surround, Pro Logic etc.
+
+ * 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.
+
+   Original author: Steve Davies <steve@daviesfam.org>
+*/
+
+/* The principle:  Make rear channels by extracting anti-phase data
+   from the front channels, delay by 20msec and feed to rear in anti-phase
+*/
+
+
+// SPLITREAR: Define to decode two distinct rear channels -
+// 	this doesn't work so well in practice because
+//      separation in a passive matrix is not high.
+//      C (dialogue) to Ls and Rs 14dB or so -
+//      so dialogue leaks to the rear.
+//      Still - give it a try and send feedback.
+//      comment this define for old behaviour of a single
+//      surround sent to rear in anti-phase
+#define SPLITREAR
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include "af.h"
+#include "dsp.h"
+
+// instance data
+typedef struct af_surround_s
+{
+  float msecs;        // Rear channel delay in milliseconds
+  float* Ls_delaybuf; // circular buffer to be used for delaying Ls audio
+  float* Rs_delaybuf; // circular buffer to be used for delaying Rs audio
+  int delaybuf_len;   // delaybuf buffer length in samples
+  int delaybuf_rpos;  // offset in buffer where we are reading
+  int delaybuf_wpos;  // offset in buffer where we are writing
+  float filter_coefs_surround[32]; // FIR filter coefficients for surround sound 7kHz lowpass
+} af_surround_t;
+
+// Initialization and runtime control
+static int control(struct af_instance_s* af, int cmd, void* arg)
+{
+  af_surround_t *instance=af->setup;
+  switch(cmd){
+  case AF_CONTROL_REINIT:{
+    float cutoff;
+    af->data->rate   = ((af_data_t*)arg)->rate;
+    af->data->nch    = ((af_data_t*)arg)->nch*2;
+    af->data->format = ((af_data_t*)arg)->format;
+    af->data->bps    = ((af_data_t*)arg)->bps;
+    af_msg(AF_MSG_DEBUG0, "[surround]: rear delay=%0.2fms.\n", instance->msecs);
+    if (af->data->nch != 4){
+      af_msg(AF_MSG_ERROR,"Only Stereo input is supported, filter disabled.\n");
+      return AF_DETACH;
+    }
+    // Figure out buffer space (in int16_ts) needed for the 15msec delay
+    // Extra 31 samples allow for lowpass filter delay (taps-1)
+    // Double size to make virtual ringbuffer easier
+    instance->delaybuf_len = ((af->data->rate * instance->msecs / 1000)+31)*2;
+    // Free old buffers
+    if (instance->Ls_delaybuf != NULL)
+      free(instance->Ls_delaybuf);
+    if (instance->Rs_delaybuf != NULL)
+      free(instance->Rs_delaybuf);
+    // Allocate new buffers
+    instance->Ls_delaybuf=(void*)calloc(instance->delaybuf_len,sizeof(*instance->Ls_delaybuf));
+    instance->Rs_delaybuf=(void*)calloc(instance->delaybuf_len,sizeof(*instance->Rs_delaybuf));
+    af_msg(AF_MSG_DEBUG1, "Delay buffers are %d samples each.\n", instance->delaybuf_len);
+    instance->delaybuf_wpos = 0;
+    instance->delaybuf_rpos = 32; // compensate for fir delay
+    // Surround filer coefficients
+    cutoff = af->data->rate/7000;
+    if (-1 == design_fir(32, instance->filter_coefs_surround, &cutoff, LP|KAISER, 10.0)) {
+      af_msg(AF_MSG_ERROR,"[surround] Unable to design prototype filter.\n");
+      return AF_ERROR;
+    }
+
+    return AF_OK;
+  }
+  case AF_CONTROL_COMMAND_LINE:{
+    float d = 0;
+    sscanf((char*)arg,"%f",&d);
+    if (d<0){
+      af_msg(AF_MSG_ERROR,"Error setting rear delay length in af_surround. Delay has to be positive.\n");
+      return AF_ERROR;
+    }
+    instance->msecs=d;
+    return AF_OK;
+  }
+  }
+  return AF_UNKNOWN;
+}
+
+// Deallocate memory
+static void uninit(struct af_instance_s* af)
+{
+  af_surround_t *instance=af->setup;
+  if(af->data->audio)
+    free(af->data->audio);
+  if(af->data)
+    free(af->data);
+  if(instance->Ls_delaybuf)
+    free(instance->Ls_delaybuf);
+  if(instance->Rs_delaybuf)
+    free(instance->Rs_delaybuf);
+  free(af->setup);
+}
+
+// The beginnings of an active matrix...
+static double steering_matrix[][12] = {
+//	LL	RL	LR	RR	LS	RS
+//	LLs	RLs	LRs	RRs	LC	RC
+       {.707,	.0,	.0,	.707,	.5,	-.5,
+	.5878,	-.3928,	.3928,	-.5878,	.5,	.5},
+};
+
+// Experimental moving average dominances
+//static int amp_L = 0, amp_R = 0, amp_C = 0, amp_S = 0;
+
+// Filter data through filter
+static af_data_t* play(struct af_instance_s* af, af_data_t* data){
+  af_surround_t* instance = (af_surround_t*)af->setup;
+  int16_t*     in = data->audio;
+  int16_t*     out;
+  int i, samples;
+  double *matrix = steering_matrix[0]; // later we'll index based on detected dominance
+
+  if (AF_OK != RESIZE_LOCAL_BUFFER(af, data))
+    return NULL;
+
+  out = af->data->audio;
+
+  // fprintf(stderr, "pl_surround: play %d bytes, %d samples\n", ao_plugin_data.len, samples);
+
+  samples  = data->len / (data->nch * sizeof(int16_t));
+
+  // Testing - place a 1kHz tone on Lt and Rt in anti-phase: should decode in S
+  //sinewave(in, samples, pl_surround.input_channels, 1000, 0.0, pl_surround.rate);
+  //sinewave(&in[1], samples, pl_surround.input_channels, 1000, PI, pl_surround.rate);
+
+  for (i=0; i<samples; i++) {
+
+    // Dominance:
+    //abs(in[0])  abs(in[1]);
+    //abs(in[0]+in[1])  abs(in[0]-in[1]);
+    //10 * log( abs(in[0]) / (abs(in[1])|1) );
+    //10 * log( abs(in[0]+in[1]) / (abs(in[0]-in[1])|1) );
+
+    // About volume balancing...
+    //   Surround encoding does the following:
+    //       Lt=L+.707*C+.707*S, Rt=R+.707*C-.707*S
+    //   So S should be extracted as:
+    //       (Lt-Rt)
+    //   But we are splitting the S to two output channels, so we
+    //   must take 3dB off as we split it:
+    //       Ls=Rs=.707*(Lt-Rt)
+    //   Trouble is, Lt could be +32767, Rt -32768, so possibility that S will
+    //   overflow.  So to avoid that, we cut L/R by 3dB (*.707), and S by 6dB (/2).
+    //   this keeps the overall balance, but guarantees no overflow.
+
+    // output front left and right
+    out[0] = matrix[0]*in[0] + matrix[1]*in[1];
+    out[1] = matrix[2]*in[0] + matrix[3]*in[1];
+    // output Ls and Rs - from 20msec ago, lowpass filtered @ 7kHz
+    out[2] = fir(32, instance->filter_coefs_surround,
+                 &instance->Ls_delaybuf[instance->delaybuf_rpos +
+                                        instance->delaybuf_len/2]);
+#ifdef SPLITREAR
+    out[3] = fir(32, instance->filter_coefs_surround,
+                 &instance->Rs_delaybuf[instance->delaybuf_rpos +
+                                        instance->delaybuf_len/2]);
+#else
+    out[3] = -out[2];
+#endif
+    // calculate and save surround for 20msecs time
+#ifdef SPLITREAR
+    instance->Ls_delaybuf[instance->delaybuf_wpos] =
+    instance->Ls_delaybuf[instance->delaybuf_wpos + instance->delaybuf_len/2] =
+      matrix[6]*in[0] + matrix[7]*in[1];
+    instance->Rs_delaybuf[instance->delaybuf_wpos] =
+    instance->Rs_delaybuf[instance->delaybuf_wpos++ + instance->delaybuf_len/2] =
+      matrix[8]*in[0] + matrix[9]*in[1];
+#else
+    instance->Ls_delaybuf[instance->delaybuf_wpos] =
+    instance->Ls_delaybuf[instance->delaybuf_wpos++ + instance->delaybuf_len/2] =
+      matrix[4]*in[0] + matrix[5]*in[1];
+#endif
+    instance->delaybuf_rpos++;
+    instance->delaybuf_wpos %= instance->delaybuf_len/2;
+    instance->delaybuf_rpos %= instance->delaybuf_len/2;
+
+    // next samples...
+    in = &in[data->nch];  out = &out[af->data->nch];
+  }
+
+  // Show some state
+  //printf("\npl_surround: delaybuf_pos=%d, samples=%d\r\033[A", pl_surround.delaybuf_pos, samples);
+  
+  // Set output data
+  data->audio = af->data->audio;
+  data->len   = (data->len*af->mul.n)/af->mul.d;
+  data->nch   = af->data->nch;
+
+  return data;
+}
+
+static int open(af_instance_t* af){
+  af_surround_t *pl_surround;
+  af->control=control;
+  af->uninit=uninit;
+  af->play=play;
+  af->mul.n=2;
+  af->mul.d=1;
+  af->data=calloc(1,sizeof(af_data_t));
+  af->setup=pl_surround=calloc(1,sizeof(af_surround_t));
+  pl_surround->msecs=20;
+  if(af->data == NULL || af->setup == NULL)
+    return AF_ERROR;
+  return AF_OK;
+}
+
+af_info_t af_info_surround =
+{
+        "Surround decoder filter",
+        "surround",
+        "Steve Davies <steve@daviesfam.org>",
+        "",
+        AF_FLAGS_REENTRANT,
+        open
+};