Speed optimizations (runs twise as fast) and bugfix (wrong cutoff frequency buffer over run noise and garbeled output when wrong input format)

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

1 files changed, 148 insertions, 119 deletions

diff --git a/libaf/af_surround.c b/libaf/af_surround.c
index 6eae623795..8ec8e8058a 100644
--- a/libaf/af_surround.c
+++ b/libaf/af_surround.c
@@ -1,5 +1,5 @@
 /* 
-   This is an ao2 plugin to do simple decoding of matrixed surround
+   This is an libaf filter 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.
 
@@ -21,19 +21,17 @@
 */
 
 /* The principle:  Make rear channels by extracting anti-phase data
-   from the front channels, delay by 20msec and feed to rear in anti-phase
+   from the front channels, delay by 20ms 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
+/* 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 behavior of a single
+     surround sent to rear in anti-phase */
+#define SPLITREAR 1
 
 #include <stdio.h>
 #include <stdlib.h>
@@ -43,66 +41,106 @@
 #include "af.h"
 #include "dsp.h"
 
+#define L  32    // Length of fir filter
+#define LD 65536 // Length of delay buffer
+
+// 32 Tap fir filter loop unrolled
+#define FIR(x,w,y) \
+  y = ( w[0] *x[0] +w[1] *x[1] +w[2] *x[2] +w[3] *x[3]  \
+      + w[4] *x[4] +w[5] *x[5] +w[6] *x[6] +w[7] *x[7]  \
+      + w[8] *x[8] +w[9] *x[9] +w[10]*x[10]+w[11]*x[11] \
+      + w[12]*x[12]+w[13]*x[13]+w[14]*x[14]+w[15]*x[15] \
+      + w[16]*x[16]+w[17]*x[17]+w[18]*x[18]+w[19]*x[19] \
+      + w[20]*x[20]+w[21]*x[21]+w[22]*x[22]+w[23]*x[23] \
+      + w[24]*x[24]+w[25]*x[25]+w[26]*x[26]+w[27]*x[27] \
+      + w[28]*x[28]+w[29]*x[29]+w[30]*x[30]+w[31]*x[31])
+
+// Add to circular queue macro + update index
+#ifdef SPLITREAR
+#define ADDQUE(qi,rq,lq,r,l)\
+  lq[qi]=lq[qi+L]=(l);\
+  rq[qi]=rq[qi+L]=(r);\
+  qi=(qi-1)&(L-1);
+#else
+#define ADDQUE(qi,lq,l)\
+  lq[qi]=lq[qi+L]=(l);\
+  qi=(qi-1)&(L-1);
+#endif
+
+// Macro for updating queue index in delay queues
+#define UPDATEQI(qi) qi=(qi+1)&(LD-1)
+
 // 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;
+  float lq[2*L]; // Circular queue for filtering left rear channel
+  float rq[2*L]; // Circular queue for filtering right rear channel
+  float w[L]; 	 // FIR filter coefficients for surround sound 7kHz low-pass
+  float* dr;	 // Delay queue right rear channel
+  float* dl;	 // Delay queue left rear channel
+  float  d;	 // Delay time
+  int i;       	 // Position in circular buffer
+  int wi;	 // Write index for delay queue
+  int ri;	 // Read index for delay queue
+}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;
+  af_surround_t *s = af->setup;
   switch(cmd){
   case AF_CONTROL_REINIT:{
-    float cutoff;
+    float fc;
     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);
+    af->data->format = AF_FORMAT_F | AF_FORMAT_NE;
+    af->data->bps    = 4;
+
     if (af->data->nch != 4){
-      af_msg(AF_MSG_ERROR,"Only Stereo input is supported, filter disabled.\n");
+      af_msg(AF_MSG_ERROR,"[surround] Only stereo input is supported.\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");
+    fc = 2.0 * 7000.0/(float)af->data->rate;
+    if (-1 == design_fir(L, s->w, &fc, LP|HAMMING, 0)){
+      af_msg(AF_MSG_ERROR,"[surround] Unable to design low-pass filter.\n");
       return AF_ERROR;
     }
 
+    // Free previous delay queues
+    if(s->dl)
+      free(s->dl);
+    if(s->dr)
+      free(s->dr);
+    // Allocate new delay queues
+    s->dl = calloc(LD,af->data->bps);
+    s->dr = calloc(LD,af->data->bps);
+    if((NULL == s->dl) || (NULL == s->dr))
+      af_msg(AF_MSG_FATAL,"[delay] Out of memory\n");
+    
+    // Initialize delay queue index
+    if(AF_OK != af_from_ms(1, &s->d, &s->wi, af->data->rate, 0.0, 1000.0))
+      return AF_ERROR;
+    printf("%i\n",s->wi);
+    s->ri = 0;
+
+    if((af->data->format != ((af_data_t*)arg)->format) || 
+       (af->data->bps    != ((af_data_t*)arg)->bps)){
+      ((af_data_t*)arg)->format = af->data->format;
+      ((af_data_t*)arg)->bps = af->data->bps;
+      return AF_FALSE;
+    }
     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");
+    if ((d < 0) || (d > 1000)){
+      af_msg(AF_MSG_ERROR,"[surround] Invalid delay time, valid time values"
+	     " are 0ms to 1000ms current value is %0.3ms\n",d);
       return AF_ERROR;
     }
-    instance->msecs=d;
+    s->d = d;
     return AF_OK;
   }
   }
@@ -112,108 +150,100 @@ static int control(struct af_instance_s* af, int cmd, void* arg)
 // 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);
+  if(af->setup)
+    free(af->setup);
 }
 
 // The beginnings of an active matrix...
-static double steering_matrix[][12] = {
+static float 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
+// Experimental moving average dominance
 //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
+  af_surround_t* s   = (af_surround_t*)af->setup;
+  float*	 m   = steering_matrix[0]; 
+  float*     	 in  = data->audio; 	// Input audio data
+  float*     	 out = NULL;		// Output audio data
+  float*	 end = in + data->len / sizeof(float); // Loop end
+  int 		 i   = s->i;	// Filter queue index
+  int 		 ri  = s->ri;	// Read index for delay queue
+  int 		 wi  = s->wi;	// Write index for delay queue
 
   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]);
+  while(in < end){
+    /* 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 +1, Rt -1, 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] = m[0]*in[0] + m[1]*in[1];
+    out[1] = m[2]*in[0] + m[3]*in[1];
+
+    // Low-pass output @ 7kHz
+    FIR((&s->lq[i]), s->w, s->dl[wi]);
+
+    // Delay output by d ms
+    out[2] = s->dl[ri];
+
 #ifdef SPLITREAR
-    out[3] = fir(32, instance->filter_coefs_surround,
-                 &instance->Rs_delaybuf[instance->delaybuf_rpos +
-                                        instance->delaybuf_len/2]);
+    // Low-pass output @ 7kHz
+    FIR((&s->rq[i]), s->w, s->dr[wi]);
+
+    // Delay output by d ms
+    out[3] = s->dr[ri];
 #else
     out[3] = -out[2];
 #endif
-    // calculate and save surround for 20msecs time
+
+    // Update delay queues indexes
+    UPDATEQI(ri);
+    UPDATEQI(wi);
+
+    // Calculate and save surround in circular queue
 #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];
+    ADDQUE(i, s->rq, s->lq, m[6]*in[0]+m[7]*in[1], m[8]*in[0]+m[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];
+    ADDQUE(i, s->lq, m[4]*in[0]+m[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];
+    // Next sample...
+    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);
   
+  // Save indexes
+  s->i  = i; s->ri = ri; s->wi = wi;
+
   // Set output data
   data->audio = af->data->audio;
   data->len   = (data->len*af->mul.n)/af->mul.d;
@@ -223,17 +253,16 @@ static af_data_t* play(struct af_instance_s* af, af_data_t* 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;
+  af->setup=calloc(1,sizeof(af_surround_t));
   if(af->data == NULL || af->setup == NULL)
     return AF_ERROR;
+  ((af_surround_t*)af->setup)->d = 20;
   return AF_OK;
 }
 
@@ -243,6 +272,6 @@ af_info_t af_info_surround =
         "surround",
         "Steve Davies <steve@daviesfam.org>",
         "",
-        AF_FLAGS_REENTRANT,
+        AF_FLAGS_NOT_REENTRANT,
         open
 };